Koala Library
koala_example.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 #include <limits.h>
00039 
00040 #include <koala/koala.h> // koala library
00041 
00042 int main(int argc, char *argv[]) {
00043   int rc=0;
00044   int i,n;
00045   char version,revision;
00046   int Bat_Type,Bat_Voltage,Bat_Current,Bat_Capacity,Chrg_Current;
00047   unsigned int bit_conf;
00048   int us_mask;
00049   unsigned char io_dir;
00050   int sat_nb;double lat_val;char lat_car;double long_val;char long_car;struct tm date_time;double speed;int altitude; char valid_sat;  
00051   int us_sensors[KOALA_US_SENSORS_NUMBER]; 
00052   #define DATA_NUMBER 5
00053   int data[DATA_NUMBER];
00054   int lspeed,lpos,lcur,rspeed,rpos,rcur;
00055   int accel_array[KOALA_ACCEL_VALUES_NUMBER],gyro_array[KOALA_GYRO_VALUES_NUMBER];
00056   int magne_array[KOALA_MAGNE_VALUES_NUMBER];
00057   koala_auto_data_t auto_data;
00058   int magne_xmax,magne_xmin,magne_ymax,magne_ymin,magne_xrange,magne_yrange,magne[KOALA_MAGNE_VALUES_NUMBER];
00059   double magne_x,magne_y,angle,goal_angle;
00060   int out,auto_ctrl,refresh;
00061   char c;
00062   char line[80];
00063   
00064         // initialise koala library
00065   if((koala_init( argc , argv )) < 0 )
00066   {
00067     fprintf(stderr,"ERROR: Unable to initialize the koala library!\n");
00068     return -1;
00069   }
00070 
00071 
00072   printf("Koala V2.5 robot example program\n(C) K-Team S.A\n");
00073   
00074   
00075   // get robot firmware version and revision  
00076   rc=koala_get_firmware_version_revision(&version,&revision);
00077   
00078   if (rc<0)
00079   {
00080                 rc=koala_get_firmware_version_revision(&version,&revision);
00081   
00082                 if (rc<0) // retry, because of the serial start level
00083                 {
00084                   fprintf(stderr,"ERROR %d: Koala did not respond correctly!\n",rc);
00085                   return -2;
00086                 } 
00087     
00088   } 
00089   
00090   printf("Koala version: %c  revision: %d\n",version,revision);
00091   
00092   /*
00093   
00094   // ---- BATTERY ----
00095   // Get battery values
00096   rc=koala_get_battery_data(&Bat_Type,&Bat_Voltage,&Bat_Current,&Chrg_Current);
00097   
00098   if (rc<0)
00099   {
00100     fprintf(stderr,"ERROR %d: could not read Koala battery values!\n",rc);
00101     return -3;
00102   } 
00103     
00104   printf("\nBattery values:\
00105           \nType              :%s\
00106           \nVoltage           : %3.1f [V]\
00107           \nCurrent           : %4d [mA]\
00108           \nCharge current    : %4d [mA]\n" 
00109             ,Bat_Type==1?"NiMH":(Bat_Type==0?"Li-ION":"Not Init"),Bat_Voltage/10.0,Bat_Current*100,Chrg_Current*10);
00110 
00111         
00112         
00113         // Set auto mode configuration  
00114   bit_conf=KOALA_AUTOM_NONE;
00115         rc= koala_configure_auto_monitoring_mode(bit_conf);
00116         if (rc<0)
00117   {
00118     fprintf(stderr,"ERROR %d: could not set auto mode configuration!\n",rc);
00119     return -4;
00120   }  
00121         
00122         
00123         // ---- IO AND US CONFIGURATION ----
00124         // Set some US and IO
00125         us_mask=KOALA_US_LEFT_FRONT | KOALA_US_RIGHT_FRONT | KOALA_US_BACK_LEFT | KOALA_US_BACK_RIGHT;
00126         io_dir=KOALA_IO_0_INPUT | KOALA_IO_1_PWMS | KOALA_IO_2_OUTPUT | KOALA_IO_3_INPUT;
00127 
00128         rc=koala_configure_us_io(us_mask,io_dir);
00129         if (rc<0)
00130   {
00131     fprintf(stderr,"ERROR %d: could not set US, IO and GPS rate parameters!\n",rc);
00132     return -5;
00133   }
00134   
00135   
00136   // ---- US SENSORS ----
00137   // read US sensors
00138   rc=koala_read_us_sensors(us_sensors);
00139   if (rc<0)
00140   {
00141     fprintf(stderr,"ERROR %d: could not set US, IO and GPS rate parameters!\n",rc);
00142     return -6;
00143   }
00144   
00145   
00146   printf("\nUS sensors:\n");
00147   for (i=0; i<KOALA_US_SENSORS_NUMBER;i++)
00148   {
00149     printf("  US %d: %d\n",i,us_sensors[i]);
00150   }
00151 
00152         // ---- GPS ----
00153         // Read gps
00154         rc=koala_gps_data(&valid_sat,&sat_nb,&lat_val,&lat_car,&long_val,&long_car,&date_time,&speed,&altitude);
00155         
00156         if (rc<0)
00157   {
00158     fprintf(stderr,"ERROR %d: could get GPS data!\n",rc);
00159     return -7;
00160   }
00161         
00162         printf("\nGPS data:\n  data validity: %s 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",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);
00163   
00164   
00165   // ---- I2C ----
00166   // scan i2c
00167   rc=koala_scan_i2c(&n,data);                                              
00168         if (rc<0)
00169   {
00170     fprintf(stderr,"ERROR %d: could not scan i2c bus!\n",rc);
00171     return -8;
00172   }
00173   printf("\ni2c address: found %d devices\n",n);
00174   for (i=0; i<n;i++)
00175         {
00176                 printf(" device %d: i2c %d\n",i,data[i]);
00177         }
00178   
00179   
00180   // ----MOTOR CONFIGURATIONS ----
00181   // configure motor PID
00182   rc=koala_configure_pid(KOALA_MOTOR_P,KOALA_MOTOR_I,KOALA_MOTOR_D);
00183   if (rc<0)
00184   {
00185     fprintf(stderr,"ERROR %d: could not set PID data!\n",rc);
00186     return -9;
00187   }
00188   
00189   // set motor speed profile
00190   rc=koala_set_speed_profile(KOALA_MOTOR_ACC_INC, KOALA_MOTOR_ACC_DIV,KOALA_MOTOR_MIN_SPACC,KOALA_MOTOR_CST_SPEED,KOALA_MOTOR_POS_MARGIN,KOALA_MOTOR_MAX_CURRENT);
00191   if (rc<0)
00192   {
00193     fprintf(stderr,"ERROR %d: could not set speed profile!\n",rc);
00194     return -10;
00195   }
00196  
00197  
00198   // ---- MOTORS ENCODERS ---- 
00199    // set position encoders
00200   rc=koala_set_position_encoders(0,0); // reset encoders
00201   if (rc<0)
00202   {
00203     fprintf(stderr,"ERROR %d: could not set position encoders!\n",rc);
00204     return -11;
00205   }
00206 
00207 
00208   // ---- ACCELEROMETER / GYROSCOPE ----
00209   // make the robot rotates in place: push any key to stop
00210   
00211   lspeed=50;
00212   rspeed=-50;
00213   printf("Make the robot rotates in place at speed: %6d %6d\n\n  Push any key to stop\n\n",lspeed,rspeed);
00214   usleep(800000);
00215   
00216   // set motor speed 
00217         rc=koala_set_motor_speed_accel(lspeed,rspeed);
00218   if (rc<0)
00219   {
00220     fprintf(stderr,"ERROR %d: could not set motor speed !\n",rc);
00221     return -12;
00222   }
00223   
00224   // read speed, position, current and acceleration until any key is pushed
00225   while(koala_kbhit()==0)
00226   { 
00227         koala_read_motor_speed(&lspeed,&rspeed);
00228                 koala_read_motor_current(&lcur,&rcur);
00229                 koala_read_motor_position(&lpos,&rpos);
00230                 koala_read_accelerometer(accel_array);
00231                 koala_read_gyroscope(gyro_array);
00232                 
00233                 printf("left : speed %6d  position: %6d  current: %6d\n",lspeed,lpos,lcur);
00234                 printf("right: speed %6d  position: %6d  current: %6d\n",rspeed,rpos,rcur);
00235                 printf("\nAcceleration sensor [g]:  x: %6.3f y: %6.3f z: %6.3f\n\n",accel_array[KOALA_NEW_ACCEL_X]*KOALA_ACCEL_G,accel_array[KOALA_NEW_ACCEL_Y]*KOALA_ACCEL_G,accel_array[KOALA_NEW_ACCEL_Z]*KOALA_ACCEL_G);
00236                 printf("\nGyrometer sensor [deg/s]:  x: %6.3f y: %6.3f z: %6.3f\n\n",gyro_array[KOALA_NEW_GYRO_X]*KOALA_GYRO_DEG_S,gyro_array[KOALA_NEW_GYRO_Y]*KOALA_GYRO_DEG_S,gyro_array[KOALA_NEW_GYRO_Z]*KOALA_GYRO_DEG_S);
00237                         
00238                 
00239                 usleep(300000);
00240   }
00241   
00242   koala_set_motor_speed(0,0); // stop robot
00243   
00244   */
00245   
00246   // ---- MAGNETOMETER ----
00247   
00248    // make the robot rotates in place: push any key to stop
00249   
00250   koala_set_position_encoders(0,0); // reset encoders
00251   lspeed=50;
00252   rspeed=-50;
00253   
00254   #define NB_TURN 2.5
00255   
00256   printf("The robot will rotate in place %3.1f turns to calibrate magnetometer at speed: %6d %6d\n\n  Push any key to stop\n\n",NB_TURN,lspeed,rspeed);
00257   usleep(800000);
00258   
00259   if (rc<0)
00260   {
00261     fprintf(stderr,"ERROR %d: could not set motor speed !\n",rc);
00262     return -12;
00263   }
00264   
00265   // read speed, position, current and magnetometer until any key is pushed
00266         
00267         // Set auto mode configuration  
00268   bit_conf=KOALA_AUTOM_MOTOR_POSITION | KOALA_AUTOM_MAGNE_VALUE;   // magnetometer and position
00269         koala_configure_auto_monitoring_mode(bit_conf);
00270         
00271         
00272         magne_xmax=magne_ymax=INT_MIN;
00273         magne_xmin=magne_ymin=INT_MAX;
00274         
00275         koala_set_motor_speed_accel(lspeed,rspeed);
00276         out=0;
00277         refresh=0;
00278         
00279         
00280   while((koala_kbhit()==0) && !out)
00281   { 
00282                 
00283                 if (koala_get_from_auto_mode(&auto_data)>=0) {
00284           //printf("\nreceived mode %c: \n",data.mode); 
00285           
00286                   switch(auto_data.mode)
00287                         {
00288                                 case 'p' : // Motor Position
00289                                          if (refresh>166) { // don't print all the data
00290                                                  printf("Position: left: %6.1f degree  right: %6.1f degree  Magnetometer: x:%5d  y:%5d  z:%5d [mGauss]\n",auto_data.left_position*360*KOALA_PULSE_TO_MM/M_PI/KOALA_WHEELS_DISTANCE,auto_data.right_position*360*KOALA_PULSE_TO_MM/M_PI/KOALA_WHEELS_DISTANCE,auto_data.magne[0],auto_data.magne[1],auto_data.magne[2]);
00291                                          
00292                                                  refresh=0;      
00293                                          }
00294                                          
00295                                          if (auto_data.left_position >NB_TURN*KOALA_WHEELS_DISTANCE*M_PI/KOALA_PULSE_TO_MM )  // test number of turn
00296                                          {
00297                                                 out=1;
00298                                          }
00299                                          
00300                                 break;
00301                                 case '@':
00302                                         /*if (i>166) { // don't print all the data
00303                                                 printf("\nMagnetometer: x:%5d  y:%5d  z:%5d [mGauss]\n",auto_data.magne[0],auto_data.magne[1],auto_data.magne[2]);
00304                                                 i=0;
00305                                         }*/
00306                                         
00307                                         if (auto_data.magne[0]>magne_xmax) {
00308                                                 magne_xmax=auto_data.magne[0];
00309                                         } else
00310                                         if (auto_data.magne[0]<magne_xmin) {
00311                                                 magne_xmin=auto_data.magne[0];
00312                                         }
00313                                 
00314                                         if (auto_data.magne[1]>magne_ymax) {
00315                                                 magne_ymax=auto_data.magne[1];
00316                                         } else
00317                                         if (auto_data.magne[1]<magne_ymin) {
00318                                                 magne_ymin=auto_data.magne[1];
00319                                         }
00320                                 
00321                                 break;
00322                                 default:
00323                                  printf("\nERROR: received invalid auto mode: %c (0x%x)\n",auto_data.mode,auto_data.mode);              
00324                         }
00325           
00326         } 
00327                 refresh++;      
00328                 usleep(3000);
00329   }
00330   
00331   koala_set_motor_speed_open_loop(0,0);
00332   if (koala_kbhit())
00333         {
00334                 printf("\n\nCalibration interrupted by a key, leaving program!\n");
00335                 return -1;
00336         }
00337         
00338   
00339    
00340         // Set auto mode configuration  
00341   bit_conf=KOALA_AUTOM_NONE;   // none
00342         koala_configure_auto_monitoring_mode(bit_conf);
00343   
00344 
00345         magne_xrange=magne_xmax-magne_xmin;
00346         magne_yrange=magne_ymax-magne_ymin;
00347         
00348         #define KEY_ROT_SPEED 50
00349         #define CONT_ROT_SPEED 30
00350         out=0;
00351         auto_ctrl=0;
00352         #define ANGLE_THRESH 5.0
00353   //    control angle with arrow keys 
00354   koala_change_term_mode(1); // change terminal mode for kbhit and getchar to return immediately
00355   refresh=0;
00356         while(!out)
00357         { 
00358   
00359         if (auto_ctrl)
00360         {
00361                 if (((angle-goal_angle) < ANGLE_THRESH) && ((angle-goal_angle) > -ANGLE_THRESH))
00362                 {
00363                         auto_ctrl = 0;
00364                         koala_set_motor_speed(0,0);      // stop robot
00365                         if (goal_angle==0) // NORTH
00366                                 printf("\n\nNORTH angle reached (angle: %6.1f)!\n",angle);
00367                         else
00368                                 printf("\n\nAngle %6.1f reached (angle: %6.1f)!\n",goal_angle,angle);
00369                         fflush(stdout); // make the display refresh
00370                         usleep(2000000);
00371                         continue;
00372                 } 
00373         
00374                 // rotates correctly to approach goal 
00375                 if ((angle-goal_angle) > 0)
00376                 {
00377                         koala_set_motor_speed_accel(-CONT_ROT_SPEED ,CONT_ROT_SPEED);
00378                 } else
00379                 {
00380                         koala_set_motor_speed_accel(CONT_ROT_SPEED ,-CONT_ROT_SPEED);
00381                 }
00382                 
00383         }
00384   
00385                 if (koala_kbhit())
00386                 {
00387                         c=getchar();
00388 
00389                         // get special keys
00390                         if (c== 27  ) 
00391                         {
00392 
00393                          if (c=getchar()==91) // escape with [
00394                          {
00395                                  c = getchar(); 
00396                          
00397                                  switch(c)
00398                                  {
00399 
00400                                         case 68: // LEFT arrow = left
00401                                                 koala_set_motor_speed_accel(-KEY_ROT_SPEED ,KEY_ROT_SPEED);
00402                                         break;
00403 
00404                                         case 67: // RIGHT arrow = right
00405                                                 koala_set_motor_speed_accel(KEY_ROT_SPEED ,-KEY_ROT_SPEED);
00406                                         break;
00407 
00408                                         default:
00409                                                 //printf("special key  : %d\n",c);
00410                                         break;
00411                                         } // switch(c)
00412                                 } // escape with [      
00413         
00414                         } // if (c== '\027')     
00415                         else 
00416                         {
00417                                 switch(c)
00418                                 {                               
00419                                         case 'q': // quit to main menu
00420                                                 out=1;
00421                                                 break;
00422                                         case ' ': // spacebar
00423                                         case 's': // stop motor
00424                                           auto_ctrl=0;
00425                                                 koala_set_motor_speed(0,0);      // stop robot
00426                                         break;
00427                                         case 'n': // rotates to the North
00428                                                 auto_ctrl=1;
00429                                                 goal_angle=0;
00430                                         break;
00431                                         case 'a': // enter angle
00432                                                 koala_change_term_mode(0); // switch to normal key input mode
00433                                                 printf("\nEnter angle value in the range [0.0-360[and push ENTER: ");
00434                                                 fgets(line,80,stdin);
00435                                                 if(sscanf(line,"%lf",&goal_angle)!=1) {
00436                                                         printf("ERROR: invalid angle : %s\n",line);
00437                                                         fflush(stdout); // make the display refresh
00438                                                 usleep(2000000);
00439                                                 } else
00440                                                 {
00441                                                         if ((goal_angle >=360) || (goal_angle<0)) {
00442                                                                 printf("ERROR: invalid angle range: angle %6.1f is not in range [0.0-360[ !",goal_angle);
00443                                                                 fflush(stdout); // make the display refresh
00444                                                         usleep(2000000);
00445                                                         } else
00446                                                         {
00447                                                                 auto_ctrl=1;
00448                                                         }
00449                                                         
00450                                                         
00451                                                 }
00452                                                 koala_change_term_mode(1); // switch to special key input mode again
00453                                         break;
00454                                 default:
00455                                                 //printf("normal key : %d\n",c);
00456                                          break;
00457                                 }
00458                         }
00459                 } // if (koala_kbhit())
00460                 else
00461                 {
00462                         //koala_set_motor_speed(0,0);    // stop robot
00463                 }
00464                 
00465                 koala_read_magnetometer(magne); 
00466          
00467                 magne_x=(magne[0]-(magne_xmax+magne_xmin)/2.0)*2.0/magne_xrange;
00468                 magne_y=(magne[1]-(magne_ymax+magne_ymin)/2.0)*2.0/magne_yrange;
00469                 
00470                 
00471                 
00472                 if (magne_x==0) {
00473                 {
00474                         if (magne_y>0) {
00475                                 angle=270.0;
00476                         } else
00477                         {
00478                                 angle=90.0;
00479                         }       
00480                 }
00481                 } else
00482                 { 
00483                         angle=-atan2(magne_y,magne_x)*180/M_PI; 
00484                         if (angle <0) {
00485                                 angle+=360;
00486                         }
00487                 }       
00488                 
00489                 if (refresh>30)
00490                 {
00491                         koala_clrscr(); // erase screen
00492                         printf("Magnetometer max ranges: x: %d  y: %d\nrelative x: %3.2f  relative y: %3.2f\n",magne_xrange,magne_yrange,magne_x,magne_y); 
00493                          
00494                         if (auto_ctrl)
00495                         {
00496                                 printf("Orientation (cw): %6.1f degrees in control mode: goal_angle=%6.1f\n",angle,goal_angle);
00497                         } else
00498                         {
00499                                 printf("Orientation (cw): %6.1f degrees \n",angle);
00500                         }
00501                         printf("\nKeys\n  q to quit  \n <- -> rotate left/right\n  n rotates to the North\n  a enter the angle to rotate to\n\n");
00502                         refresh=0;
00503                 }
00504                 refresh++;
00505                 usleep(10000);
00506         }
00507   koala_change_term_mode(0); // switch to normal key input mode  
00508    
00509   koala_set_motor_speed_open_loop(0 ,0); // stop robot and release motors       
00510         return 0;
00511 }