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 motor( int argc, char * argv[], void * data)
00312 {
00313 int rc;
00314 int ratio;
00315
00316 ratio = 170 + atoi(argv[1]);
00317 rc = kio_ChangePWM_ratio(koreio, 5 , ratio);
00318
00319 ratio = 163 - atoi(argv[2]);
00320 rc = kio_ChangePWM_ratio(koreio, 4 , ratio);
00321
00322
00323
00324
00325
00326
00327
00328 printf("d\r\n");
00329 }
00330
00331
00332
00335 int gripper( int argc, char * argv[], void * data)
00336 {
00337 int rc;
00338
00339 rc = kio_ChangePWM_ratio(koreio, 6 , atoi(argv[1]));
00340
00341 rc = kio_ChangePWM_ratio(koreio, 7 , atoi(argv[2]));
00342
00343
00344
00345
00346
00347
00348
00349 printf("g\r\n");
00350 }
00351
00352
00353
00356 int setfreq(int argc, char * argv[], void * data)
00357 {
00358 int rc;
00359
00360 rc = kio_ChangePWM_freq(koreio, atoi(argv[1]));
00361
00362 if(rc < 0)
00363 printf("Wrong value\r\n");
00364 else
00365 printf("Frequence set to %lu Hz\r\n", atoi(argv[1]));
00366 }
00367
00368
00371 int readio( int argc, char * argv[], void * data)
00372 {
00373 int rc;
00374
00375 rc = kio_ReadIO(koreio,atoi(argv[1]));
00376
00377 if(rc < 0)
00378 printf("wrong io or not in input mode\r\n");
00379 else
00380 printf("read io %d: %d\r\n",atoi(argv[1]),rc);
00381 }
00382
00383
00386 int changepw( int argc, char * argv[], void * data)
00387 {
00388 int rc;
00389
00390 rc = kio_ChangePW(koreio,atoi(argv[1]));
00391
00392 if(rc < 0)
00393 printf("wrong pw\r\n");
00394 else
00395 printf("Changed pw: %d\r\n",atoi(argv[1]));
00396 }
00397
00398
00401 int setpw( int argc, char * argv[], void * data)
00402 {
00403 int rc;
00404
00405 rc = kio_SetPW(koreio,atoi(argv[1]));
00406
00407 if(rc < 0)
00408 printf("wrong pw\r\n");
00409 else
00410 printf("Set pw: %d\r\n",atoi(argv[1]));
00411 }
00412
00413
00416 int readi2c(int argc, char * argv[], void * data)
00417 {
00418 int rc;
00419 uint32_t i2c_values;
00420 char temp;
00421 char i;
00422 char n_read;
00423
00424 kio_i2c_StartRead(koreio,atoi(argv[1]), atoi(argv[2]), atoi(argv[3]));
00425
00426 usleep(10000);
00427
00428 rc = kio_i2c_ReturnRead(koreio,atoi(argv[3]), &i2c_values);
00429
00430 n_read = atoi(argv[3]);
00431 if (n_read > 4)
00432 n_read = 4;
00433
00434 for(i = 0; i < n_read; i++)
00435 {
00436 temp = i2c_values;
00437 printf("Read n°%d : 0x%x\r\n",i,temp);
00438 i2c_values = i2c_values >> 8;
00439 }
00440
00441 }
00442
00443
00446 int writei2c(int argc, char * argv[], void * data)
00447 {
00448
00449 kio_i2c_Write(koreio, atoi(argv[1]), atoi(argv[2]), atoi(argv[3]));
00450
00451 printf("Write 0x%x on device 0x%x at register 0x%x\r\n", atoi(argv[3]), atoi(argv[1]), atoi(argv[2]));
00452 }
00453
00454
00455
00459 int startscan(int argc, char * argv[], void * data)
00460 {
00461 kio_i2c_StartScan(koreio);
00462 printf("I2C address scan started\r\n");
00463 }
00464
00465
00468 int listscan(int argc, char * argv[], void * data)
00469 {
00470 char addlist[128];
00471 int rc,i;
00472
00473 rc = kio_i2c_ListScan(koreio, addlist);
00474
00475 if(rc < 0)
00476 printf("List scan error\r\n");
00477 else
00478 printf("Secondary bus scan: %d device found\r\n",rc);
00479
00480 for(i=0; i<rc; i++)
00481 printf("device %d: 0x%x\r\n",i,addlist[i]);
00482
00483 }
00484
00485
00488 int resetpw( int argc, char * argv[], void * data)
00489 {
00490 int rc;
00491
00492 rc = kio_ClearPW(koreio,atoi(argv[1]));
00493
00494 if(rc < 0)
00495 printf("wrong pw\r\n");
00496 else
00497 printf("Cleared pw: %d\r\n",atoi(argv[1]));
00498 }
00499
00500
00507 int timer( int argc, char * argv[], void * data)
00508 {
00509 int rc;
00510 char * msg[] = {"reset", "stopped", "started"};
00511
00512 rc = kio_Timer(koreio,atoi(argv[1]));
00513
00514 if(rc < 0)
00515 printf("wrong action\r\n");
00516 else
00517 printf("Timer is %s\r\n",msg[atoi(argv[1])]);
00518 }
00519
00520 int sequence( int argc, char * argv[], void * data)
00521 {
00522 int rc, val;
00523
00524 kio_ChangePWM_ratio(koreio, 4 , 163); usleep(10);
00525 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00526 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00527 kio_ChangePWM_ratio(koreio, 7 , 170);usleep(10);
00528 usleep(1000000);
00529
00530 kio_ChangePWM_ratio(koreio, 4 , 163);usleep(10);
00531 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00532 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00533 kio_ChangePWM_ratio(koreio, 7 , 0);usleep(10);
00534 usleep(1000000);
00535
00536 kio_ChangePWM_ratio(koreio, 4 , 163);usleep(10);
00537 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00538 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00539 kio_ChangePWM_ratio(koreio, 7 , 170);
00540 usleep(1000000);
00541
00542 kio_ChangePWM_ratio(koreio, 4 , 163);usleep(10);
00543 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00544 kio_ChangePWM_ratio(koreio, 6 , 80);usleep(10);
00545 kio_ChangePWM_ratio(koreio, 7 , 0);
00546 usleep(1000000);
00547
00548 kio_ChangePWM_ratio(koreio, 4 , 163);usleep(10);
00549 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00550 kio_ChangePWM_ratio(koreio, 6 , 80);usleep(10);
00551 kio_ChangePWM_ratio(koreio, 7 , 170);
00552 usleep(1000000);
00553
00554
00555 kio_ChangePWM_ratio(koreio, 4 , 161);usleep(10);
00556 kio_ChangePWM_ratio(koreio, 5 , 172);usleep(10);
00557 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00558 kio_ChangePWM_ratio(koreio, 7 , 0);
00559 usleep(1000000);
00560
00561 kio_ChangePWM_ratio(koreio, 4 , 160);usleep(10);
00562 kio_ChangePWM_ratio(koreio, 5 , 173);usleep(10);
00563 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00564 kio_ChangePWM_ratio(koreio, 7 , 0);
00565 usleep(1000000);
00566
00567
00568 kio_ChangePWM_ratio(koreio, 4 , 166);usleep(10);
00569 kio_ChangePWM_ratio(koreio, 5 , 177);usleep(10);
00570 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00571 kio_ChangePWM_ratio(koreio, 7 , 170);
00572 usleep(1000000);
00573
00574
00575 kio_ChangePWM_ratio(koreio, 4 , 153);usleep(10);
00576 kio_ChangePWM_ratio(koreio, 5 , 180);usleep(10);
00577 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00578 kio_ChangePWM_ratio(koreio, 7 , 0);
00579 usleep(2000000);
00580
00581
00582 kio_ChangePWM_ratio(koreio, 4 , 160);usleep(10);
00583 kio_ChangePWM_ratio(koreio, 5 , 173);usleep(10);
00584 kio_ChangePWM_ratio(koreio, 6 , 80);usleep(10);
00585 kio_ChangePWM_ratio(koreio, 7 , 170);
00586 usleep(1000000);
00587
00588
00589 kio_ChangePWM_ratio(koreio, 4 , 163);usleep(10);
00590 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00591 kio_ChangePWM_ratio(koreio, 6 , 80);usleep(10);
00592 kio_ChangePWM_ratio(koreio, 7 , 0);
00593 usleep(1000000);
00594
00595
00596 kio_ChangePWM_ratio(koreio, 4 , 171);usleep(10);
00597 kio_ChangePWM_ratio(koreio, 5 , 162);usleep(10);
00598 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00599 kio_ChangePWM_ratio(koreio, 7 , 0);
00600 usleep(1000000);
00601
00602
00603 kio_ChangePWM_ratio(koreio, 4 , 155);usleep(10);
00604 kio_ChangePWM_ratio(koreio, 5 , 162);usleep(10);
00605 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00606 kio_ChangePWM_ratio(koreio, 7 , 0);
00607 usleep(1000000);
00608
00609
00610 kio_ChangePWM_ratio(koreio, 4 , 155);usleep(10);
00611 kio_ChangePWM_ratio(koreio, 5 , 178);usleep(10);
00612 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00613 kio_ChangePWM_ratio(koreio, 7 , 0);
00614 usleep(2000000);
00615
00616
00617 kio_ChangePWM_ratio(koreio, 4 , 163);usleep(10);
00618 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00619 kio_ChangePWM_ratio(koreio, 6 , 80);usleep(10);
00620 kio_ChangePWM_ratio(koreio, 7 , 0);
00621 usleep(500000);
00622
00623
00624 kio_ChangePWM_ratio(koreio, 4 , 163);usleep(10);
00625 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00626 kio_ChangePWM_ratio(koreio, 6 , 80);usleep(10);
00627 kio_ChangePWM_ratio(koreio, 7 , 170);
00628 usleep(500000);
00629
00630
00631 kio_ChangePWM_ratio(koreio, 4 , 168);usleep(10);
00632 kio_ChangePWM_ratio(koreio, 5 , 165);usleep(10);
00633 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00634 kio_ChangePWM_ratio(koreio, 7 , 170);
00635 usleep(2000000);
00636
00637
00638 kio_ChangePWM_ratio(koreio, 4 , 158);usleep(10);
00639 kio_ChangePWM_ratio(koreio, 5 , 165);usleep(10);
00640 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00641 kio_ChangePWM_ratio(koreio, 7 , 0);
00642 usleep(1000000);
00643
00644
00645 kio_ChangePWM_ratio(koreio, 4 , 163);usleep(10);
00646 kio_ChangePWM_ratio(koreio, 5 , 170);usleep(10);
00647 kio_ChangePWM_ratio(koreio, 6 , 200);usleep(10);
00648 kio_ChangePWM_ratio(koreio, 7 , 170);
00649 usleep(1000000);
00650
00651
00652
00653
00654
00655
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 { "readios" , 0 , 0 , readios },
00669 { "readio" , 1 , 1 , readio },
00670 { "setio" , 1 , 1 , setio },
00671 { "cleario" , 1 , 1 , resetio },
00672 { "setratio" , 2 , 2 , setratio },
00673 { "setfreq" , 1 , 1 , setfreq },
00674 { "setpw" , 1 , 1 , setpw },
00675 { "clearpw" , 1 , 1 , resetpw },
00676 { "changepw" , 1 , 1 , changepw },
00677 { "changeio" , 1 , 1 , changeio },
00678 { "changeled" , 1 , 1 , changeled },
00679 { "configio" , 2 , 2 , configio },
00680 { "readad" , 1 , 1 , readad },
00681 { "setad" , 2 , 2 , setad },
00682 { "startscan" , 0 , 0 , startscan},
00683 { "listscan" , 0 , 0 , listscan},
00684 { "readi2c" , 3 , 3 , readi2c },
00685 { "writei2c" , 3 , 3 , writei2c },
00686 { "readcan" , 0 , 0 , readcan},
00687 { "can7seg" , 1 , 1 , can7seg},
00688 { "canled" , 2 , 2 , canled},
00689 { "timer" , 1 , 1 , timer},
00690 { "d" , 2 , 2 , motor },
00691 { "g" , 2 , 2 , gripper },
00692 { "seq" , 0 , 0 , sequence },
00693 { "help" , 0 , 0 , help } ,
00694 { NULL , 0 , 0 , NULL }
00695 };
00696
00697
00700 int help( int argc , char * argv[] , void * data)
00701 {
00702 kb_command_t * scan = cmds;
00703 while(scan->name != NULL)
00704 {
00705 printf("%s\r\n",scan->name);
00706 scan++;
00707 }
00708 return 0;
00709 }
00710
00711
00715 static char buf[1024];
00716
00717 int main( int argc , char * argv[] )
00718 {
00719 int rc,ver;
00720
00721
00722 kb_set_debug_level(2);
00723
00724 if((rc = kb_init( argc , argv )) < 0 )
00725 return 1;
00726
00727 signal( SIGINT , ctrlc_handler );
00728
00729 printf("K-Team KoreIO Test Program\r\n");
00730
00731 koreio = knet_open( "KoreIO:Board", KNET_BUS_ANY, 0 , NULL );
00732 if(!koreio)
00733 {
00734 printf("Cannot open KoreIO device trying alternate address\r\n");
00735 koreio = knet_open( "KoreIO:AltBoard", KNET_BUS_ANY, 0 , NULL );
00736 if(!koreio)
00737 {
00738 printf("Cannot open KoreIO device\r\n");
00739 return 1;
00740 }
00741 }
00742
00743
00744 kio_GetFWVersion(koreio,&ver);
00745
00746 printf("KoreIO firmware %d.%d\r\n", (ver&0x000000F0)>>4, (ver&0x0000000F));
00747
00748
00749 kio_ChangePWM_freq(koreio, 0 );
00750 kio_ConfigIO(koreio,4,2);
00751 kio_ConfigIO(koreio,5,2);
00752 kio_ConfigIO(koreio,6,2);
00753 kio_ConfigIO(koreio,7,2);
00754 kio_ChangePWM_ratio(koreio, 4 , 163);
00755 kio_ChangePWM_ratio(koreio, 5 , 170);
00756 kio_ChangePWM_ratio(koreio, 6 , 80);
00757 kio_ChangePWM_ratio(koreio, 7 , 170);
00758
00759
00760
00761 while (!quitReq) {
00762 printf("\n> ");
00763
00764 if ( fgets( buf , sizeof(buf) , stdin ) != NULL ) {
00765 buf[strlen(buf)-1] = '\0';
00766 kb_parse_command( buf , cmds , NULL);
00767 }
00768 }
00769
00770 knet_close( koreio );
00771 }