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