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
00040 int initMot(knet_dev_t *hDev)
00041 {
00042 if(hDev)
00043 {
00044 kmot_SetMode( hDev , kMotModeIdle );
00045 kmot_SetSampleTime( hDev , 1550 );
00046 kmot_SetMargin( hDev , 6 );
00047 if(hDev == mot1)
00048 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk | kMotSWOptDirectionInv);
00049 else
00050 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00051 kmot_ResetError( hDev );
00052 kmot_SetBlockedTime( hDev , 10 );
00053 kmot_SetLimits( hDev , kMotRegCurrent , 0 , 500 );
00054 kmot_SetLimits( hDev , kMotRegPos , -10000 , 10000 );
00055
00056
00057 kmot_ConfigurePID( hDev , kMotRegSpeed , 620, 3 , 10 );
00058 kmot_ConfigurePID( hDev,kMotRegPos,600,20,30);
00059 kmot_SetSpeedProfile(hDev,15000,30);
00060
00061 return 1;
00062 }
00063 else
00064 {
00065 printf("initMot error, handle cannot be null\r\n");
00066 return 0;
00067 }
00068 }
00069
00070
00071
00082 int initKH3( void )
00083 {
00084
00085 kh3_init();
00086
00087
00088 dsPic = knet_open( "Khepera3:dsPic" , KNET_BUS_I2C , 0 , NULL );
00089 mot1 = knet_open( "Khepera3:mot1" , KNET_BUS_I2C , 0 , NULL );
00090 mot2 = knet_open( "Khepera3:mot2" , KNET_BUS_I2C , 0 , NULL );
00091
00092 if(dsPic!=0)
00093 {
00094 if((mot1!=0)&&(mot2!=0))
00095 {
00096 initMot(mot1);
00097 initMot(mot2);
00098 return 0;
00099 }
00100 else
00101 return -1;
00102 }
00103
00104 return -2;
00105
00106 }
00107
00108
00109
00112 int proxIR( int argc, char * argv[], void * data)
00113 {
00114 char Buffer[MAXBUFFERSIZE];
00115
00116 int sensors[11],i,n;
00117
00118
00119 char bar[9][20];
00120
00121 while (!kb_kbhit())
00122 {
00123 if(kh3_proximity_ir((char *)Buffer, dsPic))
00124 {
00125 for (i=0;i<9;i++)
00126 {
00127 sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
00128
00129 n=(int)(sensors[i]/4096.0*15);
00130
00131 if (n==0)
00132 sprintf(bar[i],"|\33[%dC>|",14);
00133 else
00134 if (n>=14)
00135 sprintf(bar[i],"|>\33[%dC|",14);
00136 else
00137 sprintf(bar[i],"|\33[%dC>\33[%dC|",14-n,n);
00138
00139 }
00140
00141
00142 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",
00143 sensors[0],bar[0], sensors[1],bar[1],
00144 sensors[2],bar[2], sensors[3],bar[3],
00145 sensors[4],bar[4], sensors[5],bar[5],
00146 sensors[6],bar[6], sensors[7],bar[7],
00147 sensors[8], bar[8],
00148 ((Buffer[19] | Buffer[20]<<8) | (Buffer[21] | Buffer[22]<<8)<<16));
00149 }
00150 else
00151 printf("\r\nn, error...\r\n");
00152
00153 usleep(200000);
00154 kb_clrscr();
00155 }
00156 }
00157
00158
00161 int ambIR( int argc, char * argv[], void * data)
00162 {
00163 char Buffer[MAXBUFFERSIZE];
00164
00165 int sensors[11],i,n;
00166
00167
00168 char bar[9][20];
00169
00170 while (!kb_kbhit())
00171 {
00172 if(kh3_ambiant_ir((char *)Buffer, dsPic))
00173 {
00174 for (i=0;i<9;i++)
00175 {
00176 sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
00177
00178 n=(int)(sensors[i]/4096.0*15);
00179
00180 if (n==0)
00181 sprintf(bar[i],"|\33[%dC>|",14);
00182 else
00183 if (n>=14)
00184 sprintf(bar[i],"|>\33[%dC|",14);
00185 else
00186 sprintf(bar[i],"|\33[%dC>\33[%dC|",14-n,n);
00187
00188 }
00189
00190
00191 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",
00192 sensors[0],bar[0], sensors[1],bar[1],
00193 sensors[2],bar[2], sensors[3],bar[3],
00194 sensors[4],bar[4], sensors[5],bar[5],
00195 sensors[6],bar[6], sensors[7],bar[7],
00196 sensors[8], bar[8],
00197 ((Buffer[19] | Buffer[20]<<8) | (Buffer[21] | Buffer[22]<<8)<<16));
00198 }
00199 else
00200 printf("\r\no, error...\r\n");
00201
00202 usleep(200000);
00203 kb_clrscr();
00204
00205 }
00206
00207
00208 }
00209
00210
00213 int voltageBAT( int argc, char * argv[], void * data)
00214 {
00215 char Buffer[MAXBUFFERSIZE];
00216 short argument;
00217
00218 argument = atoi(argv[1]);
00219 if(kh3_battery_voltage((char *)Buffer, argument, dsPic)){
00220 switch(argument){
00221 case 0:
00222 printf("\r\n%c,%3.3u,%3.3u => battery voltage = %u.%uV\r\n",
00223 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00224 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00225 break;
00226 case 1:
00227 printf("\r\n%c,",Buffer[0]);
00228 if ((Buffer[2] & 0x80) != 0){
00229 Buffer[2] = Buffer[2] & 0x7F;
00230 printf("-%3.3u,%3.3u => battery current = -%u.%uA\r\n",
00231 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00232 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00233 }
00234 else{
00235 printf("%3.3u,%3.3u => battery current = %u.%uA\r\n",
00236 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00237 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00238 }
00239 break;
00240 case 2:
00241 printf("\r\n,%c,",Buffer[0]);
00242 if ((Buffer[2] & 0x80) != 0){
00243 Buffer[2] = Buffer[2] & 0x7F;
00244 printf("-%3.3u,%3.3u => battery average current = -%u.%uA\r\n",
00245 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00246 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00247 }
00248 else{
00249 printf("%3.3u,%3.3u => battery average current = %u.%uA\r\n",
00250 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00251 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00252 }
00253 break;
00254 case 3:
00255 printf("\r\n%c,%3.3u => battery remaining capacity = %umAh\r\n",
00256 Buffer[0], (Buffer[1] | Buffer[2]<<8),
00257 (Buffer[1] | Buffer[2]<<8) );
00258 break;
00259 case 4:
00260 printf("\r\n%c,%3.3u,%3.3u => temperature = %u.%uC\r\n",
00261 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00262 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00263 break;
00264 case 5:
00265 printf("\r\n%c,%3u, => battery remaining capacity = %u%%\r\n",
00266 Buffer[0], (Buffer[1] | Buffer[2]<<8),
00267 (Buffer[1] | Buffer[2]<<8));
00268 break;
00269
00270
00271 default:
00272 printf("argument error\r\n");
00273 }
00274 }
00275 else
00276 printf("\r\nv, error...\r\n");
00277 }
00278
00279
00282 int tstampRST( int argc, char * argv[], void * data)
00283 {
00284 char Buffer[MAXBUFFERSIZE];
00285
00286 if(kh3_reset_tstamp((char *)Buffer, dsPic))
00287 printf("\r\n%c\r\n", Buffer[0]);
00288 else
00289 printf("\r\nz, error...\r\n");
00290 }
00291
00292
00295 int revisionOS( int argc, char * argv[], void * data)
00296 {
00297 char Buffer[MAXBUFFERSIZE];
00298 unsigned int ver ;
00299
00300
00301
00302
00303 if(kh3_revision((char *)Buffer, dsPic)){
00304 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
00305 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00306 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00307 kmot_GetFWVersion( mot1 , &ver );
00308 printf("Right Motor Firmware v%u.%u\n" ,
00309 KMOT_VERSION(ver) , KMOT_REVISION(ver) );
00310
00311 kmot_GetFWVersion( mot2 , &ver );
00312 printf("Left Motor Firmware v%u.%u\n" ,
00313 KMOT_VERSION(ver) , KMOT_REVISION(ver) );
00314
00315
00316 }
00317 else
00318 printf("\r\nb, error...\r\n");
00319 }
00320
00321
00331 int configureOS( int argc, char * argv[], void * data)
00332 {
00333 char Buffer[MAXBUFFERSIZE];
00334 short index;
00335 short value;
00336
00337
00338 index = atoi(argv[1]);
00339 value = atoi(argv[2]);
00340
00341
00342 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00343 printf("\r\n%c\r\n", Buffer[0]);
00344 else
00345 printf("\r\nc, error...\r\n");
00346 }
00347
00348
00356 int measureUS( int argc, char * argv[], void * data)
00357 {
00358 char Buffer[MAXBUFFERSIZE];
00359 int i;
00360 short usnoise;
00361 short echonbr;
00362 short usvalue;
00363 short usampl;
00364 short argument;
00365
00366 argument = atoi(argv[1]);
00367 if(kh3_measure_us((char *)Buffer, argument, dsPic))
00368 {
00369
00370 printf("\r\n%c", Buffer[0]);
00371 for(i = 0; i < 22; i++)
00372 printf(",%4.4u", (Buffer[1+i*2] | Buffer[2+i*2]<<8));
00373 printf("\r\n");
00374
00375
00376 echonbr = (Buffer[1] | Buffer[2]<<8);
00377 printf("echonbr = %d\r\n", echonbr);
00378
00379
00380 if(echonbr < 10)
00381 {
00382 for(i = 0; i < echonbr ; i++)
00383 {
00384
00385 usvalue = (Buffer[i*8+3] | Buffer[i*8+4]<<8);
00386 usampl = (Buffer[i*8+5] | Buffer[i*8+6]<<8);
00387
00388
00389 printf("Echo %d : Amplitude = %d, Distance = %dcm.\r\n", i+1, usampl, usvalue);
00390 }
00391 }
00392 else
00393 printf("read error\r\n");
00394 }
00395 else
00396 printf("\r\ng, error...");
00397 }
00398
00399
00400
00405 int getallus( int argc, char * argv[], void * data)
00406 {
00407 char Buffer[MAXBUFFERSIZE];
00408 int i;
00409 short usnoise;
00410 short echonbr;
00411 short usvalue;
00412 short usampl;
00413 short argument;
00414
00415 char usvalues[5];
00416
00417
00418 short index=0,value=31;
00419
00420
00421 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00422 printf("\r\n%c\r\n", Buffer[0]);
00423 else
00424 printf("\r\nconfigure OS error!\r\n");
00425
00426 while (!kb_kbhit())
00427 {
00428
00429 for (i=0;i<5;i++)
00430 {
00431
00432 if(kh3_measure_us((char *)Buffer, i+1, dsPic))
00433 {
00434 usvalues[i] = (Buffer[0*8+3] | Buffer[0*8+4]<<8);
00435 }
00436 else
00437 printf("\r\ng, error...");
00438 }
00439
00440 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]);
00441
00442 usleep(200000);
00443 kb_clrscr();
00444 }
00445
00446 }
00447
00448
00457 int motSpeed( int argc, char *argv[], void *data)
00458 {
00459 if(mot1!=0 && mot2!=0)
00460 {
00461 kmot_SetPoint( mot1 , kMotRegSpeed , atoi(argv[1]));
00462 kmot_SetPoint( mot2 , kMotRegSpeed , atoi(argv[2]));
00463 return 0;
00464 }
00465 else
00466 return -1;
00467 }
00468
00469
00478 int motMove( int argc, char *argv[], void *data)
00479 {
00480 if(mot1!=0 && mot2!=0)
00481 {
00482 kmot_SetPoint( mot1 , kMotRegPos, atoi(argv[1]));
00483 kmot_SetPoint( mot2 , kMotRegPos, atoi(argv[2]));
00484 return 0;
00485 }
00486 else
00487 return -1;
00488 }
00489
00490
00494 int motStop( int argc, char *argv[], void *data)
00495 {
00496 if(mot1!=0 && mot2!=0)
00497 {
00498 kmot_SetMode( mot1 , kMotModeStopMotor );
00499 kmot_SetMode( mot2 , kMotModeStopMotor );
00500 return 0;
00501 }
00502 else
00503 return -1;
00504 }
00505
00506
00507
00508
00514 #define BR_IRGAIN 30
00515 #define fwSpeed 20
00516 #define MIN_SPEED 10
00517 #define RotSpeedL 50
00518 #define RotSpeedR -50
00519
00520 int braitenbergAvoidance( int argc , char * argv[] , void * data)
00521 {
00522 int Connections_B[9] = { 2, -2, -4, -12, 5, 2, 2, 2, 4};
00523 int Connections_A[9] = { 2, 2, 2, 5, -12, -4, -2, 2, 4};
00524 int i, buflen, sensval;
00525 char buffer[MAXBUFFERSIZE];
00526 char * scan;
00527 long int lspeed16, rspeed16;
00528 int tabsens[9];
00529 int left_speed, right_speed;
00530 unsigned int immobility = 0;
00531 unsigned int prevpos_left, pos_left, prevpos_right, pos_right;
00532 u_int8_t valueLL,valueLH,valueHL,valueHH;
00533
00534
00535 char cmd[3] = { 2, 'N', 0};
00536
00537
00538 prevpos_left = kmot_GetMeasure( mot1 , kMotRegPos );
00539 prevpos_right = kmot_GetMeasure( mot2 , kMotRegPos );
00540
00541
00542 printf("\nPush RETURN to stop!\n");
00543
00544 while(!kb_kbhit())
00545 {
00546 lspeed16 = 0; rspeed16 = 0;
00547
00548 #if 0
00549 kh3_sendcommand( dsPic, cmd );
00550 while(!kb_gpio_get(KNET_INT0));
00551
00552 buflen = knet_llread( dsPic, buffer, 30);
00553 #endif
00554 kh3_proximity_ir((char *)buffer, dsPic);
00555 scan = buffer+3;
00556
00557
00558 for (i = 0; i < 9; i++)
00559 {
00560 sensval = *(scan) | (*(scan+1))<<8;
00561 if(sensval > 1000)
00562 tabsens[i] = 450;
00563 else if (sensval < 100)
00564 tabsens[i] = 0;
00565 else
00566 tabsens[i] = (sensval - 100) >> 1;
00567 scan = scan + 2;
00568 }
00569
00570 for (i = 0; i < 9; i++)
00571 {
00572 lspeed16 += Connections_A[i] * tabsens[i];
00573 rspeed16 += Connections_B[i] * tabsens[i];
00574 }
00575
00576 left_speed = ((lspeed16 / BR_IRGAIN) + fwSpeed);
00577 right_speed = ((rspeed16 / BR_IRGAIN) + fwSpeed);
00578
00579 if(left_speed > 0 && left_speed < MIN_SPEED)
00580 left_speed = MIN_SPEED;
00581 if(left_speed < 0 && left_speed > -MIN_SPEED)
00582 left_speed = -MIN_SPEED;
00583 if(right_speed > 0 && right_speed < MIN_SPEED)
00584 right_speed = MIN_SPEED;
00585 if(right_speed < 0 && right_speed > -MIN_SPEED)
00586 right_speed = -MIN_SPEED;
00587
00588
00589 #ifndef SHIFT_SPEED
00590 left_speed *= 256;
00591 right_speed *= 256;
00592 #endif
00593
00594
00595 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
00596 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
00597
00598
00599
00600 left_speed = kmot_GetMeasure( mot1 , kMotMesSpeed );
00601 right_speed = kmot_GetMeasure( mot2 , kMotMesSpeed );
00602
00603
00604 pos_left = kmot_GetMeasure( mot1 , kMotRegPos );
00605 pos_right = kmot_GetMeasure( mot2 , kMotRegPos );
00606
00607
00608 if((pos_left < (prevpos_left + 700)) && (pos_left > (prevpos_left -700)) && (pos_right < (prevpos_right + 700)) && (pos_right > (prevpos_right -700)))
00609 {
00610 if(++immobility > 5)
00611 {
00612 left_speed = RotSpeedL;
00613 right_speed = RotSpeedR;
00614 #ifndef SHIFT_SPEED
00615 left_speed *= 256;
00616 right_speed *= 256;
00617 #endif
00618 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
00619 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
00620
00621 do{
00622 usleep(10);
00623 kh3_proximity_ir((char *)buffer, dsPic);
00624 }while (((buffer[7] | (buffer[8]<<8) ) >250) || ((buffer[9] | (buffer[10]<<8)) >250));
00625
00626 immobility = 0;
00627 prevpos_left = pos_left;
00628 prevpos_right = pos_right;
00629 }
00630 }
00631 else
00632 {
00633 immobility = 0;
00634 prevpos_left = pos_left;
00635 prevpos_right = pos_right;
00636 }
00637
00638
00639
00640
00641 usleep(20000);
00642 }
00643
00644 kmot_SetMode( mot1 , kMotModeStopMotor );
00645 kmot_SetMode( mot2 , kMotModeStopMotor );
00646
00647 }
00648
00649
00650
00653 int quit( int argc , char * argv[] , void * data)
00654 {
00655 quitReq = 1;
00656 }
00657
00658
00659 int help( int argc , char * argv[] , void * data);
00660
00664 static kb_command_t cmds[] = {
00665 { "quit" , 0 , 0 , quit } ,
00666 { "exit" , 0 , 0 , quit } ,
00667 { "bye" , 0 , 0 , quit } ,
00668 { "setcfg" , 2 , 2 , configureOS },
00669 { "getrev" , 0 , 0 , revisionOS },
00670 { "getbat" , 1 , 1 , voltageBAT },
00671 { "rststamp" , 0 , 0 , tstampRST },
00672 { "getambir" , 0 , 0 , ambIR },
00673 { "getproxir" , 0 , 0 , proxIR },
00674 { "getus" , 1 , 1 , measureUS },
00675 { "getallus" , 0 , 0 , getallus },
00676 { "setmotspeed" , 2 , 2 , motSpeed },
00677 { "setmotmove" , 2 , 2 , motMove },
00678 { "motstop" , 0 , 0 , motStop },
00679 { "braiten" , 0 , 0 , braitenbergAvoidance},
00680 { "help" , 0 , 0 , help } ,
00681 { NULL , 0 , 0 , NULL }
00682 };
00683
00684
00687 int help( int argc , char * argv[] , void * data)
00688 {
00689 kb_command_t * scan = cmds;
00690 while(scan->name != NULL)
00691 {
00692 printf("%s\r\n",scan->name);
00693 scan++;
00694 }
00695 return 0;
00696 }
00697
00698
00701 int main( int arc, char *argv[])
00702 {
00703 char Buffer[MAXBUFFERSIZE];
00704 char i;
00705
00706
00707 printf("Khepera3 test program (C) K-Team S.A\r\n");
00708
00709 if(!initKH3())
00710 {
00711 printf("Init OK...\r\n");
00712
00713 kh3_revision((char *)Buffer, dsPic);
00714 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
00715 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00716 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00717
00718
00719
00720
00721 while (!quitReq)
00722 {
00723
00724
00725 printf("\n> ");
00726
00727 if ( fgets( buf , sizeof(buf) , stdin ) != NULL )
00728 {
00729 buf[strlen(buf)-1] = '\0';
00730 kb_parse_command( buf , cmds , NULL);
00731 }
00732 }
00733
00734 printf("Exiting...\r\n");
00735
00736
00737 return 0;
00738 }
00739 else
00740 printf("Fatal error, unable to initialize\r\n");
00741
00742 return -1;
00743 }
00744
00745