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 #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 }