Koala Library
koala_auto_mode_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 
00051 int main(int argc, char *argv[]) {
00052   int rc=0;
00053   int i;
00054   double dval;
00055   koala_auto_data_t data;
00056   unsigned int bit_conf;
00057   char revision,version;
00058   double dmean;
00059   
00060         // initialise koala library
00061   if((rc=koala_init( argc , argv )) < 0 )
00062   {
00063     fprintf(stderr,"ERROR %d: Unable to initialize the koala library!\n",rc);
00064     return -1;
00065   }
00066 
00067 
00068   printf("Koala V2.5 robot Auto mode test program\n(C) K-Team S.A\n");
00069   
00070   // get robot firmware version and revision
00071   rc=koala_get_firmware_version_revision(&version,&revision);
00072   
00073   if (rc<0)
00074   {
00075                 rc=koala_get_firmware_version_revision(&version,&revision);
00076   
00077                 if (rc<0) // retry, because of the serial start level
00078                 {
00079                   fprintf(stderr,"ERROR %d: Koala did not respond correctly!\n",rc);
00080                   return -2;
00081                 } 
00082     
00083   } 
00084   
00085   
00086   printf("Koala version: %c  revision: %d\n",version,revision);
00087         
00088         // Set auto mode configuration
00089         
00090         bit_conf=KOALA_AUTOM_MOTOR_POSITION | KOALA_AUTOM_MAGNE_VALUE;
00091         
00092         //bit_conf=KOALA_AUTOM_ALL;
00093      
00094   //bit_conf=KOALA_AUTOM_GPS_DATA ;//KOALA_AUTOM_ACCEL_VALUE; //KOALA_AUTOM_US_SENSOR_BIT | KOALA_AUTOM_GPS_DATA ;
00095 
00096         rc= koala_configure_auto_monitoring_mode(bit_conf);
00097         if (rc<0)
00098   {
00099     fprintf(stderr,"ERROR %d: could not set auto mode configuration!\n",rc);
00100     return -4;
00101   }  
00102         
00103   printf("Read serial until any key pushed!\n\n");
00104   while(koala_kbhit()==0)
00105   {
00106         if (koala_get_from_auto_mode(&data)>=0) {
00107           //printf("\nreceived mode %c: \n",data.mode); 
00108           
00109                   switch(data.mode)
00110                         {
00111                                 case 'e' : // Motor Speed
00112                                          printf("\nMotor speed  : left: %5d  right: %5d\n",data.left_speed,data.right_speed);
00113                                 break;
00114                                 case 'g' : // US sensor
00115                                         printf("\nUS sensors:\n");
00116                                         for (i=0; i<KOALA_US_SENSORS_NUMBER;i++)
00117                                         {
00118                                                 printf("  US %d: %d\n",i,data.us[i]);
00119                                         }
00120                                 break;
00121                                 case 'm' : // Accelerometer value
00122                                                 printf("\nAcceleration sensor [g]\n       new data                                                      old data    average\naxis X: ");
00123                                                 dmean=0;
00124                                                 for (i=0;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00125                                                 {
00126                                                         dval=data.accel[i]*KOALA_ACCEL_G;
00127                                                         printf("%6.1f ",dval);
00128                                                         dmean+=dval;                                                       
00129                                                 }   
00130                                                 
00131                                                 printf(" %6.1f",dmean/10.0);
00132 
00133                                                 printf("\naxis Y: ");
00134                                                 dmean=0;
00135                                                 for (i=1;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00136                                                 {
00137 
00138                                                         dval=data.accel[i]*KOALA_ACCEL_G;
00139                                                         printf("%6.1f ",dval);
00140                                                         dmean+=dval;                                                                                                       
00141                                                 } 
00142                                                 printf(" %6.1f",dmean/10.0);
00143                                                         
00144                                                 printf("\naxis Z: ");
00145                                                 dmean=0;
00146                                                 for (i=2;i<KOALA_ACCEL_VALUES_NUMBER;i+=3)
00147                                                 {
00148                                                         dval=data.accel[i]*KOALA_ACCEL_G;
00149                                                         printf("%6.1f ",dval);
00150                                                         dmean+=dval;                                                                                                       
00151                                                 }
00152                                                 printf(" %6.1f",dmean/10.0);     
00153                                                 printf("\n"); 
00154                                 break;
00155                                 case 'n' : // Gyroscope value
00156                         printf("\ngyro sensor [deg/s]\n       new data                                                      old data    average\ngyro X: ");
00157                                                 dmean=0;
00158                                                 for (i=0;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
00159                                                 {
00160                                                         dval=data.gyro[i]*KOALA_GYRO_DEG_S;
00161                                                         printf("%6.1f ",dval);
00162                                                         dmean+=dval;                                                       
00163                                                 }   
00164                                                 
00165                                                 printf(" %6.1f",dmean/10.0);
00166 
00167                                                 printf("\ngyro Y: ");
00168                                                 dmean=0;
00169                                                 for (i=1;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
00170                                                 {
00171 
00172                                                         dval=data.gyro[i]*KOALA_GYRO_DEG_S;
00173                                                         printf("%6.1f ",dval);
00174                                                         dmean+=dval;                                                                                                       
00175                                                 } 
00176                                                 printf(" %6.1f",dmean/10.0);
00177                                                         
00178                                                 printf("\ngyro Z: ");
00179                                                 dmean=0;
00180                                                 for (i=2;i<KOALA_GYRO_VALUES_NUMBER;i+=3)
00181                                                 {
00182                                                         dval=data.gyro[i]*KOALA_GYRO_DEG_S;
00183                                                         printf("%6.1f ",dval);
00184                                                         dmean+=dval;                                                                                                       
00185                                                 }
00186                                                 printf(" %6.1f",dmean/10.0);     
00187                                                 printf("\n");  
00188                                 break;
00189                                 case 'o' : // Motor Current
00190                                         printf("\nMotor current: left: %5d  right: %5d\n",data.left_current,data.right_current);
00191                                 break;
00192                                 case 'p' : // Motor Position
00193                                          printf("\nMotor current: left: %5d  right: %5d\n",data.left_position,data.right_position);
00194                                 break;
00195                                 case 'q' : // GPS data  
00196                                 
00197                                          printf("\nGPS data:\
00198                                                                         \n  valid mode: %c\
00199                                                 \n  sat number: %2d\
00200                                                 \n  latitude  : %5.4f %c\
00201                                                 \n  longitude : %5.4f %c\
00202                                                 \n  time      : %02d:%02d:%02d\
00203                                                 \n  date      : %02d.%02d.%02d\
00204                                                 \n  speed     : %3.1f [knots]\
00205                                                 \n  altitude  : %5d [m]\n",data.gps.valid_sat,data.gps.sat_nb,data.gps.lat_val,data.gps.lat_car,data.gps.long_val,data.gps.long_car,data.gps.date_time.tm_hour,data.gps.date_time.tm_min,data.gps.date_time.tm_sec,data.gps.date_time.tm_mday,data.gps.date_time.tm_mon,data.gps.date_time.tm_year,data.gps.speed,data.gps.altitude);
00206                                 break;
00207                                 case '$' : // GPS raw data
00208                                         printf("\nGPS raw data: %s\n",data.gps_raw);
00209                                 break;
00210                                 case '@':
00211                                         printf("\nMagnetometer: x:%5d  y:%5d  z:%5d [mGauss]\n",data.magne[0],data.magne[1],data.magne[2]);
00212                                 break;
00213                                 default:
00214                                  printf("\nERROR: received invalid auto mode: %c (0x%x)\n",data.mode,data.mode);
00215                         
00216                         }
00217           
00218         } 
00219                 
00220                 
00221         usleep(10000);
00222   }
00223   
00224   bit_conf=KOALA_AUTOM_NONE;
00225   
00226   koala_configure_auto_monitoring_mode(bit_conf);
00227 
00228             
00229         return 0;
00230 }