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,%4.4u,%4.4u => 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("-%4.4u,%4.4u => 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("%4.4u,%4.4u => 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("-%4.4u,%4.4u => 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("%4.4u,%4.4u => 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,%4.4u,%4.4u => battery accumulared current = %u.%uAh\r\n",
00190 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00191 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00192 break;
00193 case 4:
00194 printf("\r\n%c,%4.4u,%4.4u => temperature = %u.%u°C\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
00199 default:
00200 printf("argument error\r\n");
00201 }
00202 }
00203 else
00204 printf("\r\nv, error...\r\n");
00205 }
00206
00207
00210 int tstampRST( int argc, char * argv[], void * data)
00211 {
00212 char Buffer[MAXBUFFERSIZE];
00213
00214 if(kh3_reset_tstamp((char *)Buffer, dsPic))
00215 printf("\r\n%c\r\n", Buffer[0]);
00216 else
00217 printf("\r\nz, error...\r\n");
00218 }
00219
00220
00223 int revisionOS( int argc, char * argv[], void * data)
00224 {
00225 char Buffer[MAXBUFFERSIZE];
00226
00227 if(kh3_revision((char *)Buffer, dsPic))
00228 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
00229 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00230 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00231 else
00232 printf("\r\nb, error...\r\n");
00233 }
00234
00235
00245 int configureOS( int argc, char * argv[], void * data)
00246 {
00247 char Buffer[MAXBUFFERSIZE];
00248 short index;
00249 short value;
00250
00251
00252 index = atoi(argv[1]);
00253 value = atoi(argv[2]);
00254
00255
00256 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00257 printf("\r\n%c\r\n", Buffer[0]);
00258 else
00259 printf("\r\nc, error...\r\n");
00260 }
00261
00262
00269 int measureUS( int argc, char * argv[], void * data)
00270 {
00271 char Buffer[MAXBUFFERSIZE];
00272 int i;
00273 short usnoise;
00274 short echonbr;
00275 float usconst = 1.715;
00276 short usvalue;
00277 short usampl;
00278 short argument;
00279
00280 argument = atoi(argv[1]);
00281 if(kh3_measure_us((char *)Buffer, argument, dsPic))
00282 {
00283
00284 printf("\r\n%c", Buffer[0]);
00285 for(i = 0; i < 22; i++)
00286 printf(",%4.4u", (Buffer[1+i*2] | Buffer[2+i*2]<<8));
00287 printf("\r\n");
00288
00289
00290 echonbr = (Buffer[1] | Buffer[2]<<8);
00291 printf("echonbr = %d\r\n", echonbr);
00292
00293
00294 if(echonbr < 10)
00295 {
00296 for(i = 0; i < echonbr ; i++)
00297 {
00298
00299 usvalue = (Buffer[i*8+3] | Buffer[i*8+4]<<8) * usconst;
00300 usampl = (Buffer[i*8+5] | Buffer[i*8+6]<<8);
00301
00302
00303 printf("Echo %d : Amplitude = %d, Distance = %dcm.\r\n", i+1, usampl, usvalue);
00304 }
00305 }
00306 else
00307 printf("read error\r\n");
00308 }
00309 else
00310 printf("\r\ng, error...");
00311 }
00312
00313
00322 int motSpeed( int argc, char *argv[], void *data)
00323 {
00324 if(mot1!=0 && mot2!=0)
00325 {
00326 kmot_SetPoint( mot1 , kMotRegSpeed , atoi(argv[1]));
00327 kmot_SetPoint( mot2 , kMotRegSpeed , atoi(argv[2]));
00328 return 0;
00329 }
00330 else
00331 return -1;
00332 }
00333
00334
00342 int motMove( int argc, char *argv[], void *data)
00343 {
00344 if(mot1!=0 && mot2!=0)
00345 {
00346 kmot_SetPoint( mot1 , kMotRegPos, atoi(argv[1]));
00347 kmot_SetPoint( mot2 , kMotRegPos, atoi(argv[2]));
00348 return 0;
00349 }
00350 else
00351 return -1;
00352 }
00353
00354
00358 int motStop( int argc, char *argv[], void *data)
00359 {
00360 if(mot1!=0 && mot2!=0)
00361 {
00362 kmot_SetMode( mot1 , kMotModeStopMotor );
00363 kmot_SetMode( mot2 , kMotModeStopMotor );
00364 return 0;
00365 }
00366 else
00367 return -1;
00368 }
00369
00370
00375 #define BR_IRGAIN 20
00376 #define fwSpeed 60
00377
00378 int braitenbergAvoidance( int argc , char * argv[] , void * data)
00379 {
00380 int Connections_B[9] = { 2, -2, -6, -15, 5, 2, 1, 5, 7};
00381 int Connections_A[9] = { 5, 1, 2, 5, -15, -6, -2, 2, 7};
00382 int i, buflen, sensval;
00383 char buffer[MAXBUFFERSIZE];
00384 char * scan;
00385 long int lspeed16, rspeed16;
00386 int tabsens[9];
00387 int left_speed, right_speed;
00388 u_int8_t valueLL,valueLH,valueHL,valueHH;
00389
00390
00391 char cmd[3] = { 2, 'N', 0};
00392
00393 while(1)
00394 {
00395 lspeed16 = 0; rspeed16 = 0;
00396
00397 kh3_sendcommand( dsPic, cmd );
00398 while(!kb_gpio_get(KNET_INT0));
00399 buflen = knet_llread( dsPic, buffer, 30);
00400 scan = buffer+3;
00401
00402
00403 for (i = 0; i < 9; i++)
00404 {
00405 sensval = *(scan) | (*(scan+1))<<8;
00406 if(sensval > 2000)
00407 tabsens[1] = 500;
00408 else
00409 tabsens[i] = sensval >> 2;
00410 scan = scan + 2;
00411 }
00412
00413 for (i = 0; i < 9; i++)
00414 {
00415 lspeed16 += Connections_A[i] * tabsens[i];
00416 rspeed16 += Connections_B[i] * tabsens[i];
00417 }
00418
00419 left_speed = ((lspeed16 / BR_IRGAIN) + fwSpeed);
00420 right_speed = ((rspeed16 / BR_IRGAIN) + fwSpeed);
00421
00422 if(left_speed > 0 && left_speed < 30)
00423 left_speed = 30;
00424 if(left_speed < 0 && left_speed > -30)
00425 left_speed = -30;
00426 if(right_speed > 0 && right_speed < 30)
00427 right_speed = 30;
00428 if(right_speed < 0 && right_speed > -30)
00429 right_speed = -30;
00430
00431
00432
00433
00434
00435
00436 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
00437
00438
00439
00440
00441
00442
00443 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
00444
00445 printf("lens = %d, rsens = %d lspd = %d rspd = %d\r\n", (int)lspeed16, (int)rspeed16, left_speed, right_speed);
00446
00447 left_speed = kmot_GetMeasure( mot1 , kMotMesSpeed );
00448
00449
00450
00451
00452
00453
00454
00455 right_speed = kmot_GetMeasure( mot2 , kMotMesSpeed );
00456
00457
00458
00459
00460
00461
00462
00463 printf("lspd = %d rspd = %d\r\n", left_speed, right_speed);
00464
00465 usleep(200000);
00466 }
00467 }
00468
00469
00470
00473 int quit( int argc , char * argv[] , void * data)
00474 {
00475 quitReq = 1;
00476 }
00477
00478
00479 int help( int argc , char * argv[] , void * data);
00480
00484 static kb_command_t cmds[] = {
00485 { "quit" , 0 , 0 , quit } ,
00486 { "exit" , 0 , 0 , quit } ,
00487 { "bye" , 0 , 0 , quit } ,
00488 { "setcfg" , 2 , 2 , configureOS },
00489 { "getrev" , 0 , 0 , revisionOS },
00490 { "getbat" , 1 , 1 , voltageBAT },
00491 { "rststamp" , 0 , 0 , tstampRST },
00492 { "getambir" , 0 , 0 , ambIR },
00493 { "getproxir" , 0 , 0 , proxIR },
00494 { "getus" , 1 , 1 , measureUS },
00495 { "setmotspeed" , 2 , 2 , motSpeed },
00496 { "setmotmove" , 2 , 2 , motMove },
00497 { "motstop" , 0 , 0 , motStop },
00498 { "braiten" , 0 , 0 , braitenbergAvoidance},
00499 { "help" , 0 , 0 , help } ,
00500 { NULL , 0 , 0 , NULL }
00501 };
00502
00503
00506 int help( int argc , char * argv[] , void * data)
00507 {
00508 kb_command_t * scan = cmds;
00509 while(scan->name != NULL)
00510 {
00511 printf("%s\r\n",scan->name);
00512 scan++;
00513 }
00514 return 0;
00515 }
00516
00517 #define MAXBUFFERSIZE 256
00518
00519
00520 int main( int arc, char *argv[])
00521 {
00522 char Buffer[MAXBUFFERSIZE];
00523 char i;
00524
00525
00526 printf("Khepera3 test program (C) K-Team S.A\r\n");
00527
00528 if(!initKH3())
00529 {
00530 printf("Init oke...\r\n");
00531
00532 while (!quitReq)
00533 {
00534
00535 memset(Buffer,0,sizeof(Buffer));
00536 kh3_proximity_ir((char *)Buffer, dsPic);
00537 printf("\r\n%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u,%4.4u\r\n",
00538 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),(Buffer[5] |
00539 Buffer[6]<<8), (Buffer[7] | Buffer[8]<<8),
00540 (Buffer[9] | Buffer[10]<<8), (Buffer[11] |
00541 Buffer[12]<<8),(Buffer[13] | Buffer[14]<<8), (Buffer[15] | Buffer[16]<<8)
00542 ,(Buffer[17] | Buffer[18]<<8),(Buffer[19] |
00543 Buffer[20]<<8),(Buffer[21] | Buffer[22]<<8));
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575 }
00576
00577 printf("Exiting...\r\n");
00578 }
00579 else
00580 printf("Fatal error, unable to initialize\r\n");
00581
00582 }
00583
00584