00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include "korebot.h"
00012
00013
00028 char * koa_get_nextValue(char * current, char * value)
00029 {
00030 while(*current != ',' && *current != '\n')
00031 {
00032 *value = *current;
00033 value++;
00034 current++;
00035 }
00036
00037 if(*current == '\n')
00038 return NULL;
00039 else
00040 {
00041 *value = '\0';
00042 return current+1;
00043 }
00044 }
00045
00046
00051 int koa_getOSVersion( knet_dev_t * dev ,
00052 unsigned char *version ,
00053 unsigned char *revision,
00054 unsigned char *pversion,
00055 unsigned char *prevision)
00056 {
00057 char *p;
00058 char buf[16];
00059 int rc;
00060
00061 if ((rc=knet_printf( dev , "B\r\n")) < 0 )
00062 return rc;
00063
00064 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00065 return rc;
00066
00067
00068 if ( buf[0] != 'b' )
00069 return 0;
00070
00071 p = &buf[2];
00072
00073 if ( version != NULL ) {
00074 *version = atoi(p);
00075 }
00076
00077 if ( revision != NULL ) {
00078
00079 while (*p!='\0' && *p!='.' ) p++;
00080 if (*p == '.' ) {
00081 p++;
00082 *revision = atoi(p);
00083 }
00084 }
00085
00086 if ( pversion != NULL ) {
00087
00088 while (*p!='\0' && *p!=',' ) p++;
00089 if (*p == ',' ) {
00090 p++;
00091 *pversion = atoi(p);
00092 }
00093 }
00094
00095 if ( prevision != NULL ) {
00096
00097 while (*p!='\0' && *p!='.' ) p++;
00098 if (*p == '.' ) {
00099 p++;
00100 *prevision = atoi(p);
00101 }
00102 }
00103
00104 return 0;
00105 }
00106
00107
00118 int koa_setPidPosition(knet_dev_t * dev, int Kp, int Ki, int Kd)
00119 {
00120 char buf[32];
00121 int rc;
00122
00123 if ((rc=knet_printf( dev , "F,%d,%d,%d\r\n" , Kp, Ki, Kd)) < 0 )
00124 return rc;
00125
00126 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00127 return rc;
00128
00129
00130 if ( buf[0] != 'f' )
00131 return 0;
00132
00133 return 1;
00134 }
00135
00136
00146 int koa_setPositionCounter(knet_dev_t * dev, long pos_left, long pos_right)
00147 {
00148 char buf[32];
00149 int rc;
00150
00151 if ((rc=knet_printf( dev , "G,%d,%d\r\n" , pos_left, pos_right)) < 0 )
00152 return rc;
00153
00154 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00155 return rc;
00156
00157
00158 if ( buf[0] != 'g' )
00159 return 0;
00160
00161 return 1;
00162 }
00163
00164
00175 int koa_setPidSpeed(knet_dev_t * dev, int Kp, int Ki, int Kd)
00176 {
00177 char buf[32];
00178 int rc;
00179
00180 if ((rc=knet_printf( dev , "A,%d,%d,%d\r\n" , Kp, Ki, Kd)) < 0 )
00181 return rc;
00182
00183 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00184 return rc;
00185
00186
00187 if ( buf[0] != 'a' )
00188 return 0;
00189
00190 return 1;
00191 }
00192
00193
00212 int koa_setPosition(knet_dev_t * dev, long pos_left, long pos_right)
00213 {
00214 char buf[32];
00215 int rc;
00216
00217 if ((rc=knet_printf( dev , "C,%d,%d\r\n" , pos_left, pos_right)) < 0 )
00218 return rc;
00219
00220 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00221 return rc;
00222
00223
00224 if ( buf[0] != 'c' )
00225 return 0;
00226
00227 return 1;
00228 }
00229
00230
00242 int koa_readSpeed(knet_dev_t * dev, int * speedtable)
00243 {
00244 char buf[256];
00245 char val[8];
00246 char * scan;
00247 int rc;
00248
00249 if ((rc=knet_printf( dev , "E\r\n" )) < 0 )
00250 return rc;
00251
00252 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00253 return rc;
00254
00255
00256 if ( buf[0] != 'e' )
00257 return 0;
00258
00259 scan = buf+2;
00260 while(scan) {
00261 scan = koa_get_nextValue(scan,val);
00262 *speedtable= atoi(val);
00263 speedtable++;
00264 }
00265
00266 return 1;
00267 }
00268
00269
00280 int koa_readPosition(knet_dev_t * dev, int * postable)
00281 {
00282 char buf[256];
00283 char val[8];
00284 char * scan;
00285 int rc;
00286
00287 if ((rc=knet_printf( dev , "H\r\n" )) < 0 )
00288 return rc;
00289
00290 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00291 return rc;
00292
00293
00294 if ( buf[0] != 'h' )
00295 return 0;
00296
00297 scan = buf+2;
00298 while(scan) {
00299 scan = koa_get_nextValue(scan,val);
00300 *postable = atoi(val);
00301 postable++;
00302 }
00303
00304 return 1;
00305 }
00306
00307
00315 int koa_readInput(knet_dev_t * dev, int channel)
00316 {
00317 char buf[256];
00318 char val[8];
00319 char * scan;
00320 int rc;
00321
00322 if ((rc=knet_printf( dev , "Y,%d\r\n", channel)) < 0 )
00323 return rc;
00324
00325 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00326 return rc;
00327
00328
00329 if ( buf[0] != 'y' )
00330 return -1;
00331
00332 scan = buf+2;
00333 scan = koa_get_nextValue(scan,val);
00334 return atoi(val);
00335 }
00336
00337
00346 int koa_readAD(knet_dev_t * dev, int channel)
00347 {
00348 char buf[256];
00349 char val[8];
00350 char * scan;
00351 int rc;
00352
00353 if ((rc=knet_printf( dev , "I,%d\r\n", channel)) < 0 )
00354 return rc;
00355
00356 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00357 return rc;
00358
00359
00360 if ( buf[0] != 'i' )
00361 return -1;
00362
00363 scan = buf+2;
00364 scan = koa_get_nextValue(scan,val);
00365 return atoi(val);
00366 }
00367
00368
00384 int koa_setProfile( knet_dev_t * dev, int max_speed_left, int acc_left, int max_speed_right, int acc_right)
00385 {
00386 char buf[32];
00387 int rc;
00388
00389 if ((rc=knet_printf( dev ,"J,%d,%d,%d,%d\r\n",max_speed_left,acc_left,max_speed_right,acc_right)) < 0 )
00390 return rc;
00391
00392 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00393 return rc;
00394
00395
00396 if ( buf[0] != 'j' )
00397 return 0;
00398
00399 return 1;
00400 }
00401
00402
00414 int koa_readStatus(knet_dev_t * dev, int * statustable)
00415 {
00416 char buf[256];
00417 char val[8];
00418 char * scan;
00419 int rc;
00420
00421 if ((rc=knet_printf( dev , "K\r\n" )) < 0 )
00422 return rc;
00423
00424 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00425 return rc;
00426
00427
00428 if ( buf[0] != 'k' )
00429 return 0;
00430
00431 scan = buf+2;
00432 while(scan) {
00433 scan = koa_get_nextValue(scan,val);
00434 *statustable = atoi(val);
00435 statustable++;
00436 }
00437
00438 return 1;
00439 }
00440
00441
00451 int koa_changeLed( knet_dev_t * dev , short int led, short int action )
00452 {
00453 char buf[32];
00454 int rc;
00455
00456 if ((rc=knet_printf( dev , "L,%d,%d\r\n" , led , action)) < 0 )
00457 return rc;
00458
00459 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00460 return rc;
00461
00462
00463 if ( buf[0] != 'l' )
00464 return 0;
00465
00466 return 1;
00467 }
00468
00469
00470
00482 int koa_readSensor(knet_dev_t * dev, int sensor)
00483 {
00484 char buf[256];
00485 char val[8];
00486 char * scan;
00487 int rc;
00488
00489 if ((rc=knet_printf( dev , "I,%d\r\n", sensor)) < 0 )
00490 return rc;
00491
00492 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00493 return rc;
00494
00495
00496 if ( buf[0] != 'i' )
00497 return -1;
00498
00499 scan = buf+2;
00500 scan = koa_get_nextValue(scan,val);
00501 return atoi(val);
00502 }
00503
00504
00516 int koa_setSpeed( knet_dev_t * dev , short int left , short int right )
00517 {
00518 char buf[32];
00519 int rc;
00520
00521 if ((rc=knet_printf( dev , "D,%d,%d\r\n" , left , right )) < 0 )
00522 return rc;
00523
00524 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00525 return rc;
00526
00527
00528 if ( buf[0] != 'd' )
00529 return 0;
00530
00531 return 1;
00532 }
00533
00534
00543 int koa_readBattery(knet_dev_t * dev)
00544 {
00545 char buf[256];
00546 char val[8];
00547 char * scan;
00548 int rc;
00549
00550 if ((rc=knet_printf( dev , "S\r\n")) < 0 )
00551 return rc;
00552
00553 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00554 return rc;
00555
00556
00557 if ( buf[0] != 's' )
00558 return -1;
00559
00560 scan = buf+2;
00561 scan = koa_get_nextValue(scan,val);
00562 return atoi(val);
00563 }
00564
00565
00575 int koa_readAmbient(knet_dev_t * dev, int * senstable)
00576 {
00577 char buf[256];
00578 char val[8];
00579 char * scan;
00580 int rc;
00581
00582 if ((rc=knet_printf( dev , "O\r\n" )) < 0 )
00583 return rc;
00584
00585 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00586 return rc;
00587
00588
00589 if ( buf[0] != 'o' )
00590 return 0;
00591
00592 scan = buf+2;
00593 while(scan) {
00594 scan = koa_get_nextValue(scan,val);
00595 *senstable = atoi(val);
00596 senstable++;
00597 }
00598
00599 return 1;
00600 }
00601
00602
00614 int koa_setPWM( knet_dev_t * dev , int left , int right )
00615 {
00616 char buf[32];
00617 int rc;
00618
00619 if ((rc=knet_printf( dev , "P,%d,%d\r\n" , left , right )) < 0 )
00620 return rc;
00621
00622 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00623 return rc;
00624
00625
00626 if ( buf[0] != 'p' )
00627 return 0;
00628
00629 return 1;
00630 }
00631
00632
00645 int koa_setOutput(knet_dev_t * dev, int output, int action)
00646 {
00647 char buf[32];
00648 int rc;
00649
00650 if ((rc=knet_printf( dev , "Q,%d,%d\r\n" , output, action)) < 0 )
00651 return rc;
00652
00653 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00654 return rc;
00655
00656
00657 if ( buf[0] != 'q' )
00658 return 0;
00659
00660 return 1;
00661 }
00662
00663
00673 int koa_readProximity(knet_dev_t * dev, int * senstable)
00674 {
00675 char buf[256];
00676 char val[8];
00677 char * scan;
00678 int rc;
00679
00680 if ((rc=knet_printf( dev , "N\r\n" )) < 0 )
00681 return rc;
00682
00683 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00684 return rc;
00685
00686
00687 if ( buf[0] != 'n' )
00688 return 0;
00689
00690 scan = buf+2;
00691 while(scan) {
00692 scan = koa_get_nextValue(scan,val);
00693 *senstable = atoi(val);
00694 senstable++;
00695 }
00696
00697 return 1;
00698 }