00001
00015 #include <korebot/korebot.h>
00016
00017 #define MAXBUFFERSIZE 100
00018
00019 static int quitReq = 0;
00020 static char buf[1024];
00021
00022
00025 static knet_dev_t * dsPic;
00026 static knet_dev_t * mot1;
00027 static knet_dev_t * mot2;
00028
00029
00030
00031
00032
00033
00034 int kbhit(void)
00035 {
00036 struct timeval tv;
00037 fd_set read_fd;
00038
00039 tv.tv_sec=0;
00040 tv.tv_usec=0;
00041 FD_ZERO(&read_fd);
00042
00043 FD_SET(0,&read_fd);
00044
00045 if(select(1, &read_fd, NULL, NULL,
00046 &tv) == -1)
00047 return 0;
00048
00049
00050
00051
00052 if(FD_ISSET(0,&read_fd))
00053 return 1;
00054
00055
00056
00057
00058 return 0;
00059
00060 }
00061
00062
00070 void clear_screen()
00071 {
00072 printf ("\33[2J" );
00073 }
00074
00075
00085 int initMot(knet_dev_t *hDev)
00086 {
00087 if(hDev)
00088 {
00089 kmot_SetMode( hDev , kMotModeIdle );
00090 kmot_SetSampleTime( hDev , 1550 );
00091 kmot_SetMargin( hDev , 6 );
00092 if(hDev == mot1)
00093 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk | kMotSWOptDirectionInv);
00094 else
00095 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00096 kmot_ResetError( hDev );
00097 kmot_SetBlockedTime( hDev , 10 );
00098 kmot_SetLimits( hDev , kMotRegCurrent , 0 , 500 );
00099 kmot_SetLimits( hDev , kMotRegPos , -10000 , 10000 );
00100
00101
00102 kmot_ConfigurePID( hDev , kMotRegSpeed , 400 , 0 , 10 );
00103 kmot_ConfigurePID( hDev,kMotRegPos,620,3,10);
00104 kmot_SetSpeedProfile(hDev,30,10);
00105
00106 return 1;
00107 }
00108 else
00109 {
00110 printf("initMot error, handle cannot be null\r\n");
00111 return 0;
00112 }
00113 }
00114
00115
00116
00127 int initKH3( void )
00128 {
00129
00130 kh3_init();
00131
00132
00133 dsPic = knet_open( "Khepera3:dsPic" , KNET_BUS_I2C , 0 , NULL );
00134 mot1 = knet_open( "Khepera3:mot1" , KNET_BUS_I2C , 0 , NULL );
00135 mot2 = knet_open( "Khepera3:mot2" , KNET_BUS_I2C , 0 , NULL );
00136
00137 if(dsPic!=0)
00138 {
00139 if((mot1!=0)&&(mot2!=0))
00140 {
00141 initMot(mot1);
00142 initMot(mot2);
00143 return 0;
00144 }
00145 else
00146 return -1;
00147 }
00148
00149 return -2;
00150
00151 }
00152
00153
00154
00157 int proxIR( int argc, char * argv[], void * data)
00158 {
00159 char Buffer[MAXBUFFERSIZE];
00160
00161 int sensors[11],i,n;
00162
00163
00164 char bar[9][20];
00165
00166 while (!kbhit())
00167 {
00168 if(kh3_proximity_ir((char *)Buffer, dsPic))
00169 {
00170 for (i=0;i<9;i++)
00171 {
00172 sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
00173
00174 n=(int)(sensors[i]/4096.0*15);
00175
00176 if (n==0)
00177 sprintf(bar[i],"|\33[%dC>|",14);
00178 else
00179 if (n>=14)
00180 sprintf(bar[i],"|>\33[%dC|",14);
00181 else
00182 sprintf(bar[i],"|\33[%dC>\33[%dC|",14-n,n);
00183
00184 }
00185
00186
00187 printf("\nPush RETURN to stop!\n near far\nback left : %4.4u %s\nleft 90 : %4.4u %s\nleft 45 : %4.4u %s\nfront left : %4.4u %s\nfront right : %4.4u %s\nright 45 : %4.4u %s\nright 90 : %4.4u %s\nback right : %4.4u %s\nback : %4.4u %s\ntime stamp : %lu\r\n",
00188 sensors[0],bar[0], sensors[1],bar[1],
00189 sensors[2],bar[2], sensors[3],bar[3],
00190 sensors[4],bar[4], sensors[5],bar[5],
00191 sensors[6],bar[6], sensors[7],bar[7],
00192 sensors[8], bar[8],
00193 ((Buffer[19] | Buffer[20]<<8) | (Buffer[21] | Buffer[22]<<8)<<16));
00194 }
00195 else
00196 printf("\r\nn, error...\r\n");
00197
00198 usleep(200000);
00199 clear_screen();
00200 }
00201 }
00202
00203
00206 int ambIR( int argc, char * argv[], void * data)
00207 {
00208 char Buffer[MAXBUFFERSIZE];
00209
00210 int sensors[11],i,n;
00211
00212
00213 char bar[9][20];
00214
00215 while (!kbhit())
00216 {
00217 if(kh3_ambiant_ir((char *)Buffer, dsPic))
00218 {
00219 for (i=0;i<9;i++)
00220 {
00221 sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
00222
00223 n=(int)(sensors[i]/4096.0*15);
00224
00225 if (n==0)
00226 sprintf(bar[i],"|\33[%dC>|",14);
00227 else
00228 if (n>=14)
00229 sprintf(bar[i],"|>\33[%dC|",14);
00230 else
00231 sprintf(bar[i],"|\33[%dC>\33[%dC|",14-n,n);
00232
00233 }
00234
00235
00236 printf("\nPush RETURN to stop!\n dark bright\nback left : %4.4u %s\nleft 90 : %4.4u %s\nleft 45 : %4.4u %s\nfront left : %4.4u %s\nfront right : %4.4u %s\nright 45 : %4.4u %s\nright 90 : %4.4u %s\nback right : %4.4u %s\nback : %4.4u %s\ntime stamp : %lu\r\n",
00237 sensors[0],bar[0], sensors[1],bar[1],
00238 sensors[2],bar[2], sensors[3],bar[3],
00239 sensors[4],bar[4], sensors[5],bar[5],
00240 sensors[6],bar[6], sensors[7],bar[7],
00241 sensors[8], bar[8],
00242 ((Buffer[19] | Buffer[20]<<8) | (Buffer[21] | Buffer[22]<<8)<<16));
00243 }
00244 else
00245 printf("\r\no, error...\r\n");
00246
00247 usleep(200000);
00248 clear_screen();
00249
00250 }
00251
00252
00253 }
00254
00255
00258 int voltageBAT( int argc, char * argv[], void * data)
00259 {
00260 char Buffer[MAXBUFFERSIZE];
00261 short argument;
00262
00263 argument = atoi(argv[1]);
00264 if(kh3_battery_voltage((char *)Buffer, argument, dsPic)){
00265 switch(argument){
00266 case 0:
00267 printf("\r\n%c,%3.3u,%3.3u => battery voltage = %u.%uV\r\n",
00268 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00269 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00270 break;
00271 case 1:
00272 printf("\r\n%c,",Buffer[0]);
00273 if ((Buffer[2] & 0x80) != 0){
00274 Buffer[2] = Buffer[2] & 0x7F;
00275 printf("-%3.3u,%3.3u => battery current = -%u.%uA\r\n",
00276 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00277 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00278 }
00279 else{
00280 printf("%3.3u,%3.3u => battery current = %u.%uA\r\n",
00281 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00282 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00283 }
00284 break;
00285 case 2:
00286 printf("\r\n,%c,",Buffer[0]);
00287 if ((Buffer[2] & 0x80) != 0){
00288 Buffer[2] = Buffer[2] & 0x7F;
00289 printf("-%3.3u,%3.3u => battery average current = -%u.%uA\r\n",
00290 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00291 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00292 }
00293 else{
00294 printf("%3.3u,%3.3u => battery average current = %u.%uA\r\n",
00295 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00296 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00297 }
00298 break;
00299 case 3:
00300 printf("\r\n%c,%3.3u => battery remaining capacity = %umAh\r\n",
00301 Buffer[0], (Buffer[1] | Buffer[2]<<8),
00302 (Buffer[1] | Buffer[2]<<8) );
00303 break;
00304 case 4:
00305 printf("\r\n%c,%3.3u,%3.3u => temperature = %u.%uC\r\n",
00306 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00307 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00308 break;
00309 case 5:
00310 printf("\r\n%c,%3.3u, => battery remaining capacity = %u%\r\n",
00311 Buffer[0], (Buffer[1] | Buffer[2]<<8),
00312 (Buffer[1] | Buffer[2]<<8));
00313 break;
00314
00315
00316 default:
00317 printf("argument error\r\n");
00318 }
00319 }
00320 else
00321 printf("\r\nv, error...\r\n");
00322 }
00323
00324
00327 int tstampRST( int argc, char * argv[], void * data)
00328 {
00329 char Buffer[MAXBUFFERSIZE];
00330
00331 if(kh3_reset_tstamp((char *)Buffer, dsPic))
00332 printf("\r\n%c\r\n", Buffer[0]);
00333 else
00334 printf("\r\nz, error...\r\n");
00335 }
00336
00337
00340 int revisionOS( int argc, char * argv[], void * data)
00341 {
00342 char Buffer[MAXBUFFERSIZE];
00343 unsigned int ver ;
00344
00345
00346
00347
00348 if(kh3_revision((char *)Buffer, dsPic)){
00349 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
00350 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00351 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00352 kmot_GetFWVersion( mot1 , &ver );
00353 printf("Right Motor Firmware v%u.%u\n" ,
00354 KMOT_VERSION(ver) , KMOT_REVISION(ver) );
00355
00356 kmot_GetFWVersion( mot2 , &ver );
00357 printf("Left Motor Firmware v%u.%u\n" ,
00358 KMOT_VERSION(ver) , KMOT_REVISION(ver) );
00359
00360
00361 }
00362 else
00363 printf("\r\nb, error...\r\n");
00364 }
00365
00366
00376 int configureOS( int argc, char * argv[], void * data)
00377 {
00378 char Buffer[MAXBUFFERSIZE];
00379 short index;
00380 short value;
00381
00382
00383 index = atoi(argv[1]);
00384 value = atoi(argv[2]);
00385
00386
00387 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00388 printf("\r\n%c\r\n", Buffer[0]);
00389 else
00390 printf("\r\nc, error...\r\n");
00391 }
00392
00393
00401 int measureUS( int argc, char * argv[], void * data)
00402 {
00403 char Buffer[MAXBUFFERSIZE];
00404 int i;
00405 short usnoise;
00406 short echonbr;
00407 short usvalue;
00408 short usampl;
00409 short argument;
00410
00411 argument = atoi(argv[1]);
00412 if(kh3_measure_us((char *)Buffer, argument, dsPic))
00413 {
00414
00415 printf("\r\n%c", Buffer[0]);
00416 for(i = 0; i < 22; i++)
00417 printf(",%4.4u", (Buffer[1+i*2] | Buffer[2+i*2]<<8));
00418 printf("\r\n");
00419
00420
00421 echonbr = (Buffer[1] | Buffer[2]<<8);
00422 printf("echonbr = %d\r\n", echonbr);
00423
00424
00425 if(echonbr < 10)
00426 {
00427 for(i = 0; i < echonbr ; i++)
00428 {
00429
00430 usvalue = (Buffer[i*8+3] | Buffer[i*8+4]<<8);
00431 usampl = (Buffer[i*8+5] | Buffer[i*8+6]<<8);
00432
00433
00434 printf("Echo %d : Amplitude = %d, Distance = %dcm.\r\n", i+1, usampl, usvalue);
00435 }
00436 }
00437 else
00438 printf("read error\r\n");
00439 }
00440 else
00441 printf("\r\ng, error...");
00442 }
00443
00444
00445
00450 int getallus( int argc, char * argv[], void * data)
00451 {
00452 char Buffer[MAXBUFFERSIZE];
00453 int i;
00454 short usnoise;
00455 short echonbr;
00456 short usvalue;
00457 short usampl;
00458 short argument;
00459
00460 char usvalues[5];
00461
00462
00463 short index=0,value=31;
00464
00465
00466 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00467 printf("\r\n%c\r\n", Buffer[0]);
00468 else
00469 printf("\r\nconfigure OS error!\r\n");
00470
00471 while (!kbhit())
00472 {
00473
00474 for (i=0;i<5;i++)
00475 {
00476
00477 if(kh3_measure_us((char *)Buffer, i+1, dsPic))
00478 {
00479 usvalues[i] = (Buffer[0*8+3] | Buffer[0*8+4]<<8);
00480 }
00481 else
00482 printf("\r\ng, error...");
00483 }
00484
00485 printf("Push RETURN to stop!\n\nUS sensor : distance [cm]\nleft 90 : %3d\nleft 45 : %3d\nfront : %3d\nright 45 : %3d\nright 90 : %3d\n",usvalues[0],usvalues[1],usvalues[2],usvalues[3],usvalues[4]);
00486
00487 usleep(200000);
00488 clear_screen();
00489 }
00490
00491 }
00492
00493
00502 int motSpeed( int argc, char *argv[], void *data)
00503 {
00504 if(mot1!=0 && mot2!=0)
00505 {
00506 kmot_SetPoint( mot1 , kMotRegSpeed , atoi(argv[1]));
00507 kmot_SetPoint( mot2 , kMotRegSpeed , atoi(argv[2]));
00508 return 0;
00509 }
00510 else
00511 return -1;
00512 }
00513
00514
00523 int motMove( int argc, char *argv[], void *data)
00524 {
00525 if(mot1!=0 && mot2!=0)
00526 {
00527 kmot_SetPoint( mot1 , kMotRegPos, atoi(argv[1]));
00528 kmot_SetPoint( mot2 , kMotRegPos, atoi(argv[2]));
00529 return 0;
00530 }
00531 else
00532 return -1;
00533 }
00534
00535
00539 int motStop( int argc, char *argv[], void *data)
00540 {
00541 if(mot1!=0 && mot2!=0)
00542 {
00543 kmot_SetMode( mot1 , kMotModeStopMotor );
00544 kmot_SetMode( mot2 , kMotModeStopMotor );
00545 return 0;
00546 }
00547 else
00548 return -1;
00549 }
00550
00551
00552
00553
00559 #define BR_IRGAIN 30
00560 #define fwSpeed 20
00561 #define MIN_SPEED 10
00562 #define RotSpeedL 50
00563 #define RotSpeedR -50
00564
00565 int braitenbergAvoidance( int argc , char * argv[] , void * data)
00566 {
00567 int Connections_B[9] = { 2, -2, -4, -12, 5, 2, 2, 2, 4};
00568 int Connections_A[9] = { 2, 2, 2, 5, -12, -4, -2, 2, 4};
00569 int i, buflen, sensval;
00570 char buffer[MAXBUFFERSIZE];
00571 char * scan;
00572 long int lspeed16, rspeed16;
00573 int tabsens[9];
00574 int left_speed, right_speed;
00575 unsigned int immobility = 0;
00576 unsigned int prevpos_left, pos_left, prevpos_right, pos_right;
00577 u_int8_t valueLL,valueLH,valueHL,valueHH;
00578
00579
00580 char cmd[3] = { 2, 'N', 0};
00581
00582
00583 prevpos_left = kmot_GetMeasure( mot1 , kMotRegPos );
00584 prevpos_right = kmot_GetMeasure( mot2 , kMotRegPos );
00585
00586
00587 printf("\nPush RETURN to stop!\n");
00588
00589 while(!kbhit())
00590 {
00591 lspeed16 = 0; rspeed16 = 0;
00592
00593 #if 0
00594 kh3_sendcommand( dsPic, cmd );
00595 while(!kb_gpio_get(KNET_INT0));
00596
00597 buflen = knet_llread( dsPic, buffer, 30);
00598 #endif
00599 kh3_proximity_ir((char *)buffer, dsPic);
00600 scan = buffer+1;
00601
00602
00603 for (i = 0; i < 9; i++)
00604 {
00605 sensval = *(scan) | (*(scan+1))<<8;
00606 if(sensval > 1000)
00607 tabsens[i] = 450;
00608 else if (sensval < 100)
00609 tabsens[i] = 0;
00610 else
00611 tabsens[i] = (sensval - 100) >> 1;
00612 scan = scan + 2;
00613 }
00614
00615 for (i = 0; i < 9; i++)
00616 {
00617 lspeed16 += Connections_A[i] * tabsens[i];
00618 rspeed16 += Connections_B[i] * tabsens[i];
00619 }
00620
00621 left_speed = ((lspeed16 / BR_IRGAIN) + fwSpeed);
00622 right_speed = ((rspeed16 / BR_IRGAIN) + fwSpeed);
00623
00624 if(left_speed > 0 && left_speed < MIN_SPEED)
00625 left_speed = MIN_SPEED;
00626 if(left_speed < 0 && left_speed > -MIN_SPEED)
00627 left_speed = -MIN_SPEED;
00628 if(right_speed > 0 && right_speed < MIN_SPEED)
00629 right_speed = MIN_SPEED;
00630 if(right_speed < 0 && right_speed > -MIN_SPEED)
00631 right_speed = -MIN_SPEED;
00632
00633
00634 #ifndef SHIFT_SPEED
00635 left_speed *= 256;
00636 right_speed *= 256;
00637 #endif
00638
00639
00640 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
00641 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
00642
00643
00644
00645 left_speed = kmot_GetMeasure( mot1 , kMotMesSpeed );
00646 right_speed = kmot_GetMeasure( mot2 , kMotMesSpeed );
00647
00648
00649 pos_left = kmot_GetMeasure( mot1 , kMotRegPos );
00650 pos_right = kmot_GetMeasure( mot2 , kMotRegPos );
00651
00652
00653 if((pos_left < (prevpos_left + 700)) && (pos_left > (prevpos_left -700)) && (pos_right < (prevpos_right + 700)) && (pos_right > (prevpos_right -700)))
00654 {
00655 if(++immobility > 5)
00656 {
00657 left_speed = RotSpeedL;
00658 right_speed = RotSpeedR;
00659 #ifndef SHIFT_SPEED
00660 left_speed *= 256;
00661 right_speed *= 256;
00662 #endif
00663 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
00664 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
00665
00666 do{
00667 usleep(10);
00668 kh3_proximity_ir((char *)buffer, dsPic);
00669 }while (((buffer[7] | (buffer[8]<<8) ) >250) || ((buffer[9] | (buffer[10]<<8)) >250));
00670
00671 immobility = 0;
00672 prevpos_left = pos_left;
00673 prevpos_right = pos_right;
00674 }
00675 }
00676 else
00677 {
00678 immobility = 0;
00679 prevpos_left = pos_left;
00680 prevpos_right = pos_right;
00681 }
00682
00683
00684
00685
00686 usleep(20000);
00687 }
00688
00689 kmot_SetMode( mot1 , kMotModeStopMotor );
00690 kmot_SetMode( mot2 , kMotModeStopMotor );
00691
00692 }
00693
00694
00695
00698 int quit( int argc , char * argv[] , void * data)
00699 {
00700 quitReq = 1;
00701 }
00702
00703
00704 int help( int argc , char * argv[] , void * data);
00705
00709 static kb_command_t cmds[] = {
00710 { "quit" , 0 , 0 , quit } ,
00711 { "exit" , 0 , 0 , quit } ,
00712 { "bye" , 0 , 0 , quit } ,
00713 { "setcfg" , 2 , 2 , configureOS },
00714 { "getrev" , 0 , 0 , revisionOS },
00715 { "getbat" , 1 , 1 , voltageBAT },
00716 { "rststamp" , 0 , 0 , tstampRST },
00717 { "getambir" , 0 , 0 , ambIR },
00718 { "getproxir" , 0 , 0 , proxIR },
00719 { "getus" , 1 , 1 , measureUS },
00720 { "getallus" , 0 , 0 , getallus },
00721 { "setmotspeed" , 2 , 2 , motSpeed },
00722 { "setmotmove" , 2 , 2 , motMove },
00723 { "motstop" , 0 , 0 , motStop },
00724 { "braiten" , 0 , 0 , braitenbergAvoidance},
00725 { "help" , 0 , 0 , help } ,
00726 { NULL , 0 , 0 , NULL }
00727 };
00728
00729
00732 int help( int argc , char * argv[] , void * data)
00733 {
00734 kb_command_t * scan = cmds;
00735 while(scan->name != NULL)
00736 {
00737 printf("%s\r\n",scan->name);
00738 scan++;
00739 }
00740 return 0;
00741 }
00742
00743 int main( int arc, char *argv[])
00744 {
00745 char Buffer[MAXBUFFERSIZE];
00746 char i;
00747
00748
00749 printf("Khepera3 test program (C) K-Team S.A\r\n");
00750
00751 if(!initKH3())
00752 {
00753 printf("Init oke...\r\n");
00754
00755 kh3_revision((char *)Buffer, dsPic);
00756 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
00757 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00758 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00759
00760
00761
00762
00763 while (!quitReq)
00764 {
00765
00766
00767 printf("\n> ");
00768
00769 if ( fgets( buf , sizeof(buf) , stdin ) != NULL )
00770 {
00771 buf[strlen(buf)-1] = '\0';
00772 kb_parse_command( buf , cmds , NULL);
00773 }
00774 }
00775
00776 printf("Exiting...\r\n");
00777 }
00778 else
00779 printf("Fatal error, unable to initialize\r\n");
00780 }
00781
00782