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
00243 int configureOS( int argc, char * argv[], void * data)
00244 {
00245 char Buffer[MAXBUFFERSIZE];
00246 short index;
00247 short value;
00248
00249
00250 index = atoi(argv[1]);
00251 value = atoi(argv[2]);
00252
00253
00254 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00255 printf("\r\n%c\r\n", Buffer[0]);
00256 else
00257 printf("\r\nc, error...\r\n");
00258 }
00259
00260
00266 int measureUS( int argc, char * argv[], void * data)
00267 {
00268 char Buffer[MAXBUFFERSIZE];
00269 int i;
00270 short usnoise;
00271 short echonbr;
00272 float usconst = 1.715;
00273 short usvalue;
00274 short usampl;
00275 short argument;
00276
00277 argument = atoi(argv[1]);
00278 if(kh3_measure_us((char *)Buffer, argument, dsPic))
00279 {
00280
00281 printf("\r\n%c", Buffer[0]);
00282 for(i = 0; i < 22; i++)
00283 printf(",%4.4u", (Buffer[1+i*2] | Buffer[2+i*2]<<8));
00284 printf("\r\n");
00285
00286
00287 echonbr = (Buffer[1] | Buffer[2]<<8);
00288 printf("echonbr = %d\r\n", echonbr);
00289
00290
00291 if(echonbr < 10)
00292 {
00293 for(i = 0; i < echonbr ; i++)
00294 {
00295
00296 usvalue = (Buffer[i*8+3] | Buffer[i*8+4]<<8) * usconst;
00297 usampl = (Buffer[i*8+5] | Buffer[i*8+6]<<8);
00298
00299
00300 printf("Echo %d : Amplitude = %d, Distance = %dcm.\r\n", i+1, usampl, usvalue);
00301 }
00302 }
00303 else
00304 printf("read error\r\n");
00305 }
00306 else
00307 printf("\r\ng, error...");
00308 }
00309
00310
00317 int motSpeed( int argc, char *argv[], void *data)
00318 {
00319 if(mot1!=0 && mot2!=0)
00320 {
00321 kmot_SetPoint( mot1 , kMotRegSpeed , atoi(argv[1]));
00322 kmot_SetPoint( mot2 , kMotRegSpeed , atoi(argv[2]));
00323 return 0;
00324 }
00325 else
00326 return -1;
00327 }
00328
00329
00336 int motMove( int argc, char *argv[], void *data)
00337 {
00338 if(mot1!=0 && mot2!=0)
00339 {
00340 kmot_SetPoint( mot1 , kMotRegPos, atoi(argv[1]));
00341 kmot_SetPoint( mot2 , kMotRegPos, atoi(argv[2]));
00342 return 0;
00343 }
00344 else
00345 return -1;
00346 }
00347
00348
00353 int motStop( int argc, char *argv[], void *data)
00354 {
00355 if(mot1!=0 && mot2!=0)
00356 {
00357 kmot_SetMode( mot1 , kMotModeStopMotor );
00358 kmot_SetMode( mot2 , kMotModeStopMotor );
00359 return 0;
00360 }
00361 else
00362 return -1;
00363 }
00364
00365
00371 #define BR_IRGAIN 20
00372 #define fwSpeed 60
00373
00374 int braitenbergAvoidance( int argc , char * argv[] , void * data)
00375 {
00376 int Connections_B[9] = { 2, -2, -6, -15, 5, 2, 1, 5, 7};
00377 int Connections_A[9] = { 5, 1, 2, 5, -15, -6, -2, 2, 7};
00378 int i, buflen, sensval;
00379 char buffer[MAXBUFFERSIZE];
00380 char * scan;
00381 long int lspeed16, rspeed16;
00382 int tabsens[9];
00383 int left_speed, right_speed;
00384 u_int8_t valueLL,valueLH,valueHL,valueHH;
00385
00386
00387 char cmd[3] = { 2, 'N', 0};
00388
00389 while(1)
00390 {
00391 lspeed16 = 0; rspeed16 = 0;
00392
00393 kh3_sendcommand( dsPic, cmd );
00394 while(!kb_gpio_get(KNET_INT0));
00395 buflen = knet_llread( dsPic, buffer, 30);
00396 scan = buffer+3;
00397
00398
00399 for (i = 0; i < 9; i++)
00400 {
00401 sensval = *(scan) | (*(scan+1))<<8;
00402 if(sensval > 2000)
00403 tabsens[1] = 500;
00404 else
00405 tabsens[i] = sensval >> 2;
00406 scan = scan + 2;
00407 }
00408
00409 for (i = 0; i < 9; i++)
00410 {
00411 lspeed16 += Connections_A[i] * tabsens[i];
00412 rspeed16 += Connections_B[i] * tabsens[i];
00413 }
00414
00415 left_speed = ((lspeed16 / BR_IRGAIN) + fwSpeed);
00416 right_speed = ((rspeed16 / BR_IRGAIN) + fwSpeed);
00417
00418 if(left_speed > 0 && left_speed < 30)
00419 left_speed = 30;
00420 if(left_speed < 0 && left_speed > -30)
00421 left_speed = -30;
00422 if(right_speed > 0 && right_speed < 30)
00423 right_speed = 30;
00424 if(right_speed < 0 && right_speed > -30)
00425 right_speed = -30;
00426
00427
00428
00429
00430
00431
00432 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
00433
00434
00435
00436
00437
00438
00439 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
00440
00441 printf("lens = %d, rsens = %d lspd = %d rspd = %d\r\n", (int)lspeed16, (int)rspeed16, left_speed, right_speed);
00442
00443 left_speed = kmot_GetMeasure( mot1 , kMotMesSpeed );
00444
00445
00446
00447
00448
00449
00450
00451 right_speed = kmot_GetMeasure( mot2 , kMotMesSpeed );
00452
00453
00454
00455
00456
00457
00458
00459 printf("lspd = %d rspd = %d\r\n", left_speed, right_speed);
00460
00461 usleep(200000);
00462 }
00463 }
00464
00465
00466
00469 int quit( int argc , char * argv[] , void * data)
00470 {
00471 quitReq = 1;
00472 }
00473
00474
00475 int help( int argc , char * argv[] , void * data);
00476
00480 static kb_command_t cmds[] = {
00481 { "quit" , 0 , 0 , quit } ,
00482 { "exit" , 0 , 0 , quit } ,
00483 { "bye" , 0 , 0 , quit } ,
00484 { "setcfg" , 2 , 2 , configureOS },
00485 { "getrev" , 0 , 0 , revisionOS },
00486 { "getbat" , 1 , 1 , voltageBAT },
00487 { "rststamp" , 0 , 0 , tstampRST },
00488 { "getambir" , 0 , 0 , ambIR },
00489 { "getproxir" , 0 , 0 , proxIR },
00490 { "getus" , 1 , 1 , measureUS },
00491 { "setmotspeed" , 2 , 2 , motSpeed },
00492 { "setmotmove" , 2 , 2 , motMove },
00493 { "motstop" , 0 , 0 , motStop },
00494 { "braiten" , 0 , 0 , braitenbergAvoidance},
00495 { "help" , 0 , 0 , help } ,
00496 { NULL , 0 , 0 , NULL }
00497 };
00498
00499
00502 int help( int argc , char * argv[] , void * data)
00503 {
00504 kb_command_t * scan = cmds;
00505 while(scan->name != NULL)
00506 {
00507 printf("%s\r\n",scan->name);
00508 scan++;
00509 }
00510 return 0;
00511 }
00512
00513 #define MAXBUFFERSIZE 256
00514
00515
00516 int main( int arc, char *argv[])
00517 {
00518 char Buffer[MAXBUFFERSIZE];
00519 char i;
00520
00521
00522 printf("Khepera3 test program (C) K-Team S.A\r\n");
00523
00524 if(!initKH3())
00525 {
00526 printf("Init oke...\r\n");
00527
00528 while (!quitReq)
00529 {
00530
00531 memset(Buffer,0,sizeof(Buffer));
00532 kh3_proximity_ir((char *)Buffer, dsPic);
00533 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",
00534 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),(Buffer[5] |
00535 Buffer[6]<<8), (Buffer[7] | Buffer[8]<<8),
00536 (Buffer[9] | Buffer[10]<<8), (Buffer[11] |
00537 Buffer[12]<<8),(Buffer[13] | Buffer[14]<<8), (Buffer[15] | Buffer[16]<<8)
00538 ,(Buffer[17] | Buffer[18]<<8),(Buffer[19] |
00539 Buffer[20]<<8),(Buffer[21] | Buffer[22]<<8));
00540
00541
00542
00543
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 printf("Exiting...\r\n");
00574 }
00575 else
00576 printf("Fatal error, unable to initialize\r\n");
00577
00578 }
00579
00580