00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include <signal.h>
00012 #include <korebot/korebot.h>
00013
00028 static int quitReq = 0;
00029 static knet_dev_t * koreio;
00030
00031
00035 static void ctrlc_handler( int sig )
00036 {
00037 quitReq = 1;
00038 }
00039
00040
00043 int quit( int argc , char * argv[] , void * data)
00044 {
00045 quitReq = 1;
00046 }
00047
00048
00053 int readios( int argc , char * argv[] , void * data)
00054 {
00055
00056 }
00057
00058
00066 int configio(int argc, char * argv[], void * data)
00067 {
00068 unsigned config;
00069 int rc;
00070 char * msg[] = {"input", "output", "pwm"};
00071
00072 rc = kio_ConfigIO(koreio,atoi(argv[1]),atoi(argv[2]));
00073
00074 if(rc == -1)
00075 printf("wrong io\r\n");
00076 else if(rc == -2)
00077 printf("wrong config\r\n");
00078 else
00079 printf("Configured io %d in %s mode\r\n",atoi(argv[1]),msg[atoi(argv[2])]);
00080 }
00081
00082
00085 int resetio( int argc, char * argv[], void * data)
00086 {
00087 int rc;
00088
00089 rc = kio_ClearIO(koreio,atoi(argv[1]));
00090
00091 if(rc < 0)
00092 printf("wrong io\r\n");
00093 else
00094 printf("Cleared io: %d\r\n",atoi(argv[1]));
00095 }
00096
00097
00100 int changeled( int argc, char * argv[], void * data)
00101 {
00102 int rc;
00103
00104 rc = kio_ChangeLed(koreio,atoi(argv[1]));
00105
00106 if(rc < 0)
00107 printf("wrong state\r\n");
00108 else
00109 printf("Changed LED state\r\n");
00110 }
00111
00112
00115 int changeio( int argc, char * argv[], void * data)
00116 {
00117 int rc;
00118
00119 rc = kio_ChangeIO(koreio,atoi(argv[1]));
00120
00121 if(rc < 0)
00122 printf("wrong io\r\n");
00123 else
00124 printf("Changed io: %d\r\n",atoi(argv[1]));
00125 }
00126
00127
00130 int setio( int argc, char * argv[], void * data)
00131 {
00132 int rc;
00133
00134 rc = kio_SetIO(koreio,atoi(argv[1]));
00135
00136 if(rc < 0)
00137 printf("wrong io\r\n");
00138 else
00139 printf("Set io: %d\r\n",atoi(argv[1]));
00140 }
00141
00142
00145 int readad( int argc, char * argv[], void * data)
00146 {
00147 uint16_t val;
00148 uint32_t time;
00149 int rc;
00150
00151 rc = kio_ReadAnalog(koreio,atoi(argv[1]),&val,&time);
00152
00153 if(rc < 0)
00154 printf("wrong ad\r\n");
00155 else
00156 printf("read ad %d: %u at %lu mS\r\n",atoi(argv[1]),val,time);
00157 }
00158
00159
00160
00163 int readcan(int argc, char * argv[], void * data)
00164 {
00165 char can_status, len;
00166 uint32_t can_data1, can_data2;
00167 uint32_t time, id;
00168 int rc;
00169 rc = kio_ReadCAN(koreio,&id,&len,&can_data1,&can_data2,&can_status,&time);
00170
00171 if(rc == -1)
00172 printf("Buffer empty\r\n");
00173 else if(rc == -2)
00174 printf("Error communication with turret\r\n");
00175 else
00176 {
00177 printf("ID: 0x%lx, len: %d, Data: 0x%lx%lx, status: 0x%x, at %lu mS\r\n",id,len,can_data1, can_data2,can_status,time);
00178 printf("%d messages remaining in buffer\r\n", rc);
00179 }
00180 }
00181
00182
00186 int can7seg(int argc, char * argv[], void * data)
00187 {
00188 #define id 0x00000400
00189 #define null 0x1E7F4000
00190 #define one 0x1E7F7900
00191 #define two 0x1E7F2400
00192 #define three 0x1E7F3000
00193 #define four 0x1E7F1900
00194 #define five 0x1E7F1200
00195 #define six 0x1E7F0200
00196 #define seven 0x1E7F7800
00197 #define eight 0x1E7F0000
00198 #define nine 0x1E7F1000
00199 #define can_status 0x02
00200
00201 int rc;
00202 uint32_t can_data = 0;
00203 char len = 3;
00204
00205 switch (atoi(argv[1]))
00206 {
00207 case 0:
00208 can_data = null;
00209 break;
00210 case 1:
00211 can_data = one;
00212 break;
00213 case 2:
00214 can_data = two;
00215 break;
00216 case 3:
00217 can_data = three;
00218 break;
00219 case 4:
00220 can_data = four;
00221 break;
00222 case 5:
00223 can_data = five;
00224 break;
00225 case 6:
00226 can_data = six;
00227 break;
00228 case 7:
00229 can_data = seven;
00230 break;
00231 case 8:
00232 can_data = eight;
00233 break;
00234 default:
00235 can_data = nine;
00236 break;
00237 }
00238
00239 rc = kio_SendCAN(koreio, id, can_data, can_data, len, can_status);
00240
00241 if(rc != 0)
00242 printf("CAN write failed\r\n");
00243 else
00244 printf("Number %d set on the 7 seg\r\n",atoi(argv[1]));
00245 }
00246
00247
00250 int canled( int argc, char * argv[], void *data)
00251 {
00252 #define id_led 0x00000300
00253 #define can_data1 0x1E
00254
00255 int rc;
00256 uint32_t can_data = 0;
00257 char len = 3;
00258 char status = 0x02;
00259 char on_off = 0xFF;
00260
00261 if (atoi(argv[2]))
00262 on_off = 0;
00263
00264 can_data = (((((can_data | can_data1) << 8) | atoi(argv[1])) << 8) | on_off) << 8;
00265 rc = kio_SendCAN(koreio, id_led, can_data, can_data, len, can_status);
00266
00267 if(rc != 0)
00268 printf("CAN write failed\r\n");
00269 else
00270 printf("Leds %d set\r\n",atoi(argv[1]));
00271
00272 }
00273
00274
00275
00278 int setad( int argc, char * argv[], void * data)
00279 {
00280 int rc;
00281 float val;
00282
00283 rc = kio_SetANValue(koreio,atoi(argv[1]), atoi(argv[2]));
00284
00285 val = (atoi(argv[2]) - 139) *0.035;
00286 if(rc < 0)
00287 printf("wrong analog output\r\n");
00288 else
00289 printf("ananlog output %d: %f V\r\n",atoi(argv[1]), val);
00290
00291 }
00292
00293
00296 int setratio( int argc, char * argv[], void * data)
00297 {
00298 int rc;
00299
00300 rc = kio_ChangePWM_ratio(koreio, atoi(argv[1]), atoi(argv[2]));
00301
00302 if(rc < 0)
00303 printf("wrong PWM output\r\n");
00304 else
00305 printf("channel %d set to %u%%\r\n",atoi(argv[1]), atoi(argv[2]));
00306 }
00307
00308
00311 int sequence( int argc, char * argv[], void * data)
00312 {
00313 int rc, val;
00314
00315 val = 50 + 82;
00316 rc = kio_ChangePWM_ratio(koreio, 0 , val);
00317 val = (255 - val) + 82 ;
00318 rc = kio_ChangePWM_ratio(koreio, 1 , val);
00319
00320
00321 val = 123 + 82;
00322 rc = kio_ChangePWM_ratio(koreio, 2 , val);
00323 val = (255 - val) + 82 ;
00324 rc = kio_ChangePWM_ratio(koreio, 3 , val);
00325
00326 val = 100 + 82;
00327 rc = kio_ChangePWM_ratio(koreio, 4 , val);
00328 val = (255 - val) + 82 ;
00329 rc = kio_ChangePWM_ratio(koreio, 5 , val);
00330
00331 val = 0;
00332 rc = kio_ChangePWM_ratio(koreio, 6 , val);
00333
00334 val = 0;
00335 rc = kio_ChangePWM_ratio(koreio, 7 , val);
00336
00338
00339 usleep(2000000);
00340 val = 82;
00341 rc = kio_ChangePWM_ratio(koreio, 4 , val);
00342 val = (255 - val) + 82 ;
00343 rc = kio_ChangePWM_ratio(koreio, 5 , val);
00344
00345 usleep(200000);
00346
00347 val = 8+ 82;
00348 rc = kio_ChangePWM_ratio(koreio, 0 , val);
00349 val = (255 - val) + 82 ;
00350 rc = kio_ChangePWM_ratio(koreio, 1 , val);
00351
00352
00353 val = 255;
00354 rc = kio_ChangePWM_ratio(koreio, 2 , val);
00355 val = (255 - val) + 82 ;
00356 rc = kio_ChangePWM_ratio(koreio, 3 , val);
00357
00358 val = 0;
00359 rc = kio_ChangePWM_ratio(koreio, 6 , val);
00360
00361 val = 0;
00362 rc = kio_ChangePWM_ratio(koreio, 7 , val);
00363
00365
00366 usleep(2000000);
00367
00368 val = 170 + 82;
00369 rc = kio_ChangePWM_ratio(koreio, 0 , val);
00370 val = (255 - val) + 82 ;
00371 rc = kio_ChangePWM_ratio(koreio, 1 , val);
00372
00373
00374 val = 255;
00375 rc = kio_ChangePWM_ratio(koreio, 2 , val);
00376 val = (255 - val) + 82 ;
00377 rc = kio_ChangePWM_ratio(koreio, 3 , val);
00378
00379 val = 170 + 82;
00380 rc = kio_ChangePWM_ratio(koreio, 4 , val);
00381 val = (255 - val) + 82 ;
00382 rc = kio_ChangePWM_ratio(koreio, 5 , val);
00383
00384 val = 0;
00385 rc = kio_ChangePWM_ratio(koreio, 6 , val);
00386
00387 val = 0;
00388 rc = kio_ChangePWM_ratio(koreio, 7 , val);
00389
00391
00392
00393
00394 if(rc < 0)
00395 printf("wrong PWM output\r\n");
00396 else
00397 printf("Gripper moved");
00398 }
00399
00400
00403 int gripper( int argc, char * argv[], void * data)
00404 {
00405 int rc, val;
00406
00407 val = atoi(argv[1]);
00408 if (val > 173) val = 255;
00409 else if (val < 0) val = 82;
00410 else val += 82;
00411 rc = kio_ChangePWM_ratio(koreio, 0 , val);
00412
00413 val = (255 - val) + 82 ;
00414 rc = kio_ChangePWM_ratio(koreio, 1 , val);
00415
00416
00417 val = atoi(argv[2]);
00418 if (val > 173) val = 255;
00419 else if (val < 0) val = 82;
00420 else val += 82;
00421 rc = kio_ChangePWM_ratio(koreio, 2 , val);
00422
00423 val = (255 - val) + 82 ;
00424 rc = kio_ChangePWM_ratio(koreio, 3 , val);
00425
00426 val = atoi(argv[3]);
00427 if (val > 173) val = 255;
00428 else if (val < 0) val = 82;
00429 else val += 82;
00430 rc = kio_ChangePWM_ratio(koreio, 4 , val);
00431
00432 val = (255 - val) + 82 ;
00433 rc = kio_ChangePWM_ratio(koreio, 5 , val);
00434
00435 val = atoi(argv[4]);
00436 if (val > 255) val = 255;
00437 else if (val < 55) val = 55;
00438 rc = kio_ChangePWM_ratio(koreio, 6 , val);
00439
00440 val = atoi(argv[5]);
00441 if (val > 255) val = 255;
00442 else if (val < 55) val = 55;
00443 rc = kio_ChangePWM_ratio(koreio, 7 , val);
00444
00445
00446
00447
00448 if(rc < 0)
00449 printf("wrong PWM output\r\n");
00450 else
00451 printf("Gripper moved");
00452 }
00453
00454
00455
00456
00459 int shoulder( int argc, char * argv[], void * data)
00460 {
00461 int rc, val;
00462
00463 val = atoi(argv[1]);
00464 if (val > 173) val = 255;
00465 else if (val < 0) val = 82;
00466 else val += 82;
00467 rc = kio_ChangePWM_ratio(koreio, 0 , val);
00468
00469 val = (255 - val) + 82 ;
00470 rc = kio_ChangePWM_ratio(koreio, 1 , val);
00471
00472
00473
00474 if(rc < 0)
00475 printf("wrong PWM output\r\n");
00476 else
00477 printf("poignet set to %u\r\n",val);
00478 }
00479
00480
00481
00484 int elbow( int argc, char * argv[], void * data)
00485 {
00486 int rc, val;
00487
00488 val = atoi(argv[1]);
00489 if (val > 173) val = 255;
00490 else if (val < 0) val = 82;
00491 else val += 82;
00492 rc = kio_ChangePWM_ratio(koreio, 2 , val);
00493
00494 val = (255 - val) + 82 ;
00495 rc = kio_ChangePWM_ratio(koreio, 3 , val);
00496
00497
00498
00499 if(rc < 0)
00500 printf("wrong PWM output\r\n");
00501 else
00502 printf("poignet set to %u\r\n",val);
00503 }
00504
00505
00506
00507
00510 int wrist( int argc, char * argv[], void * data)
00511 {
00512 int rc, val;
00513
00514 val = atoi(argv[1]);
00515 if (val > 173) val = 255;
00516 else if (val < 0) val = 82;
00517 else val += 82;
00518 rc = kio_ChangePWM_ratio(koreio, 4 , val);
00519
00520 val = (255 - val) + 82 ;
00521 rc = kio_ChangePWM_ratio(koreio, 5 , val);
00522
00523
00524
00525 if(rc < 0)
00526 printf("wrong PWM output\r\n");
00527 else
00528 printf("poignet set to %u\r\n",val);
00529 }
00530
00531
00534 int rotate( int argc, char * argv[], void * data)
00535 {
00536 int rc, val ;
00537
00538 val = atoi(argv[1]);
00539 if (val > 255) val = 255;
00540 else if (val < 55) val = 55;
00541 rc = kio_ChangePWM_ratio(koreio, 6 , val);
00542
00543 if(rc < 0)
00544 printf("wrong PWM output\r\n");
00545 else
00546 printf("rot set to %u\r\n",atoi(argv[1]));
00547 }
00548
00551 int finger( int argc, char * argv[], void * data)
00552 {
00553 int rc, val;
00554
00555 val = atoi(argv[1]);
00556 if (val > 255) val = 255;
00557 else if (val < 55) val = 55;
00558 rc = kio_ChangePWM_ratio(koreio, 7 , val );
00559
00560 if(rc < 0)
00561 printf("wrong PWM output\r\n");
00562 else
00563 printf("pince set to %u\r\n",atoi(argv[1]));
00564 }
00565
00566
00567
00568
00569
00570
00571
00572
00575 int setfreq(int argc, char * argv[], void * data)
00576 {
00577 int rc;
00578
00579 rc = kio_ChangePWM_freq(koreio, atoi(argv[1]));
00580
00581 if(rc < 0)
00582 printf("Wrong value\r\n");
00583 else
00584 printf("Frequence set to %lu Hz\r\n", atoi(argv[1]));
00585 }
00586
00587
00590 int readio( int argc, char * argv[], void * data)
00591 {
00592 int rc;
00593
00594 rc = kio_ReadIO(koreio,atoi(argv[1]));
00595
00596 if(rc < 0)
00597 printf("wrong io or not in input mode\r\n");
00598 else
00599 printf("read io %d: %d\r\n",atoi(argv[1]),rc);
00600 }
00601
00602
00605 int changepw( int argc, char * argv[], void * data)
00606 {
00607 int rc;
00608
00609 rc = kio_ChangePW(koreio,atoi(argv[1]));
00610
00611 if(rc < 0)
00612 printf("wrong pw\r\n");
00613 else
00614 printf("Changed pw: %d\r\n",atoi(argv[1]));
00615 }
00616
00617
00620 int setpw( int argc, char * argv[], void * data)
00621 {
00622 int rc;
00623
00624 rc = kio_SetPW(koreio,atoi(argv[1]));
00625
00626 if(rc < 0)
00627 printf("wrong pw\r\n");
00628 else
00629 printf("Set pw: %d\r\n",atoi(argv[1]));
00630 }
00631
00632
00635 int readi2c(int argc, char * argv[], void * data)
00636 {
00637 int rc;
00638 uint32_t i2c_values;
00639 char temp;
00640 char i;
00641 char n_read;
00642
00643 kio_i2c_StartRead(koreio,atoi(argv[1]), atoi(argv[2]), atoi(argv[3]));
00644
00645 usleep(10000);
00646
00647 rc = kio_i2c_ReturnRead(koreio,atoi(argv[3]), &i2c_values);
00648
00649 n_read = atoi(argv[3]);
00650 if (n_read > 4)
00651 n_read = 4;
00652
00653 for(i = 0; i < n_read; i++)
00654 {
00655 temp = i2c_values;
00656 printf("Read n°%d : 0x%x\r\n",i,temp);
00657 i2c_values = i2c_values >> 8;
00658 }
00659
00660 }
00661
00662
00665 int writei2c(int argc, char * argv[], void * data)
00666 {
00667
00668 kio_i2c_Write(koreio, atoi(argv[1]), atoi(argv[2]), atoi(argv[3]));
00669
00670 printf("Write 0x%x on device 0x%x at register 0x%x\r\n", atoi(argv[3]), atoi(argv[1]), atoi(argv[2]));
00671 }
00672
00673
00674
00678 int startscan(int argc, char * argv[], void * data)
00679 {
00680 kio_i2c_StartScan(koreio);
00681 printf("I2C address scan started\r\n");
00682 }
00683
00684
00687 int listscan(int argc, char * argv[], void * data)
00688 {
00689 char addlist[128];
00690 int rc,i;
00691
00692 rc = kio_i2c_ListScan(koreio, addlist);
00693
00694 if(rc < 0)
00695 printf("List scan error\r\n");
00696 else
00697 printf("Secondary bus scan: %d device found\r\n",rc);
00698
00699 for(i=0; i<rc; i++)
00700 printf("device %d: 0x%x\r\n",i,addlist[i]);
00701
00702 }
00703
00704
00707 int resetpw( int argc, char * argv[], void * data)
00708 {
00709 int rc;
00710
00711 rc = kio_ClearPW(koreio,atoi(argv[1]));
00712
00713 if(rc < 0)
00714 printf("wrong pw\r\n");
00715 else
00716 printf("Cleared pw: %d\r\n",atoi(argv[1]));
00717 }
00718
00719
00726 int timer( int argc, char * argv[], void * data)
00727 {
00728 int rc;
00729 char * msg[] = {"reset", "stopped", "started"};
00730
00731 rc = kio_Timer(koreio,atoi(argv[1]));
00732
00733 if(rc < 0)
00734 printf("wrong action\r\n");
00735 else
00736 printf("Timer is %s\r\n",msg[atoi(argv[1])]);
00737 }
00738
00739 int help( int argc , char * argv[] , void * data);
00740
00744 static kb_command_t cmds[] = {
00745 { "quit" , 0 , 0 , quit } ,
00746 { "exit" , 0 , 0 , quit } ,
00747 { "bye" , 0 , 0 , quit } ,
00748 { "sequence" , 0 , 0 , sequence},
00749 { "gripper" , 5 , 5 , gripper},
00750 { "shoulder" , 1 , 1 , shoulder },
00751 { "elbow" , 1 , 1 , elbow },
00752 { "wrist" , 1 , 1 , wrist },
00753 { "rotate" , 1 , 1 , rotate },
00754 { "finger" , 1 , 1 , finger },
00755 { "readios" , 0 , 0 , readios },
00756 { "readio" , 1 , 1 , readio },
00757 { "setio" , 1 , 1 , setio },
00758 { "cleario" , 1 , 1 , resetio },
00759 { "setratio" , 2 , 2 , setratio },
00760 { "setfreq" , 1 , 1 , setfreq },
00761 { "setpw" , 1 , 1 , setpw },
00762 { "clearpw" , 1 , 1 , resetpw },
00763 { "changepw" , 1 , 1 , changepw },
00764 { "changeio" , 1 , 1 , changeio },
00765 { "changeled" , 1 , 1 , changeled },
00766 { "configio" , 2 , 2 , configio },
00767 { "readad" , 1 , 1 , readad },
00768 { "setad" , 2 , 2 , setad },
00769 { "startscan" , 0 , 0 , startscan},
00770 { "listscan" , 0 , 0 , listscan},
00771 { "readi2c" , 3 , 3 , readi2c },
00772 { "writei2c" , 3 , 3 , writei2c },
00773 { "readcan" , 0 , 0 , readcan},
00774 { "can7seg" , 1 , 1 , can7seg},
00775 { "canled" , 2 , 2 , canled},
00776 { "timer" , 1 , 1 , timer},
00777 { "help" , 0 , 0 , help } ,
00778 { NULL , 0 , 0 , NULL }
00779 };
00780
00781
00784 int help( int argc , char * argv[] , void * data)
00785 {
00786 kb_command_t * scan = cmds;
00787 while(scan->name != NULL)
00788 {
00789 printf("%s\r\n",scan->name);
00790 scan++;
00791 }
00792 return 0;
00793 }
00794
00795
00799 static char buf[1024];
00800
00801 int main( int argc , char * argv[] )
00802 {
00803 int rc,ver;
00804
00805
00806 kb_set_debug_level(2);
00807
00808 if((rc = kb_init( argc , argv )) < 0 )
00809 return 1;
00810
00811 signal( SIGINT , ctrlc_handler );
00812
00813 printf("K-Team KoreIO Test Program\r\n");
00814
00815 koreio = knet_open( "KoreIO:Board", KNET_BUS_ANY, 0 , NULL );
00816 if(!koreio)
00817 {
00818 printf("Cannot open KoreIO device trying alternate address\r\n");
00819 koreio = knet_open( "KoreIO:AltBoard", KNET_BUS_ANY, 0 , NULL );
00820 if(!koreio)
00821 {
00822 printf("Cannot open KoreIO device\r\n");
00823 return 1;
00824 }
00825 }
00826
00827
00828 kio_GetFWVersion(koreio,&ver);
00829
00830 printf("KoreIO firmware %d.%d\r\n", (ver&0x000000F0)>>4, (ver&0x0000000F));
00831
00832
00833 kio_ChangePWM_freq(koreio, 0);
00834
00835
00836
00837
00838
00839
00840
00841
00842
00843
00844 kio_ConfigIO(koreio,0,2);
00845 kio_ConfigIO(koreio,1,2);
00846 kio_ConfigIO(koreio,2,2);
00847 kio_ConfigIO(koreio,3,2);
00848 kio_ConfigIO(koreio,4,2);
00849 kio_ConfigIO(koreio,5,2);
00850 kio_ConfigIO(koreio,6,2);
00851 kio_ConfigIO(koreio,7,2);
00852
00853
00854
00855
00856
00857 while (!quitReq) {
00858 printf("\n> ");
00859
00860 if ( fgets( buf , sizeof(buf) , stdin ) != NULL ) {
00861 buf[strlen(buf)-1] = '\0';
00862 kb_parse_command( buf , cmds , NULL);
00863 }
00864 }
00865
00866 knet_close( koreio );
00867 }