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
00039 int initMot(knet_dev_t *hDev)
00040 {
00041 if(hDev)
00042 {
00043 kmot_SetMode( hDev , kMotModeIdle );
00044 kmot_SetSampleTime( hDev , 1550 );
00045 kmot_SetMargin( hDev , 6 );
00046 if(hDev == mot1)
00047 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk | kMotSWOptDirectionInv);
00048 else
00049 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00050 kmot_ResetError( hDev );
00051 kmot_SetBlockedTime( hDev , 10 );
00052 kmot_SetLimits( hDev , kMotRegCurrent , 0 , 500 );
00053 kmot_SetLimits( hDev , kMotRegPos , -10000 , 10000 );
00054
00055
00056 kmot_ConfigurePID( hDev , kMotRegSpeed , 400 , 0 , 10 );
00057 kmot_ConfigurePID( hDev,kMotRegPos,620,3,10);
00058 kmot_SetSpeedProfile(hDev,30,10);
00059
00060 return 1;
00061 }
00062 else
00063 {
00064 printf("initMot error, handle cannot be null\r\n");
00065 return 0;
00066 }
00067 }
00068
00069
00070
00081 int initKH3( void )
00082 {
00083
00084 kh3_init();
00085
00086
00087 dsPic = knet_open( "Khepera3:dsPic" , KNET_BUS_I2C , 0 , NULL );
00088 mot1 = knet_open( "Khepera3:mot1" , KNET_BUS_I2C , 0 , NULL );
00089 mot2 = knet_open( "Khepera3:mot2" , KNET_BUS_I2C , 0 , NULL );
00090
00091 if(dsPic!=0)
00092 {
00093 if((mot1!=0)&&(mot2!=0))
00094 {
00095 initMot(mot1);
00096 initMot(mot2);
00097 return 0;
00098 }
00099 else
00100 return -1;
00101 }
00102
00103 return -2;
00104
00105 }
00106
00107
00108
00111 int proxIR( int argc, char * argv[], void * data)
00112 {
00113 char Buffer[MAXBUFFERSIZE];
00114 if(kh3_proximity_ir((char *)Buffer, dsPic))
00115 printf("\r\n%c,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%lu\r\n",
00116 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00117 (Buffer[5] | Buffer[6]<<8), (Buffer[7] | Buffer[8]<<8),
00118 (Buffer[9] | Buffer[10]<<8), (Buffer[11] | Buffer[12]<<8),
00119 (Buffer[13] | Buffer[14]<<8), (Buffer[15] | Buffer[16]<<8),
00120 (Buffer[17] | Buffer[18]<<8),
00121 ((Buffer[19] | Buffer[20]<<8) | (Buffer[21] | Buffer[22]<<8)<<16));
00122 else
00123 printf("\r\nn, error...\r\n");
00124 }
00125
00126
00129 int ambIR( int argc, char * argv[], void * data)
00130 {
00131 char Buffer[MAXBUFFERSIZE];
00132 if(kh3_ambiant_ir((char *)Buffer, dsPic))
00133 printf("\r\n%c,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%lu\r\n",
00134 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00135 (Buffer[5] | Buffer[6]<<8), (Buffer[7] | Buffer[8]<<8),
00136 (Buffer[9] | Buffer[10]<<8), (Buffer[11] | Buffer[12]<<8),
00137 (Buffer[13] | Buffer[14]<<8), (Buffer[15] | Buffer[16]<<8),
00138 (Buffer[17] | Buffer[18]<<8),
00139 ((Buffer[19] | Buffer[20]<<8) | (Buffer[21] | Buffer[22]<<8)<<16));
00140 else
00141 printf("\r\no, error...\r\n");
00142 }
00143
00144
00147 int voltageBAT( int argc, char * argv[], void * data)
00148 {
00149 char Buffer[MAXBUFFERSIZE];
00150 short argument;
00151
00152 argument = atoi(argv[1]);
00153 if(kh3_battery_voltage((char *)Buffer, argument, dsPic)){
00154 switch(argument){
00155 case 0:
00156 printf("\r\n%c,%3.3u,%3.3u => battery voltage = %u.%uV\r\n",
00157 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00158 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00159 break;
00160 case 1:
00161 printf("\r\n%c,",Buffer[0]);
00162 if ((Buffer[2] & 0x80) != 0){
00163 Buffer[2] = Buffer[2] & 0x7F;
00164 printf("-%3.3u,%3.3u => battery current = -%u.%uA\r\n",
00165 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00166 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00167 }
00168 else{
00169 printf("%3.3u,%3.3u => battery current = %u.%uA\r\n",
00170 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00171 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00172 }
00173 break;
00174 case 2:
00175 printf("\r\n,%c,",Buffer[0]);
00176 if ((Buffer[2] & 0x80) != 0){
00177 Buffer[2] = Buffer[2] & 0x7F;
00178 printf("-%3.3u,%3.3u => battery average current = -%u.%uA\r\n",
00179 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00180 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00181 }
00182 else{
00183 printf("%3.3u,%3.3u => battery average current = %u.%uA\r\n",
00184 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00185 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00186 }
00187 break;
00188 case 3:
00189 printf("\r\n%c,%3.3u => battery remaining capacity = %umAh\r\n",
00190 Buffer[0], (Buffer[1] | Buffer[2]<<8),
00191 (Buffer[1] | Buffer[2]<<8) );
00192 break;
00193 case 4:
00194 printf("\r\n%c,%3.3u,%3.3u => temperature = %u.%uC\r\n",
00195 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00196 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00197 break;
00198 case 5:
00199 printf("\r\n%c,%3.3u, => battery remaining capacity = %u%\r\n",
00200 Buffer[0], (Buffer[1] | Buffer[2]<<8),
00201 (Buffer[1] | Buffer[2]<<8));
00202 break;
00203
00204
00205 default:
00206 printf("argument error\r\n");
00207 }
00208 }
00209 else
00210 printf("\r\nv, error...\r\n");
00211 }
00212
00213
00216 int tstampRST( int argc, char * argv[], void * data)
00217 {
00218 char Buffer[MAXBUFFERSIZE];
00219
00220 if(kh3_reset_tstamp((char *)Buffer, dsPic))
00221 printf("\r\n%c\r\n", Buffer[0]);
00222 else
00223 printf("\r\nz, error...\r\n");
00224 }
00225
00226
00229 int revisionOS( int argc, char * argv[], void * data)
00230 {
00231 char Buffer[MAXBUFFERSIZE];
00232 unsigned int ver ;
00233
00234
00235
00236
00237 if(kh3_revision((char *)Buffer, dsPic)){
00238 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
00239 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00240 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00241 kmot_GetFWVersion( mot1 , &ver );
00242 printf("Right Motor Firmware v%u.%u\n" ,
00243 KMOT_VERSION(ver) , KMOT_REVISION(ver) );
00244
00245 kmot_GetFWVersion( mot2 , &ver );
00246 printf("Left Motor Firmware v%u.%u\n" ,
00247 KMOT_VERSION(ver) , KMOT_REVISION(ver) );
00248
00249
00250 }
00251 else
00252 printf("\r\nb, error...\r\n");
00253 }
00254
00255
00263 int configureOS( int argc, char * argv[], void * data)
00264 {
00265 char Buffer[MAXBUFFERSIZE];
00266 short index;
00267 short value;
00268
00269
00270 index = atoi(argv[1]);
00271 value = atoi(argv[2]);
00272
00273
00274 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00275 printf("\r\n%c\r\n", Buffer[0]);
00276 else
00277 printf("\r\nc, error...\r\n");
00278 }
00279
00280
00286 int measureUS( int argc, char * argv[], void * data)
00287 {
00288 char Buffer[MAXBUFFERSIZE];
00289 int i;
00290 short usnoise;
00291 short echonbr;
00292 short usvalue;
00293 short usampl;
00294 short argument;
00295
00296 argument = atoi(argv[1]);
00297 if(kh3_measure_us((char *)Buffer, argument, dsPic))
00298 {
00299
00300 printf("\r\n%c", Buffer[0]);
00301 for(i = 0; i < 22; i++)
00302 printf(",%4.4u", (Buffer[1+i*2] | Buffer[2+i*2]<<8));
00303 printf("\r\n");
00304
00305
00306 echonbr = (Buffer[1] | Buffer[2]<<8);
00307 printf("echonbr = %d\r\n", echonbr);
00308
00309
00310 if(echonbr < 10)
00311 {
00312 for(i = 0; i < echonbr ; i++)
00313 {
00314
00315 usvalue = (Buffer[i*8+3] | Buffer[i*8+4]<<8);
00316 usampl = (Buffer[i*8+5] | Buffer[i*8+6]<<8);
00317
00318
00319 printf("Echo %d : Amplitude = %d, Distance = %dcm.\r\n", i+1, usampl, usvalue);
00320 }
00321 }
00322 else
00323 printf("read error\r\n");
00324 }
00325 else
00326 printf("\r\ng, error...");
00327 }
00328
00329
00336 int motSpeed( int argc, char *argv[], void *data)
00337 {
00338 if(mot1!=0 && mot2!=0)
00339 {
00340 kmot_SetPoint( mot1 , kMotRegSpeed , atoi(argv[1]));
00341 kmot_SetPoint( mot2 , kMotRegSpeed , atoi(argv[2]));
00342 return 0;
00343 }
00344 else
00345 return -1;
00346 }
00347
00348
00355 int motMove( int argc, char *argv[], void *data)
00356 {
00357 if(mot1!=0 && mot2!=0)
00358 {
00359 kmot_SetPoint( mot1 , kMotRegPos, atoi(argv[1]));
00360 kmot_SetPoint( mot2 , kMotRegPos, atoi(argv[2]));
00361 return 0;
00362 }
00363 else
00364 return -1;
00365 }
00366
00367
00372 int motStop( int argc, char *argv[], void *data)
00373 {
00374 if(mot1!=0 && mot2!=0)
00375 {
00376 kmot_SetMode( mot1 , kMotModeStopMotor );
00377 kmot_SetMode( mot2 , kMotModeStopMotor );
00378 return 0;
00379 }
00380 else
00381 return -1;
00382 }
00383
00384
00390 #define BR_IRGAIN 30
00391 #define fwSpeed 20
00392 #define MIN_SPEED 10
00393 #define RotSpeedL 50
00394 #define RotSpeedR -50
00395
00396 int braitenbergAvoidance( int argc , char * argv[] , void * data)
00397 {
00398 int Connections_B[9] = { 2, -2, -4, -12, 5, 2, 2, 2, 4};
00399 int Connections_A[9] = { 2, 2, 2, 5, -12, -4, -2, 2, 4};
00400 int i, buflen, sensval;
00401 char buffer[MAXBUFFERSIZE];
00402 char * scan;
00403 long int lspeed16, rspeed16;
00404 int tabsens[9];
00405 int left_speed, right_speed;
00406 unsigned int immobility = 0;
00407 unsigned int prevpos_left, pos_left, prevpos_right, pos_right;
00408 u_int8_t valueLL,valueLH,valueHL,valueHH;
00409
00410
00411 char cmd[3] = { 2, 'N', 0};
00412
00413
00414 prevpos_left = kmot_GetMeasure( mot1 , kMotRegPos );
00415 prevpos_right = kmot_GetMeasure( mot2 , kMotRegPos );
00416
00417 while(1)
00418 {
00419 lspeed16 = 0; rspeed16 = 0;
00420
00421 #if 0
00422 kh3_sendcommand( dsPic, cmd );
00423 while(!kb_gpio_get(KNET_INT0));
00424
00425 buflen = knet_llread( dsPic, buffer, 30);
00426 #endif
00427 kh3_proximity_ir((char *)buffer, dsPic);
00428 scan = buffer+3;
00429
00430
00431 for (i = 0; i < 9; i++)
00432 {
00433 sensval = *(scan) | (*(scan+1))<<8;
00434 if(sensval > 1000)
00435 tabsens[i] = 450;
00436 else if (sensval < 100)
00437 tabsens[i] = 0;
00438 else
00439 tabsens[i] = (sensval - 100) >> 1;
00440 scan = scan + 2;
00441 }
00442
00443 for (i = 0; i < 9; i++)
00444 {
00445 lspeed16 += Connections_A[i] * tabsens[i];
00446 rspeed16 += Connections_B[i] * tabsens[i];
00447 }
00448
00449 left_speed = ((lspeed16 / BR_IRGAIN) + fwSpeed);
00450 right_speed = ((rspeed16 / BR_IRGAIN) + fwSpeed);
00451
00452 if(left_speed > 0 && left_speed < MIN_SPEED)
00453 left_speed = MIN_SPEED;
00454 if(left_speed < 0 && left_speed > -MIN_SPEED)
00455 left_speed = -MIN_SPEED;
00456 if(right_speed > 0 && right_speed < MIN_SPEED)
00457 right_speed = MIN_SPEED;
00458 if(right_speed < 0 && right_speed > -MIN_SPEED)
00459 right_speed = -MIN_SPEED;
00460
00461
00462 #ifndef SHIFT_SPEED
00463 left_speed *= 256;
00464 right_speed *= 256;
00465 #endif
00466
00467
00468 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
00469 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
00470
00471 printf("lens = %d, rsens = %d lspd = %d rspd = %d\r\n", (int)lspeed16, (int)rspeed16, left_speed, right_speed);
00472
00473 left_speed = kmot_GetMeasure( mot1 , kMotMesSpeed );
00474 right_speed = kmot_GetMeasure( mot2 , kMotMesSpeed );
00475
00476
00477 pos_left = kmot_GetMeasure( mot1 , kMotRegPos );
00478 pos_right = kmot_GetMeasure( mot2 , kMotRegPos );
00479
00480
00481 if((pos_left < (prevpos_left + 700)) && (pos_left > (prevpos_left -700)) && (pos_right < (prevpos_right + 700)) && (pos_right > (prevpos_right -700)))
00482 {
00483 if(++immobility > 5)
00484 {
00485 left_speed = RotSpeedL;
00486 right_speed = RotSpeedR;
00487 #ifndef SHIFT_SPEED
00488 left_speed *= 256;
00489 right_speed *= 256;
00490 #endif
00491 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
00492 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
00493
00494 do{
00495 usleep(10);
00496 kh3_proximity_ir((char *)buffer, dsPic);
00497 }while (((buffer[7] | (buffer[8]<<8) ) >250) || ((buffer[9] | (buffer[10]<<8)) >250));
00498
00499 immobility = 0;
00500 prevpos_left = pos_left;
00501 prevpos_right = pos_right;
00502 }
00503 }
00504 else
00505 {
00506 immobility = 0;
00507 prevpos_left = pos_left;
00508 prevpos_right = pos_right;
00509 }
00510
00511
00512 printf("lspd = %d rspd = %d\r\n", left_speed, right_speed);
00513
00514 usleep(20000);
00515 }
00516 }
00517
00518
00519
00522 int quit( int argc , char * argv[] , void * data)
00523 {
00524 quitReq = 1;
00525 }
00526
00527
00528 int help( int argc , char * argv[] , void * data);
00529
00533 static kb_command_t cmds[] = {
00534 { "quit" , 0 , 0 , quit } ,
00535 { "exit" , 0 , 0 , quit } ,
00536 { "bye" , 0 , 0 , quit } ,
00537 { "setcfg" , 2 , 2 , configureOS },
00538 { "getrev" , 0 , 0 , revisionOS },
00539 { "getbat" , 1 , 1 , voltageBAT },
00540 { "rststamp" , 0 , 0 , tstampRST },
00541 { "getambir" , 0 , 0 , ambIR },
00542 { "getproxir" , 0 , 0 , proxIR },
00543 { "getus" , 1 , 1 , measureUS },
00544 { "setmotspeed" , 2 , 2 , motSpeed },
00545 { "setmotmove" , 2 , 2 , motMove },
00546 { "motstop" , 0 , 0 , motStop },
00547 { "braiten" , 0 , 0 , braitenbergAvoidance},
00548 { "help" , 0 , 0 , help } ,
00549 { NULL , 0 , 0 , NULL }
00550 };
00551
00552
00555 int help( int argc , char * argv[] , void * data)
00556 {
00557 kb_command_t * scan = cmds;
00558 while(scan->name != NULL)
00559 {
00560 printf("%s\r\n",scan->name);
00561 scan++;
00562 }
00563 return 0;
00564 }
00565
00566 int main( int arc, char *argv[])
00567 {
00568 char Buffer[MAXBUFFERSIZE];
00569 char i;
00570
00571
00572 printf("Khepera3 test program (C) K-Team S.A\r\n");
00573
00574 if(!initKH3())
00575 {
00576 printf("Init oke...\r\n");
00577
00578 kh3_revision((char *)Buffer, dsPic);
00579 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
00580 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00581 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00582
00583
00584
00585
00586 while (!quitReq)
00587 {
00588
00589
00590 printf("\n> ");
00591
00592 if ( fgets( buf , sizeof(buf) , stdin ) != NULL )
00593 {
00594 buf[strlen(buf)-1] = '\0';
00595 kb_parse_command( buf , cmds , NULL);
00596 }
00597 }
00598
00599 printf("Exiting...\r\n");
00600 }
00601 else
00602 printf("Fatal error, unable to initialize\r\n");
00603
00604 }
00605
00606