Koala Library
koala_test.c
Go to the documentation of this file.
00001 //--------------------------------------------------------------------------------//
00002 //-                   KOALA( Koala extension board                       )                       -//
00003 //                                                                               -//
00004 //-  Copyright (C) Julien Tharin, K-Team S.A. 2013                               -//
00005 //-  This library is free software; you can redistribute it and/or               -//
00006 //-  modify it under the terms of the GNU Lesser General Public                  -//
00007 //-  License as published by the Free Software Foundation; either                -//
00008 //-  version 2.1 of the License, or any later version.                           -//
00009 //-                                                                              -//
00010 //-  This library is distributed in the hope that it will be useful,             -//
00011 //-  but WITHOUT ANY WARRANTY; without even the implied warranty of              -//
00012 //-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU           -//
00013 //-  Lesser General Public License for more details.                             -//
00014 //-                                                                              -//
00015 //-  You should have received a copy of the GNU Lesser General Public            -//
00016 //-  License along with this library; if not, write to the Free Software         -//
00017 //-  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA   -//
00018 //-                                                                              -//
00019 //-                               __  __  ________                               -//
00020 //- K-Team S.A.                  |  |/  /|__    __|___  _____  ___  ___          -//
00021 //- Chemin des Plans-Praz 28,    |     / __ |  | _____|/  _  \|   \/   |         -//
00022 //- 1337 Vallorbe                |  |  \    |  | ____|/  /_\  |        |         -//
00023 //- Switzerland                  |__|\__\   |__|______|_/   \_|__|\/|__|         -//
00024 //- jtharin@k-team.com   tel:+41 24 423 89 56 fax:+41 24 423 8960                -//
00025 //-                                                                              -//
00026 //--------------------------------------------------------------------------------//
00027 
00029 
00032 
00033 
00034 #include <stdlib.h>
00035 #include <stdio.h>
00036 #include <time.h>
00037 #include <string.h>
00038 
00039 #include <koala/koala.h> // koala library
00040 
00041 #include <signal.h>
00042 #include <math.h>
00043 
00044 
00045 
00046 static int quitReq = 0; // quit variable for loop
00047 
00048 /*--------------------------------------------------------------------*/
00052 static void ctrlc_handler( int sig ) 
00053 {
00054   quitReq = 1;
00055   
00056   koala_set_motor_speed_open_loop(0,0); 
00057   koala_change_term_mode(0); // revert to original terminal if called
00058   exit(0);
00059 }
00060 
00061 /*--------------------------------------------------------------------*/
00062 
00071 char *kitoa(int val, int base){
00072     static char buf[32] = {0};
00073     int i = 30;
00074     for(; val && i ; --i, val /= base)
00075         buf[i] = "0123456789abcdef"[val % base];
00076     return &buf[i+1];
00077 
00078 }
00079 
00080 
00081 
00082 
00083 
00095 long long
00096 timeval_diff(struct timeval *difference,
00097              struct timeval *end_time,
00098              struct timeval *start_time
00099             )
00100 {
00101   struct timeval temp_diff;
00102 
00103   if(difference==NULL)
00104   {
00105     difference=&temp_diff;
00106   }
00107 
00108   difference->tv_sec =end_time->tv_sec -start_time->tv_sec ;
00109   difference->tv_usec=end_time->tv_usec-start_time->tv_usec;
00110 
00111   /* Using while instead of if below makes the code slightly more robust. */
00112 
00113   while(difference->tv_usec<0)
00114   {
00115     difference->tv_usec+=1000000;
00116     difference->tv_sec -=1;
00117   }
00118 
00119   return 1000000LL*difference->tv_sec+
00120                    difference->tv_usec;
00121 
00122 } /* timeval_diff() */
00123 
00124 
00125 
00126  
00127  // #define for driver mode
00128 #define BIG_SPEED_FACTOR 25
00129 #define SPEED_FACTOR 1
00130 #define MAX_SPEED 500
00131 #define MIN_SPEED 1
00132 #define DEFAULT_SPEED 200
00133 
00134 #define ROTATE_HIGH_SPEED_FACT 0.5
00135 #define ROTATE_LOW_SPEED_FACT 0.7
00136 #define ROTATE_DIG_SPEED_FACT 0.5
00137 #define ROT_SPEED_HIGH_TRESH 300
00138 #define STOP_TIME 100000 // us
00139 
00140 #define SIGN(x) ((x)>0?1:((x)<0?-1:0))  // sign or zero
00141 
00152 int drive_robot()
00153 {
00154         int out=0,speed=DEFAULT_SPEED,vsl,vsr,anymove=0;
00155         char c;
00156         //struct timeval startt,endt;
00157 
00158         
00159         koala_clrscr(); // erase screen
00160         
00161         printf("Drive the robot with the keyboard:\n  's' 5 or spacebar for stop\n  arrows (UP, DOWN, LEFT, RIGHT, or NUMPAD 1-9) for direction\n  PAGE UP/DOWN for changing speed  by small increments\n  Home/End for changing speed by big increments\n  'q' for going back to main menu\n");
00162         
00163         
00164         printf("\ndefault parameters:\n  robot speed %d  (%5.1f mm/s)  (min %d, max %d)\n\n",DEFAULT_SPEED,DEFAULT_SPEED*KOALA_SPEED_TO_MM_S,MIN_SPEED,MAX_SPEED);
00165         
00166         koala_change_term_mode(1); // change terminal mode for kbhit and getchar to return immediately
00167         
00168 
00169         
00170         //gettimeofday(&startt,0x0);
00171         
00172         // loop until 'q' is pushed
00173         while(!out)
00174         {
00175                 if(koala_kbhit())
00176                 {
00177                         c=getchar();
00178 
00179 
00180                         // get special keys
00181                         if (c== 27  ) 
00182                         {
00183                         
00184                          if (c=getchar()==91) // escape with [
00185                          {
00186                                  c = getchar(); 
00187                          
00188                                  switch(c)
00189                                  {
00190                                         case 65: // UP arrow = forward
00191                                                          koala_set_motor_speed_accel(speed ,speed);
00192                                                         anymove=1;                                              
00193                                         break;
00194                                         case 66: // DOWN arrow = backward                       
00195                                                          koala_set_motor_speed_accel(-speed ,-speed);
00196                                                         anymove=1;
00197                                         break;
00198 
00199                                         case 68: // LEFT arrow = left
00200                                                         if (speed > ROT_SPEED_HIGH_TRESH) // at high speed, rotate too fast
00201                                                                  koala_set_motor_speed_accel(-speed*ROTATE_HIGH_SPEED_FACT ,speed*ROTATE_HIGH_SPEED_FACT);
00202                                                         else
00203                                                                  koala_set_motor_speed_accel(-speed*ROTATE_LOW_SPEED_FACT ,speed*ROTATE_LOW_SPEED_FACT);
00204                                                         anymove=1;      
00205                                         break;
00206 
00207                                         case 67: // RIGHT arrow = right
00208                                                         if (speed > ROT_SPEED_HIGH_TRESH) // at high speed, rotate too fast
00209                                                                  koala_set_motor_speed_accel(speed*ROTATE_HIGH_SPEED_FACT ,-speed*ROTATE_HIGH_SPEED_FACT);
00210                                                         else
00211                                                                  koala_set_motor_speed_accel(speed*ROTATE_LOW_SPEED_FACT ,-speed*ROTATE_LOW_SPEED_FACT );
00212                                                         anymove=1;      
00213                                         break;
00214 
00215                                         case 53: // PAGE UP  = speed up
00216                                                 speed+=SPEED_FACTOR;
00217                                                 if (speed>MAX_SPEED)
00218                                                 {
00219                                                         speed=MAX_SPEED;
00220                                                 };
00221                                                 c = getchar(); // get last character
00222                                                 
00223                                                 koala_read_motor_speed(&vsl,&vsr);
00224                                                 koala_set_motor_speed_accel(SIGN(vsl)*speed ,SIGN(vsr)*speed); // set new speed, keeping direction with sign
00225                                                 printf("\033[1`\033[Krobot speed: %d (%5.1f mm/s)",speed,speed*KOALA_SPEED_TO_MM_S); // move cursor to first column, erase line and print info
00226                                                 fflush(stdout); // make the display refresh
00227                                                 anymove=1;
00228                                         break;
00229 
00230                                         case 54: // PAGE DOWN = speed down
00231                                                 speed-=SPEED_FACTOR;
00232                                                 if (speed<MIN_SPEED)
00233                                                 {
00234                                                         speed=MIN_SPEED;
00235                                                 };
00236                                                 c = getchar(); // get last character
00237                                                 
00238                                                 koala_read_motor_speed(&vsl,&vsr);
00239                                                 koala_set_motor_speed_accel(SIGN(vsl)*speed ,SIGN(vsr)*speed); // set new speed, keeping direction with sign
00240                                                 printf("\033[1`\033[Krobot speed: %d (%5.1f mm/s)",speed,speed*KOALA_SPEED_TO_MM_S); // move cursor to first column, erase line and print info
00241                                                 fflush(stdout); // make the display refresh
00242                                                 anymove=1;
00243                                         break;
00244                         
00245 
00246                                         default:
00247                                                 //printf("special key  : %d\n",c);
00248                                         break;
00249                                         } // switch(c)
00250                                 } // escape with [
00251                                 else
00252                                 { // other special key code
00253                                         
00254                                          c = getchar(); 
00255                                          
00256                                         switch(c){
00257                                 
00258                                                 case 72: // Home  = speed up
00259                                                         speed+=BIG_SPEED_FACTOR;
00260                                                         if (speed>MAX_SPEED)
00261                                                         {
00262                                                                 speed=MAX_SPEED;
00263                                                         };
00264                                                         //c = getchar(); // get last character
00265                                                         
00266                                                          koala_read_motor_speed(&vsl,&vsr);
00267                                                          koala_set_motor_speed_accel(SIGN(vsl)*speed ,SIGN(vsr)*speed); // set new speed, keeping direction with sign
00268                                                         printf("\033[1`\033[Krobot speed: %d (%5.1f mm/s)",speed,speed*KOALA_SPEED_TO_MM_S); // move cursor to first column, erase line and print info
00269                                                         fflush(stdout); // make the display refresh
00270                                                         anymove=1;
00271                                                 break;
00272 
00273                                                 case 70: // End = speed down
00274                                                         speed-=BIG_SPEED_FACTOR;
00275                                                         if (speed<MIN_SPEED)
00276                                                         {
00277                                                                 speed=MIN_SPEED;
00278                                                         };
00279                                                         //c = getchar(); // get last character
00280                                                         
00281                                                         koala_read_motor_speed(&vsl,&vsr);
00282                                                         koala_set_motor_speed_accel(SIGN(vsl)*speed ,SIGN(vsr)*speed); // set new speed, keeping direction with sign
00283                                                         printf("\033[1`\033[Krobot speed: %d (%5.1f mm/s)",speed,speed*KOALA_SPEED_TO_MM_S); // move cursor to first column, erase line and print info
00284                                                         fflush(stdout); // make the display refresh
00285                                                         anymove=1;
00286                                                 break;
00287                                                 
00288                                                 default:
00289                                                         //printf("other special key  : %d\n",c);
00290                                                 break   ;       
00291                                                 
00292                                         }  
00293                         
00294                                 } // other special key code
00295                                                         
00296                                 
00297                         } // if (c== '\027')     
00298                         else 
00299                         {
00300                                 switch(c)
00301                                 {
00302                                         case 'q': // quit to main menu
00303                                                 out=1;
00304                                         break;
00305                                         case 's': // stop motor
00306                                         case ' ': // spacebar
00307                                         case 53:  // 5
00308                                                 koala_set_motor_speed(0,0);      // stop robot
00309                                         break;
00310                                         case 49: // 1 = left backward
00311                                                          koala_set_motor_speed_accel(-speed*ROTATE_DIG_SPEED_FACT ,-speed);
00312                                                         anymove=1;                                              
00313                                         break;
00314                                         case 50: // 2 = backward                        
00315                                                         koala_set_motor_speed_accel(-speed ,-speed);
00316                                                         anymove=1;
00317                                         break;
00318                                         case 51: // 3 = right backward
00319                                                  koala_set_motor_speed(-speed ,-speed*ROTATE_DIG_SPEED_FACT);
00320                                         break;
00321                                         case 52: // 4 = left
00322                                                 if (speed > ROT_SPEED_HIGH_TRESH) // at high speed, rotate too fast
00323                                                          koala_set_motor_speed_accel(-speed*ROTATE_HIGH_SPEED_FACT ,speed*ROTATE_HIGH_SPEED_FACT);
00324                                                 else
00325                                                          koala_set_motor_speed_accel(-speed*ROTATE_LOW_SPEED_FACT ,speed*ROTATE_LOW_SPEED_FACT);
00326                                                 anymove=1;      
00327                                                 break;
00328                                         case 54: // 6 = right
00329                                                 if (speed > ROT_SPEED_HIGH_TRESH) // at high speed, rotate too fast
00330                                                          koala_set_motor_speed_accel(speed*ROTATE_HIGH_SPEED_FACT ,-speed*ROTATE_HIGH_SPEED_FACT);
00331                                                 else
00332                                                          koala_set_motor_speed_accel(speed*ROTATE_LOW_SPEED_FACT ,-speed*ROTATE_LOW_SPEED_FACT );
00333                                                 anymove=1;      
00334                                         break;
00335                                   case 55: // 7 = left forward
00336                                                          koala_set_motor_speed_accel(speed*ROTATE_DIG_SPEED_FACT ,speed);
00337                                                         anymove=1;                                              
00338                                         break;
00339                                   case 56: // 8 = forward
00340                                                          koala_set_motor_speed_accel(speed ,speed);
00341                                                         anymove=1;                                              
00342                                         break;
00343                                         case 57: // 9 = right forward
00344                                                          koala_set_motor_speed_accel(speed,speed*ROTATE_DIG_SPEED_FACT);
00345                                                         anymove=1;                                              
00346                                         break;
00347                                 default:
00348                                                 //printf("normal key : %d\n",c);
00349                                    break;
00350                                 }
00351                   }
00352                   
00353                   
00354                 } /*else
00355                 {
00356                 
00357                         gettimeofday(&endt,0x0);;
00358                         // stop when no key is pushed after some time
00359                         
00360                         if (anymove &&  (timeval_diff(NULL,&endt,&startt)>STOP_TIME))
00361                         {
00362                                  koala_set_motor_speed(0 ,0);
00363                                 anymove=0;
00364                         }       
00365                                 
00366                 }*/
00367                 
00368 
00369 
00370                 usleep(10000); // wait some ms
00371         } // while
00372 
00373         koala_change_term_mode(0); // switch to normal key input mode   
00374         koala_set_motor_speed(0,0);      // stop robot
00375         return 0;
00376 }
00377 
00378 /*--------------------------------------------------------------------*/
00384 #define BR_IRGAIN 1
00385 #define fwSpeed 200//150
00386 #define BMIN_SPEED 10
00387 
00388 #define RotSpeedL 50
00389 #define RotSpeedR -50
00390 
00391 #define MAX_DIST 60
00392 #define MIN_DIST 25
00393  
00394 int braitenbergAvoidance()
00395 {
00396   int Connections_B[9] = { -2, -3, -4, -12,  4,  3,  2, 1, 1}; // weight of every 9 sensor for the right motor 
00397   int Connections_A[9] = { 2,  3,  4, -10, -4, -3, -2, 1, 1}; // weight of every 9 sensor for the left motor 
00398   int i, buflen, sensval;
00399 
00400 
00401   int lspeed, rspeed;
00402   int tabsens[9];
00403   int left_speed, right_speed;
00404   unsigned int immobility = 0;
00405   unsigned int prevpos_left, pos_left, prevpos_right,  pos_right;
00406   int us_sensors[KOALA_US_SENSORS_NUMBER];
00407   struct timeval startt,endt;
00408   int bat_type,bat_voltage,bat_current,chrg_current;
00409   
00410   koala_auto_data_t data;
00411 
00412 
00413         koala_configure_us_io(KOALA_US_ALL,0); // set all us sensors ON
00414 
00415   koala_read_motor_position(&prevpos_left, &prevpos_right);
00416 
00417         koala_configure_auto_monitoring_mode(KOALA_AUTOM_US_SENSOR_BIT);
00418 
00419         printf("\nPush ANY KEY to stop!\n");
00420 
00421         gettimeofday(&startt,0x0);
00422         
00423   while(!koala_kbhit())
00424   {
00425     lspeed = 0; rspeed = 0;
00426     
00427     // display battery value every 30s
00428     gettimeofday(&endt,0x0);
00429                 if ((timeval_diff(NULL,&endt,&startt)>30000000))
00430                 {
00431                                 koala_get_battery_data(&bat_type,&bat_voltage,&bat_current,&chrg_current);
00432                                 printf("\nBattery: Voltage =%3.1f [V]  Current=%5d [mA]\n", 
00433                         bat_voltage/10.0,bat_current*100);
00434                                 gettimeofday(&startt,0x0);
00435                 }       
00436 
00437         // check if any value received from the robot and process it
00438         if (koala_get_from_auto_mode(&data)>=0) {
00439          
00440          //printf("\nreceived mode %c: \n",data.mode); 
00441           
00442                 switch(data.mode) {
00443      case 'g': // ultrasounds
00444              
00445                                 //limit the sensor values
00446                                 for (i = 0; i < KOALA_US_SENSORS_NUMBER; i++)   
00447                                 {
00448                                         sensval = data.us[i];
00449                                         if(sensval > MAX_DIST)
00450                                                 tabsens[i] = 0;
00451                                         else if (sensval < MIN_DIST)
00452                                                 tabsens[i] = MAX_DIST-MIN_DIST;
00453                                         else
00454                                                 tabsens[i] = MAX_DIST-sensval;
00455                                 }
00456 
00457                                 for (i = 0; i < 9; i++)
00458                                 {
00459                                   lspeed += Connections_A[i] * tabsens[i];
00460                                   rspeed += Connections_B[i] * tabsens[i];                              
00461                                 }
00462 
00463                                 left_speed = ((lspeed * BR_IRGAIN) + fwSpeed);
00464                                 right_speed = ((rspeed * BR_IRGAIN) + fwSpeed);
00465 
00466                                 if(left_speed > 0 && left_speed < BMIN_SPEED)
00467                                   left_speed = BMIN_SPEED;
00468                                 if(left_speed < 0 && left_speed > -BMIN_SPEED)
00469                                   left_speed = -BMIN_SPEED;
00470                                 if(right_speed > 0 && right_speed < BMIN_SPEED)
00471                                   right_speed = BMIN_SPEED;
00472                                 if(right_speed < 0 && right_speed > -BMIN_SPEED)
00473                                   right_speed = -BMIN_SPEED;
00474 
00475                                 koala_set_motor_speed_accel(left_speed ,right_speed);
00476 
00477                                 //printf("lens = %d, rsens = %d lspd = %d rspd = %d\r\n", (int)lspeed16, (int)rspeed16, left_speed, right_speed);       
00478 
00479                                 /*left_speed = kmot_GetMeasure( mot1 , kMotMesSpeed );
00480                                 right_speed = kmot_GetMeasure( mot2 , kMotMesSpeed );
00481                                 koala_read_motor_speed(int *left, int *right);*/
00482                                 
00483 
00484                                 /* Get the new position of the wheel to compare with previous values */
00485                                 /*koala_read_motor_position(&pos_left, &pos_right);
00486 
00487                                 if((pos_left < (prevpos_left + 700)) && (pos_left > (prevpos_left -700)) && (pos_right < (prevpos_right + 700)) && (pos_right > (prevpos_right -700)))
00488                                 {
00489                                   if(++immobility > 5)
00490 
00491                                   {
00492                                      left_speed = RotSpeedL;
00493                                      right_speed = RotSpeedR;
00494 
00495                                                         koala_set_motor_speed_accel(left_speed ,right_speed);
00496 
00497                          do{
00498                                         usleep(10000);
00499                                         koala_read_us_sensors(us_sensors);
00500                                 }while ((us_sensors[2] >250) || (us_sensors[3] >250) || (us_sensors[4] >250));
00501 
00502                                      immobility = 0;
00503                                      prevpos_left = pos_left;
00504                                      prevpos_right = pos_right;
00505                                   }
00506                                 }
00507                                 else
00508                                 {
00509 
00510                                    immobility = 0;
00511                                    prevpos_left = pos_left;
00512                                    prevpos_right = pos_right;
00513                                 }*/
00514                                 break;
00515                                 
00516                                 default:
00517                                         printf("\nERROR: received invalid auto mode: %c (0x%x)\n",data.mode,data.mode);
00518                         }
00519     //printf("lspd = %d rspd = %d\r\n", left_speed, right_speed);                       
00520                 }
00521 
00522     usleep(20000); 
00523   }
00524   
00525   koala_configure_auto_monitoring_mode(KOALA_AUTOM_NONE);
00526   
00527   koala_set_motor_speed(0 ,0); // stop robot
00528   
00529 }
00530 
00531 
00539 int test_sound()
00540 {
00541         return 0;
00542 }
00543 
00553 int main(int argc, char *argv[]) {
00554   
00555 #define US_BAR_LEN 23   // display bar length for US sensor
00556 #define ACGY_BAR_LEN 30 // display bar length for Accel/gyro sensor
00557 #define MAX_US_DISTANCE 250.0 // max distance US
00558 #define MAX_G 2                 // max acceleration in g
00559 
00560 // convert US value to text comment
00561 #define US_VAL(val) ((val)==KOALA_US_DISABLED_SENSOR ? "Not activated" : ((val)==KOALA_US_NO_OBJECT_IN_RANGE ? "No object in range" : ((val)==KOALA_US_OBJECT_NEAR ? "Object at less than 25cm" : "Object in range 25..250cm")))
00562 
00563   double fpos,dval,dmean;
00564   char Buffer[256],line[80]; // Buffer must be >= line
00565   char bar[12][64],revision,version;
00566   int rc=0,i,n,type_of_test=0,sl,sr,pl,pr;
00567   char c;
00568   long motspeed;
00569   char l[9];
00570   int kp,ki,kd;
00571   int acc_inc,acc_div,min_spacc,cst_speed,pos_margin,max_current;
00572   int accel_array[KOALA_ACCEL_VALUES_NUMBER],gyro_array[KOALA_GYRO_VALUES_NUMBER];
00573   int Bat_Type,Bat_Voltage,Bat_Current,Chrg_Current;
00574   int lspeed,lpos,lcur,rspeed,rpos,rcur;
00575   int us_mask,io_dir;
00576   char valid_sat;int sat_nb;double lat_val;char lat_car;double long_val;char long_car;struct tm date_time;double speed;int altitude;
00577   int us_sensors[KOALA_US_SENSORS_NUMBER];
00578   int i2c_add,i2c_reg;
00579   int i2c_data[KOALA_MAX_I2C_DATA];
00580   int io_state,in_state,ad_0,ad_1;
00581   int auto_mon=0;
00582   int power_out,IO0,IO1,IO2,IO3;
00583   int left_status,right_status,left_pos,right_pos;
00584   char *motor_ctrl_status_str[] ={"Idle","Speed","Speed with Acceleration","Position","Open Loop","Current Limitation","Error"};
00585   int magne[KOALA_MAGNE_VALUES_NUMBER];
00586   koala_auto_data_t auto_data;
00587   
00588         // initialise koala library
00589   if((rc=koala_init( argc , argv )) < 0 )
00590   {
00591     fprintf(stderr,"ERROR %d: Unable to initialize the koala library!\n",rc);
00592     return -1;
00593   }
00594 
00595 
00596   printf("Koala V2.5 robot test program\n(C) K-Team S.A\n");
00597   
00598         rc=koala_get_firmware_version_revision(&version,&revision);
00599         
00600         if (rc<0)
00601   {
00602                 rc=koala_get_firmware_version_revision(&version,&revision);
00603   
00604                 if (rc<0) // retry, because of the serial start level
00605                 {
00606                   fprintf(stderr,"ERROR %d: Koala did not respond correctly (could not get version)!\n",rc);
00607                   return -2;
00608                 } 
00609     
00610   } 
00611   
00612   
00613    // Set auto mode configuration  
00614   rc= koala_configure_auto_monitoring_mode(auto_mon);
00615         if (rc<0)
00616   {
00617     fprintf(stderr,"ERROR %d: could not set auto mode configuration!\n",rc);
00618     return -3;
00619   } 
00620 
00621   
00622   printf("Koala version: %c  revision: %d\n",version,revision);
00623   
00624   
00625   // configure motor PID
00626   kp=KOALA_MOTOR_P;
00627   ki=KOALA_MOTOR_I;
00628   kd=KOALA_MOTOR_D;
00629   rc=koala_configure_pid(kp,ki,kd);
00630   if (rc<0)
00631   {
00632     fprintf(stderr,"ERROR %d: could not set PID data!\n",rc);
00633     return -4;
00634   } else
00635   {
00636         printf("PID set to default values: P=%d I=%d D=%d\n",kp,ki,kd);
00637   }
00638   
00639   // set motor speed profile
00640   acc_inc=KOALA_MOTOR_ACC_INC;
00641   acc_div=KOALA_MOTOR_ACC_DIV;
00642   min_spacc=KOALA_MOTOR_MIN_SPACC;
00643   cst_speed=KOALA_MOTOR_CST_SPEED;
00644   pos_margin=KOALA_MOTOR_POS_MARGIN;
00645   max_current=KOALA_MOTOR_MAX_CURRENT;
00646   rc=koala_set_speed_profile(acc_inc,acc_div,min_spacc, cst_speed,pos_margin,max_current);
00647   if (rc<0)
00648   {
00649     fprintf(stderr,"ERROR %d: could not set speed profile!\n",rc);
00650     return -5;
00651   } else
00652   {
00653         printf("Speed profile set to default values:\n  acc_inc=%d, acc_div= %d, min_spacc=%d, cst_speed=%d, pos_margin=%d, max_current=%d\n",acc_inc,acc_div,min_spacc, cst_speed,pos_margin,max_current);
00654   }
00655   
00656    
00657   signal( SIGINT , ctrlc_handler ); // set signal for catching ctrl-c
00658   
00659   /* For ever loop until ctrl-c key */
00660   while(quitReq==0)
00661   {
00662         
00663         // print menu
00664                 printf("\nChoose an option: enter (letter) then [its argument(s)], then push ENTER:\n\n");
00665                 printf("  (ad) read A/D ports\n");
00666                 printf("  (ag) accel and gyro sensors\n");
00667                 printf("  (am) auto monitoring mode [mask]  OR active mask: 511=ALL 256=Magne 128=NEMA 64=GPS 32=Gyro\n       16=Accel 8=Current 4=Position 2=Speed 1=US sensor (current: %d (0b%s))\n",auto_mon,kitoa(auto_mon,2));
00668                 printf("  (b)attery status\n");
00669                 printf("  (br)aitenberg avoidance algorithm\n");
00670                 printf("  (d)rive the robot with keyboard\n");
00671                 printf("  (g)et motors speed and position\n");
00672                 printf("  (gp)s get data\n");
00673                 printf("  (gs) gps sent data [data_string]\n");
00674     printf("  (io) get the digital input values\n");
00675                 printf("  (ir) i2c read [address register number_of_data]\n");
00676                 printf("  (iw) i2c write [address register number_of_data data_1 data_2 ...]\n");
00677                 printf("  (is) i2c scan\n");
00678                 printf("  (ma)gnetometer sensor\n");
00679                 printf("  (ms) motor speed [left] [right] speed in [pulse/%dms]  (1 pulse/%dms = %.3f mm/s ; 1mm/s = %.3f pulses/%dms)\n",KOALA_TIME_BTWN,KOALA_TIME_BTWN,KOALA_SPEED_TO_MM_S,1.0/KOALA_SPEED_TO_MM_S,KOALA_TIME_BTWN);
00680                 printf("  (msp) motor speed profile [left] [right] speed in [pulse/%dms]  (1 pulse/%dms = %.3f mm/s ; 1mm/s = %.3f pulses/%dms)\n",KOALA_TIME_BTWN,KOALA_TIME_BTWN,KOALA_SPEED_TO_MM_S,1.0/KOALA_SPEED_TO_MM_S,KOALA_TIME_BTWN);
00681                 printf("  (mso) motor speed open loop [left] [right] speed (-2000..2000 => -100%%..100%%))\n");         
00682                 printf("  (mst) get motor status\n");
00683                 printf("  (mp) motor position [left] [right] absolute position in [pulses]  (1 pulse = %.4f mm ; 1mm = %.2f pulses)\n",KOALA_PULSE_TO_MM,1.0/KOALA_PULSE_TO_MM);
00684                 printf("  (pa)rameteres [us_mask io_dir]  (us_mask: 1=left rear, 2=left front, 4=front left,\n       8=front, 16=front right, 32=right front, 64=right rear, 128=back right, 256=back left, all=511, 0=none)\n");
00685                 printf("  (pi)d [p i d]  current: %d %d %d\n",kp,ki,kd);
00686                 printf("  (po) set power and io [PWR_Value IO0 IO1 IO2 IO3 ]   (PWR_Value: 1=PW0,2=PWR1,4=PWR2,8=PWR3   IO=0 or 1)\n");
00687                 printf("  (s)top motor\n");
00688                 printf("  (sp) speed profile [Acc_inc  Acc_div Min_speed_acc cst_speed pos_margin max_current ]  current: %d %d %d %d %d %d\n",acc_inc,acc_div,min_spacc, cst_speed,pos_margin,max_current); 
00689                 printf("  (re)set encoders\n");
00690                 printf("  (rm) reset the microcontroller\n");
00691                 printf("  (q)uit program\n");
00692                 printf("  (u)ltrasonic sensors values\n");
00693                 
00694 
00695                 
00696                 printf("\noption: "); 
00697                 
00698                 // wait and save choice
00699                 fgets(line,80,stdin);
00700                 
00701                 
00702                 // applay selected choice
00703                 switch(line[0])
00704                 {
00705                         case 'a': // accel and gyro, AD, auto mode
00706                                 if ((strlen(line)>1 && (line[1]=='g')))
00707                                 {               
00708                                         while(!koala_kbhit())
00709                                         {       
00710                                                 koala_clrscr();
00711                                                 // get gyro sensor
00712                                                 printf("\ngyro sensor [deg/s]\n       new data                                                      old data    average\ngyro X: ");
00713                                                 if (koala_read_gyroscope(gyro_array)<0)
00714                                                 {
00715                                                         printf("\nERROR: could not get gyro data!\n");
00716                                                         break;
00717                                                 }
00718                                                 
00719                                                 dmean=0;
00720                                                 for (i=0;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
00721                                                 {
00722                                                         dval=gyro_array[i]*KOALA_GYRO_DEG_S;
00723                                                         printf("%6.1f ",dval);
00724                                                         dmean+=dval;                                                       
00725                                                 }   
00726                                                 
00727                                                 printf(" %6.1f",dmean/10.0);
00728 
00729                                                 printf("\ngyro Y: ");
00730                                                 dmean=0;
00731                                                 for (i=1;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
00732                                                 {
00733 
00734                                                         dval=gyro_array[i]*KOALA_GYRO_DEG_S;
00735                                                         printf("%6.1f ",dval);
00736                                                         dmean+=dval;                                                                                                       
00737                                                 } 
00738                                                 printf(" %6.1f",dmean/10.0);
00739                                                         
00740                                                 printf("\ngyro Z: ");
00741                                                 dmean=0;
00742                                                 for (i=2;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
00743                                                 {
00744                                                         dval=gyro_array[i]*KOALA_GYRO_DEG_S;
00745                                                         printf("%6.1f ",dval);
00746                                                         dmean+=dval;                                                                                                       
00747                                                 }
00748                                                 printf(" %6.1f",dmean/10.0);     
00749                                                 printf("\n");
00750 
00751 
00752 
00753                                                 // get accel sensor
00754                                                 if (koala_read_accelerometer(accel_array)<0)
00755                                                 {
00756                                                         printf("\nERROR: could not get accel data!\n");
00757                                                         break;
00758                                                 }
00759 
00760                                                 printf("\nAcceleration sensor [g]\n       new data                                            old data  average  [g]: -2     -1      0      1      2\nacc  X: ");
00761                                                 dmean=0;
00762                                                 for (i=0;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00763                                                 {
00764                                                         dval=accel_array[i]*KOALA_ACCEL_G;
00765                                                         printf("%5.2f ",dval);
00766                                                         dmean+=dval;                                                                                       
00767                                                 }
00768                                                 
00769                                                 dval=dmean/10.0;
00770                                                 
00771                                                 printf(" %5.2f",dval);   
00772                                                 
00773                                                 // compute bar index
00774                                                 n = (int)abs(dval*ACGY_BAR_LEN/MAX_G /2.0); 
00775                                                 // fill up bar
00776                                                 if (dval < 0)
00777                                                 {
00778                                                         for (i=0;i<ACGY_BAR_LEN/2-n;i++)
00779                                                                 bar[0][i]=' ';
00780                                                         for (i=ACGY_BAR_LEN/2-n;i<ACGY_BAR_LEN/2;i++)
00781                                                                 bar[0][i]='-';
00782                                                         bar[0][ACGY_BAR_LEN/2]='0';                                                             
00783                                                         for (i=ACGY_BAR_LEN/2+1;i<ACGY_BAR_LEN;i++)
00784                                                                 bar[0][i]=' ';  
00785                                                 }
00786                                                 else
00787                                                 {
00788                                                         for (i=0;i<ACGY_BAR_LEN/2;i++)
00789                                                                 bar[0][i]=' '; 
00790                                                                 
00791                                             bar[0][ACGY_BAR_LEN/2]='0'; 
00792                                         
00793                                                         for (i=ACGY_BAR_LEN/2+1;i<ACGY_BAR_LEN/2+n+1;i++)
00794                                                           bar[0][i]='+';
00795                                  
00796                                                         for (i=ACGY_BAR_LEN/2+n+1;i<ACGY_BAR_LEN;i++)
00797                                                                 bar[0][i]=' ';
00798                                                 }
00799                                                                  
00800                                           bar[0][ACGY_BAR_LEN/2]='0';
00801                                         
00802                                                 bar[0][ACGY_BAR_LEN]='\0';
00803                                         
00804                                                 printf("        |%s|",bar[0]);
00805                                                         
00806                                                 printf("\nacc  Y: ");
00807                                                 dmean=0;
00808                                                 for (i=1;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00809                                                 {
00810                                                         dval=accel_array[i]*KOALA_ACCEL_G;
00811                                                         printf("%5.2f ",dval);
00812                                                         dmean+=dval;
00813                                                 }
00814                                                 
00815                                                 dval=dmean/10.0;
00816                                                 printf(" %5.2f",dval);
00817                                                 
00818                                                 // compute bar index
00819                                                 n = (int)abs(dval*ACGY_BAR_LEN/MAX_G /2.0); 
00820                                                 // fill up bar
00821                                                 if (dval < 0)
00822                                                 {
00823                                                         for (i=0;i<ACGY_BAR_LEN/2-n;i++)
00824                                                                 bar[0][i]=' ';
00825                                                         for (i=ACGY_BAR_LEN/2-n;i<ACGY_BAR_LEN/2;i++)
00826                                                                 bar[0][i]='-';
00827                                                         bar[0][ACGY_BAR_LEN/2]='0';
00828                                                                 
00829                                                         for (i=ACGY_BAR_LEN/2+1;i<ACGY_BAR_LEN;i++)
00830                                                                 bar[0][i]=' ';  
00831                                                 }
00832                                                 else
00833 
00834                                                 {
00835                                                         for (i=0;i<ACGY_BAR_LEN/2;i++)
00836                                                                 bar[0][i]=' '; 
00837                                                                 
00838 
00839                                             bar[0][ACGY_BAR_LEN/2]='0'; 
00840                                         
00841                                                         for (i=ACGY_BAR_LEN/2+1;i<ACGY_BAR_LEN/2+n+1;i++)
00842                                                           bar[0][i]='+';
00843                                  
00844                                                         for (i=ACGY_BAR_LEN/2+n+1;i<ACGY_BAR_LEN;i++)
00845 
00846                                                                 bar[0][i]=' ';
00847                                                 }
00848                                                                  
00849                                           bar[0][ACGY_BAR_LEN/2]='0';
00850                                         
00851                                                 bar[0][ACGY_BAR_LEN]='\0';
00852                                         
00853                                                 printf("        |%s|",bar[0]);
00854                                                 
00855                                                 printf("\nacc  Z: ");
00856                                                 dmean=0;
00857                                                 for (i=2;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00858                                                 {
00859                                                         dval=accel_array[i]*KOALA_ACCEL_G;
00860                                                         printf("%5.2f ",dval);
00861                                                         dmean+=dval;
00862                                                 }
00863                                                 
00864                                                 dval=dmean/10.0;
00865                                                 printf(" %5.2f",dval);
00866                                                 
00867                                                 // compute bar index
00868                                                 n = (int)abs(dval*ACGY_BAR_LEN/MAX_G/2.0); 
00869                                                 // fill up bar
00870                                                 if (dval < 0)
00871                                                 {
00872                                                         for (i=0;i<ACGY_BAR_LEN/2-n;i++)
00873                                                                 bar[0][i]=' ';
00874                                                         for (i=ACGY_BAR_LEN/2-n;i<ACGY_BAR_LEN/2;i++)
00875                                                                 bar[0][i]='-';
00876                                                         bar[0][ACGY_BAR_LEN/2]='0';
00877                                                                 
00878                                                         for (i=ACGY_BAR_LEN/2+1;i<ACGY_BAR_LEN;i++)
00879                                                                 bar[0][i]=' ';  
00880                                                 }
00881                                                 else
00882                                                 {
00883                                                         for (i=0;i<ACGY_BAR_LEN/2;i++)
00884                                                                 bar[0][i]=' '; 
00885                                                                 
00886                                             bar[0][ACGY_BAR_LEN/2]='0'; 
00887                                         
00888                                                         for (i=ACGY_BAR_LEN/2+1;i<ACGY_BAR_LEN/2+n+1;i++)
00889                                                           bar[0][i]='+';
00890                                  
00891                                                         for (i=ACGY_BAR_LEN/2+n+1;i<ACGY_BAR_LEN;i++)
00892                                                                 bar[0][i]=' ';
00893                                                 }
00894                                                                  
00895                                           bar[0][ACGY_BAR_LEN/2]='0';
00896                                         
00897                                                 bar[0][ACGY_BAR_LEN]='\0';
00898                                         
00899                                                 printf("        |%s|",bar[0]);
00900                                                 
00901                                                 
00902                                                 printf("\n\nPush any key to stop\n");
00903                                                 usleep(200000); // wait 200ms
00904                                         }
00905                                         tcflush(0, TCIFLUSH);
00906                                 } else
00907                                 if ((strlen(line)>1 && (line[1]=='d')))
00908                                 {       // A/D  
00909                                         while(!koala_kbhit())
00910                                         {       
00911                                                 koala_clrscr();
00912                                                 if (koala_read_ad(&ad_0,&ad_1)<0)
00913                                                 {
00914                                                         printf("\nERROR: could not get A/D data!\n");
00915                                                         break;
00916                                                 }
00917                                                 printf("AD0: %3d (%.3f V)  AD1: %3d (%.3f V)\n",ad_0,ad_0*KOALA_AD_TO_V,ad_1,ad_1*KOALA_AD_TO_V);
00918                                                 printf("\n\nPush any key to stop\n");
00919                                                 usleep(200000); // wait 200ms
00920                                         }
00921                                         tcflush(0, TCIFLUSH);
00922                                 }       else
00923                                 if ((strlen(line)>1 && (line[1]=='m')))
00924                                 { // auto monitoring mode
00925                                         if (EOF==sscanf(line,"%*c%*c %d",&auto_mon))
00926                                                 printf("\n*** ERROR ***: auto_mon not correct\n");
00927                                         else
00928                                         {
00929                                                 koala_configure_auto_monitoring_mode(auto_mon);
00930                                                 printf("\nPush any key to stop\n");
00931                                                 sleep(1);
00932                                                 while(!koala_kbhit())
00933                                                 {       
00934                                                         if (koala_get_from_auto_mode(&auto_data)>=0) {
00935                                         //printf("\nreceived mode %c: \n",auto_data.mode); 
00936           
00937                                                                 switch(auto_data.mode)
00938                                                                 {
00939                                                                         case 'e' : // Motor Speed
00940                                                                                  printf("\nMotor speed  : left: %5d  right: %5d\n",auto_data.left_speed,auto_data.right_speed);
00941                                                                         break;
00942                                                                         case 'g' : // US sensor
00943                                                                                 printf("\nUS sensors:\n");
00944                                                                                 for (i=0; i<KOALA_US_SENSORS_NUMBER;i++)
00945                                                                                 {
00946                                                                                         printf("  US %d: %d\n",i,auto_data.us[i]);
00947                                                                                 }
00948                                                                         break;
00949                                                                         case 'm' : // Accelerometer value
00950                                                                                         printf("\nAcceleration sensor [g]\n       new data                                                      old data    average\naxis X: ");
00951                                                                                         dmean=0;
00952                                                                                         for (i=0;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00953                                                                                         {
00954                                                                                                 dval=auto_data.accel[i]*KOALA_ACCEL_G;
00955                                                                                                 printf("%6.1f ",dval);
00956                                                                                                 dmean+=dval;                                                       
00957                                                                                         }   
00958                                                 
00959                                                                                         printf(" %6.1f",dmean/10.0);
00960 
00961                                                                                         printf("\naxis Y: ");
00962                                                                                         dmean=0;
00963                                                                                         for (i=1;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00964                                                                                         {
00965 
00966                                                                                                 dval=auto_data.accel[i]*KOALA_ACCEL_G;
00967                                                                                                 printf("%6.1f ",dval);
00968                                                                                                 dmean+=dval;                                                                                                       
00969                                                                                         } 
00970                                                                                         printf(" %6.1f",dmean/10.0);
00971                                                         
00972                                                                                         printf("\naxis Z: ");
00973                                                                                         dmean=0;
00974                                                                                         for (i=2;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00975                                                                                         {
00976                                                                                                 dval=auto_data.accel[i]*KOALA_ACCEL_G;
00977                                                                                                 printf("%6.1f ",dval);
00978                                                                                                 dmean+=dval;                                                                                                       
00979                                                                                         }
00980                                                                                         printf(" %6.1f",dmean/10.0);     
00981                                                                                         printf("\n"); 
00982                                                                         break;
00983                                                                         case 'n' : // Gyroscope value
00984                                                                                         printf("\ngyro sensor [deg/s]\n       new data                                                      old data    average\ngyro X: ");
00985                                                                                         dmean=0;
00986                                                                                         for (i=0;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
00987                                                                                         {
00988                                                                                                 dval=auto_data.gyro[i]*KOALA_GYRO_DEG_S;
00989                                                                                                 printf("%6.1f ",dval);
00990                                                                                                 dmean+=dval;                                                       
00991                                                                                         }   
00992                                                 
00993                                                                                         printf(" %6.1f",dmean/10.0);
00994 
00995                                                                                         printf("\ngyro Y: ");
00996                                                                                         dmean=0;
00997                                                                                         for (i=1;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
00998                                                                                         {
00999 
01000                                                                                                 dval=auto_data.gyro[i]*KOALA_GYRO_DEG_S;
01001                                                                                                 printf("%6.1f ",dval);
01002                                                                                                 dmean+=dval;                                                                                                       
01003                                                                                         } 
01004                                                                                         printf(" %6.1f",dmean/10.0);
01005                                                         
01006                                                                                         printf("\ngyro Z: ");
01007                                                                                         dmean=0;
01008                                                                                         for (i=2;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
01009                                                                                         {
01010                                                                                                 dval=auto_data.gyro[i]*KOALA_GYRO_DEG_S;
01011                                                                                                 printf("%6.1f ",dval);
01012                                                                                                 dmean+=dval;                                                                                                       
01013                                                                                         }
01014                                                                                         printf(" %6.1f",dmean/10.0);     
01015                                                                                         printf("\n");  
01016                                                                         break;
01017                                                                         case 'o' : // Motor Current
01018                                                                                 printf("\nMotor current: left: %5d  right: %5d\n",auto_data.left_current,auto_data.right_current);
01019                                                                         break;
01020                                                                         case 'p' : // Motor Position
01021                                                                                  printf("\nMotor current: left: %5d  right: %5d\n",auto_data.left_position,auto_data.right_position);
01022                                                                         break;
01023                                                                         case 'q' : // GPS data  
01024                                                 printf("\nGPS data:\
01025                                                                                 \n  valid mode: %c\
01026                                                               \n  sat number: %2d\
01027                                                               \n  latitude  : %5.4f %c\
01028                                                               \n  longitude : %5.4f %c\
01029                                                               \n  time      : %02d:%02d:%02d\
01030                                                               \n  date      : %02d.%02d.%02d\
01031                                                               \n  speed     : %3.1f [knots]\
01032                                                               \n  altitude  : %5d [m]\n",auto_data.gps.valid_sat,auto_data.gps.sat_nb,auto_data.gps.lat_val,auto_data.gps.lat_car,auto_data.gps.long_val,auto_data.gps.long_car,auto_data.gps.date_time.tm_hour,auto_data.gps.date_time.tm_min,auto_data.gps.date_time.tm_sec,auto_data.gps.date_time.tm_mday,auto_data.gps.date_time.tm_mon,auto_data.gps.date_time.tm_year,auto_data.gps.speed,auto_data.gps.altitude);
01033                                                                         break;
01034                                                                         case '$' : // GPS raw data
01035                                                                                 printf("\nGPS raw data: %s\n",auto_data.gps_raw);
01036                                                                         break;
01037                                                                         case '@':
01038                                                                                 printf("\nMagnetometer: x:%5d  y:%5d  z:%5d [mGauss]\n",auto_data.magne[0],auto_data.magne[1],auto_data.magne[2]);
01039                                                                         break;
01040                                                                         default:
01041                                                                          printf("\nERROR: received invalid auto mode: %c (0x%x)\n",auto_data.mode,auto_data.mode);
01042                         
01043                                                                 }
01044                                         
01045                                                         } 
01046                                                                 
01047                                                         //printf("\nPush any key to stop\n");   
01048                                                         usleep(10000);
01049                                                 }
01050                                                 auto_mon=0;
01051                                                 koala_configure_auto_monitoring_mode(auto_mon);
01052                                                 tcflush(0, TCIFLUSH);
01053                                         }       
01054                                 }       
01055 
01056                                 
01057                         break;
01058                         case 'b': // braitenberg, battery status
01059                           if ((strlen(line)>1) && (line[1]=='r')) {
01060                                 // Braitenberg
01061                                 braitenbergAvoidance();
01062                                 tcflush(0, TCIFLUSH);
01063                           } else
01064                           {
01065                                         // battery status
01066                                         while(!koala_kbhit())
01067                                         {       
01068                                                 koala_clrscr(); 
01069                                                 if (koala_get_battery_data(&Bat_Type,&Bat_Voltage,&Bat_Current,&Chrg_Current)<0)
01070                                                 {
01071                                                         printf("\nERROR: could not get battery data!\n");
01072                                                         break;
01073                                                 }
01074                                                 printf("\nBattery values:\
01075                         \n  Type              : %s\
01076                         \n  Voltage           :  %3.1f [V]\
01077                         \n  Current           : %5d [mA]\
01078                         \n  Charge current    : %5d [mA]\
01079                         \n  Push any key to stop\n", 
01080                         Bat_Type==1?"NiMH":(Bat_Type==0?"Li-ION":"Not Init"),Bat_Voltage/10.0,Bat_Current*100,Chrg_Current*10);
01081                                                 printf("\nPush any key to stop\n");
01082                                                 usleep(200000); // wait 200ms
01083                                         }
01084                                         tcflush(0, TCIFLUSH);
01085                                 }
01086                         break;                          
01087                         case 'd': // drive mode
01088                                 drive_robot();
01089                         break;          
01090                         case 'g':       // get gps  OR get motor speed ,position        and current                                                             
01091                                 if ((strlen(line)>2) && (line[1]=='p'))
01092                                 {       // get gps      
01093                                         while(!koala_kbhit())
01094                                         {       
01095                                                 koala_clrscr();                                 
01096                                                 if (koala_gps_data(&valid_sat,&sat_nb,&lat_val,&lat_car,&long_val,&long_car,&date_time,&speed,&altitude)<0)
01097                                                 {
01098                                                         printf("\nERROR: could not get GPS data!\n");
01099                                                         break;
01100                                                 }
01101                                                 printf("GPS data:\n data validity: %s\n  sat number: %d\n  latitude: %5.4f %c\n  longitude: %5.4f %c\n  time: %02d:%02d:%02d\n  date: %02d.%02d.%02d\n  speed: %3.1f [knots]\n  altitude: %d\n\n",valid_sat=='A'?"yes":" no",sat_nb,lat_val,lat_car,long_val,long_car,date_time.tm_hour,date_time.tm_min,date_time.tm_sec,date_time.tm_mday,date_time.tm_mon,date_time.tm_year,speed,altitude);
01102                                                 printf("\nPush any key to stop\n");
01103                                                 usleep(200000); // wait 200ms   
01104                                         }       
01105                                         tcflush(0, TCIFLUSH); // flush input    
01106                                 }       else
01107                                 if ((strlen(line)>3) && (line[1]=='s'))
01108                                 {       // send gps
01109                                         if (EOF==sscanf(line,"%*c%*c %s",Buffer))
01110                                                 printf("\n*** ERROR ***: data not correct\n");
01111                                         else
01112                                         {
01113                                                 if (koala_send_gps_cmd(Buffer)<0)
01114                                                 {
01115                                                         printf("\nERROR: could not send GPS data!\n");
01116                                                         break;
01117                                                 }
01118                                         }       
01119                                 } 
01120                                 else
01121                                 { // // get motor speed ,position       and current     
01122                                         while(!koala_kbhit())
01123                                         {       
01124                                                 koala_clrscr();
01125                                                 if (koala_read_motor_speed(&lspeed,&rspeed)<0)
01126                                                 {
01127                                                         printf("\nERROR: could not get motor speed!\n");
01128                                                         break;
01129                                                 }               
01130                                                 if (koala_read_motor_current(&lcur,&rcur)<0)
01131                                                 {
01132                                                         printf("\nERROR: could not get motor current!\n");
01133                                                         break;
01134                                                 }       
01135                                                 if (koala_read_motor_position(&lpos,&rpos)<0)
01136                                                 {
01137                                                         printf("\nERROR: could not get motor position!\n");
01138                                                         break;
01139                                                 }
01140                                                 printf("motors speed [mm/s (pulse/ %dms)]: left: %7.1f  (%5d)  | right: %7.1f (%5d) \n",KOALA_TIME_BTWN,lspeed*KOALA_SPEED_TO_MM_S,lspeed,rspeed*KOALA_SPEED_TO_MM_S,rspeed);
01141                                                 printf("motors position [mm (pulse)]: left: %7.1f (%7d) | right: %7.1f (%7d)\n",lpos*KOALA_PULSE_TO_MM,lpos,rpos*KOALA_PULSE_TO_MM,rpos);
01142                                                 printf("motors current [mA]: left: %4d | right: %4d\n",lcur*100,rcur*100);
01143                                                 printf("\nPush any key to stop\n");
01144                                                 usleep(200000); // wait 200ms                                   
01145                                         }
01146                                         tcflush(0, TCIFLUSH); // flush input
01147                                 }                               
01148                         break;
01149                         case 'i': // i2c read,write and scan
01150                                 if ((strlen(line)>2) && (line[1]=='r'))
01151                                 { // i2c read
01152                                         if (sscanf(line,"%*c%*c %d %d %d",&i2c_add,&i2c_reg,&n)!=3)
01153                                                 printf("\n*** ERROR ***: the number of data must be an integer\n");
01154                                         else
01155                                         {
01156                                                 if (n> KOALA_MAX_I2C_DATA)
01157                                                 {
01158                                                         printf("\n*** ERROR ***: the number of data must be less or equal to %d!\n",KOALA_MAX_I2C_DATA);
01159                                                         break;
01160                                                 }
01161 
01162                                                 if (koala_read_i2c(i2c_add,i2c_reg,n,i2c_data)<0)
01163                                                 {
01164                                                         printf("\nERROR: could not read i2c!\n");
01165                                                         break;
01166                                                 }       
01167                                 
01168                                                 printf("\ni2c data:\n");
01169                                                 for (i=0; i<n;i++)
01170                                                 {
01171                                                         printf("  %d: %d\n",i,i2c_data[i]);
01172                                                 }
01173                                         }
01174                                 }       else
01175                                 if ((strlen(line)>2) && (line[1]=='w'))
01176                                 { // i2c write
01177                                         if (sscanf(line,"%*c%*c %d %d %d %s",&i2c_add,&i2c_reg,&n,Buffer)!=4)
01178                                                 printf("\n*** ERROR ***: incorrect data\n");
01179                                         else
01180                                         {
01181                                                 char *pch;
01182                                                 i=0;
01183                                                 pch = strtok (Buffer," ");
01184                                                 while (pch != NULL)
01185                                                 {
01186                 
01187                                                         if (sscanf(pch,"%d",&i2c_data[i])!=1)
01188                                                                 break;
01189                 
01190                                                         pch = strtok (NULL," ");
01191                                                         i++;
01192                                         
01193                                                         if (i == n)
01194                                                                 break;
01195                                                                 
01196                                                         if (i > KOALA_MAX_I2C_DATA)
01197                                                                 break;  
01198                                                 }
01199                                                 
01200                                                 if (i!=n)
01201                                                 {
01202                                                         printf("\n*** ERROR ***: incorrect number of data: found %d, should be %d !\n",i,n);
01203                                                         break;
01204                                                 }
01205                                                 
01206                                                 if (koala_write_i2c(i2c_add,i2c_reg,n,i2c_data)<0)
01207                                                 {
01208                                                         printf("\nERROR: could not write i2c data!\n");
01209                                                         break;
01210                                                 }       
01211                                         }       
01212                                 }       else
01213                                 if ((strlen(line)>1 && (line[1]=='s')))
01214                                 { // i2c scan
01215                                         if (koala_scan_i2c(&n,i2c_data)<0)
01216                                         {
01217                                                 printf("\nERROR: could not scan i2c bus!\n");
01218                                                 break;
01219                                         }                                                          
01220                                         printf("\ni2c address: found %d devices\n",n);
01221                                         for (i=0; i<n;i++)
01222                                         {
01223                                                 printf(" device %d: i2c %d\n",i,i2c_data[i]);
01224                                         }
01225                                 } else
01226                                 if ((strlen(line)>1 && (line[1]=='o')))
01227                                 {               
01228                                         while(!koala_kbhit())
01229                                         {       
01230                                                 koala_clrscr();
01231                                                 if (koala_read_io(&io_state,&in_state)<0)
01232                                                 {
01233                                                         printf("\nERROR: could not read io!\n");
01234                                                         break;
01235                                                 }       
01236                                                 printf("IO_0: %d  IO_1: %d  IO_2: %d IO_3: %d  IN_0: %d IN_1: %d \n",io_state&1,(io_state&2)>>1,(io_state&4)>>2,(io_state&8)>>3,in_state&1,(in_state&2)>>1);
01237                                                 printf("\n\nPush any key to stop\n");
01238                                                 usleep(200000); // wait 200ms
01239                                         }
01240                                         tcflush(0, TCIFLUSH); // flush input
01241                                 }       
01242                         break;                          
01243                         case 'm': // set motors speed profile or speed or position, get motor status
01244                                 if ((strlen(line)>2 && (line[1]=='s') && (line[2]=='p')))
01245                                 { // speed profile
01246                                         sl=sr=0;
01247                                         if (sscanf(line,"%*c%*c%*c %d %d",&sl,&sr)!=2)
01248                                                 printf("\n*** ERROR ***: the motors speeds must be integers\n");
01249                                         else
01250                                         {
01251                                                 printf("\nspeeds to set: %d %d with speed profile\n",sl,sr);    
01252                                                 if (koala_set_motor_speed_accel(sl,sr)<0)
01253                                                 {
01254                                                         printf("\nERROR: could not set motor speed with profile!\n");
01255                                                         break;
01256                                                 }
01257                                         }
01258                                 }       else
01259                                 if ((strlen(line)>2 && (line[1]=='s') && (line[2]=='o')))
01260                                 { // speed openloop
01261                                         sl=sr=0;
01262                                         if (sscanf(line,"%*c%*c%*c %d %d",&sl,&sr)!=2)
01263                                                 printf("\n*** ERROR ***: the motors speeds must be integers\n");
01264                                         else
01265                                         {
01266                                                 printf("\nspeeds to set: %d %d with openloop\n",sl,sr); 
01267                                                 if (koala_set_motor_speed_open_loop(sl,sr)<0)
01268                                                 {
01269                                                         printf("\nERROR: could not set motor speed with openloop!\n");
01270                                                         break;
01271                                                 }
01272                                         }       
01273                                 }       else                            
01274                                 if ((strlen(line)>2 && (line[1]=='s') && (line[2]=='t')))
01275                                 { // get motor status
01276                                         if (koala_get_motor_status(&left_status,&right_status,&left_pos,&right_pos)<0)
01277                                         {
01278                                                 printf("\nERROR: could not get motor status!\n");
01279                                                 break;
01280                                         }
01281                                         printf("\nleft motor : control: %s  position: %sreached\n",motor_ctrl_status_str[left_status],left_pos?"":"not ");
01282                                         printf("right motor: control: %s  position: %sreached\n",motor_ctrl_status_str[right_status],right_pos?"":"not ");              
01283                                 }       else
01284                                 
01285                                 if ((strlen(line)>2 && (line[1]=='s')))
01286                                 { // speed
01287                                         sl=sr=0;
01288                                         if (sscanf(line,"%*c%*c %d %d",&sl,&sr)!=2)
01289                                                 printf("\n*** ERROR ***: the motors speeds must be integers\n");
01290                                         else
01291                                         {
01292                                                 printf("\nspeeds to set: %d %d with speed\n",sl,sr);    
01293                                                 if (koala_set_motor_speed(sl,sr)<0)
01294                                                 {
01295                                                         printf("\nERROR: could not set motor speed!\n");
01296                                                         break;
01297                                                 }
01298                                         }       
01299                                 }       
01300                                 else
01301                                 if ((strlen(line)>1 && (line[1]=='a')))
01302                                 { // magnetometer
01303                                         koala_read_magnetometer(magne);
01304                                         printf("\nMagnetometer: x:%5d  y:%5d  z:%5d [mGauss]\n",magne[0],magne[1],magne[2]);
01305                                 } else
01306                                 if ((strlen(line)>1 && (line[1]=='p')))
01307                                 { // position
01308                                         sl=sr=0;
01309                                         if (sscanf(line,"%*c%*c %d %d",&sl,&sr)!=2)
01310                                                 printf("\n*** ERROR ***: the motors positions must be integers\n");
01311                                         else
01312                                         {
01313                                                 printf("\nposition to set: %d %d with position regulation\n",sl,sr);    
01314                                                 if (koala_set_motor_target_position(sl,sr)<0)
01315                                                 {
01316                                                         printf("\nERROR: could not set motor target position!\n");
01317                                                         break;
01318                                                 }
01319                                         }       
01320                                 }
01321                         break;
01322                         case 'p':  //pid,position, set power io
01323                                 if ((strlen(line)>2 && (line[1]=='i')))
01324                                 { // pid
01325                                         if (sscanf(line,"%*c%*c %d %d %d",&kp,&ki,&kd)==3)
01326                                         {
01327                                                 if (koala_configure_pid(kp,ki,kd)<0)
01328                                                 {
01329                                                         printf("\nERROR: could not configure pid!\n");
01330                                                         break;
01331                                                 }
01332                                         }
01333                                 }
01334                                 else
01335                                 if ((strlen(line)>2 && (line[1]=='a')))
01336                                 { 
01337                                         // us and io parameters
01338                                         if (sscanf(line,"%*c%*c %d %d",&us_mask,&io_dir)==2)
01339                                         {
01340                                                 if (koala_configure_us_io(us_mask,io_dir)<0)
01341                                                 {
01342                                                         printf("\nERROR: could not configure us and io parameters!\n");
01343                                                         break;
01344                                                 }       
01345                                         }
01346                                 } else
01347                                 if ((strlen(line)>2 && (line[1]=='o')))
01348                                 { 
01349                                         // set power io and io
01350                                         if (sscanf(line,"%*c%*c %d %d %d %d %d",&power_out,&IO0,&IO1,&IO2,&IO3)==5)
01351                                         {                                               
01352                                                 if (koala_set_pwr_io_output(power_out,IO0,IO1,IO2,IO3)<0)
01353                                                 {
01354                                                         printf("\nERROR: could not set power io and io!\n");
01355                                                         break;
01356                                                 }               
01357                                         }
01358                                 }                                       
01359                         break;  
01360                         case 'q': // quit
01361                                 quitReq=1;
01362                                 break;
01363                         case 'r': // reset  encoders the microcontroller
01364                                 if ((strlen(line)>1 && (line[1]=='e')))
01365                                 { //or encoders                                 
01366                                         if (koala_set_position_encoders(0,0)<0)
01367                                         {
01368                                                 printf("\nERROR: could not reset encoders!\n");
01369                                                 break;
01370                                         }       
01371                                 }
01372                                 else
01373                                         if ((strlen(line)>1 && (line[1]=='m')))
01374                                         {// reset the microcontroller 
01375                                                 if (koala_reset_microcontroller()<0)
01376                                                 {
01377                                                         printf("\nERROR: could not reset microcontroller!\n");
01378                                                         break;
01379                                                 }       
01380                                                 printf("\nWait for microcontroller reset...\n");
01381                                                 sleep(3); // wait for Koala to restart
01382                                                 koala_getcommand_line_nowait(Buffer) ; // read any remaining line; usualy Koala version
01383                                                 printf("\nMicrocontroller reset OK (%s) !\n",Buffer);
01384                                         }       
01385                         break;
01386                         case 's': // speed profile or stop motor                        
01387                                 if ((strlen(line)>2 && (line[1]=='p')))
01388                                 { // speed profile
01389                                         if (sscanf(line,"%*c%*c %d %d %d %d %d %d",&acc_inc,&acc_div,&min_spacc, &cst_speed,&pos_margin,&max_current)==6)
01390                                         {       
01391                                                 if (koala_set_speed_profile(acc_inc,acc_div,min_spacc,cst_speed,pos_margin,max_current)<0)
01392                                                 {
01393                                                         printf("\nERROR: could not set speed profile!\n");
01394                                                         break;
01395                                                 }
01396                                         }
01397                                 }
01398                                 else
01399                                 if ((strlen(line)>2 && (line[1]=='o')))
01400                                 {
01401                                         test_sound();
01402                                 }
01403                                 else
01404                                 {
01405                                         //stop motor
01406                                         if (koala_set_motor_speed(0,0)<0)
01407                                         {
01408                                                 printf("\nERROR: could not set stop motor!\n");
01409                                                 break;
01410                                         }
01411                                 }               
01412                         break;
01413                         case 'u': // us sensors
01414                                  // display us
01415                                         while(!koala_kbhit())
01416                                         {       
01417                                                 koala_clrscr();
01418                 
01419                                                 // get and print us sensors
01420                                                 if (koala_read_us_sensors(us_sensors)<0)
01421                                                 {
01422                                                         printf("\nERROR: could notread us sensors!\n");
01423                                                         break;
01424                                                 }
01425 
01426                                                 printf("\nUS sensors : distance [cm]\
01427                                                                                 \n                        50  100  150  200\
01428                                                                                 \n                  0|    .    :    .    :   |%.0f\n",MAX_US_DISTANCE);
01429                                                 for (i=0;i<KOALA_US_SENSORS_NUMBER;i++)
01430                                                 {  
01431                                                         
01432                                                         if((us_sensors[i] == KOALA_US_DISABLED_SENSOR) || (us_sensors[i] == KOALA_US_NO_OBJECT_IN_RANGE))
01433                                                         { // out of range or disabled
01434                                                                 sprintf(bar[i],"|\33[%dC|",US_BAR_LEN); 
01435                                                         }  else
01436                                                         {
01437                                                                 // in range or less than 25cm
01438                                                                 n=(int)(us_sensors[i]*US_BAR_LEN/MAX_US_DISTANCE);
01439                                                                                                                                                                         
01440                                                                 if (n==0)
01441                                                                         sprintf(bar[i],"|>\33[%dC|",US_BAR_LEN-1);
01442                                                                 else
01443                                                                         if (n>=US_BAR_LEN-1)
01444                                                                                 sprintf(bar[i],"|\33[%dC>|",US_BAR_LEN-1);
01445                                                                         else
01446                                                                          sprintf(bar[i],"|\33[%dC>\33[%dC|",n,US_BAR_LEN-1-n); 
01447                                                                 
01448                                                                 
01449                                                         }  
01450                                                         printf("%11s : %4d %s  %s\n",KOALA_US_SENSOR_NAMES[i],us_sensors[i],bar[i],US_VAL(us_sensors[i]));                                     
01451                                                 }                                               
01452                                                 printf("\nPush any key to end program\n");
01453                                                 usleep(200000); // wait 200ms
01454                                         }
01455                                         tcflush(0, TCIFLUSH); // flush input
01456                         break;
01457                         case ' ':
01458                                         if (koala_set_motor_speed(0,0)<0)
01459                                         {
01460                                                 printf("\nERROR: could not set stop motor!\n");
01461                                                 break;
01462                                         }
01463                         break;
01464                         default: 
01465                                 printf("\n*** ERROR ***: option %s (first char: 0x%02x) is undefined!\n\n",line,line[0]);               
01466                 }
01467 
01468   } // while
01469   
01470   koala_set_motor_speed_open_loop(0 ,0); // stop robot and release motors
01471   
01472             
01473         return 0;
01474 }