Koala Library
|
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 }