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
00056 int koa_getOSVersion( knet_dev_t * dev ,
00057 unsigned char *version ,
00058 unsigned char *revision,
00059 unsigned char *pversion,
00060 unsigned char *prevision)
00061 {
00062 char *p;
00063 char buf[16];
00064 int rc;
00065
00066 if ((rc=knet_printf( dev , "B\r\n")) < 0 )
00067 return rc;
00068
00069 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00070 return rc;
00071
00072
00073 if ( buf[0] != 'b' )
00074 return 0;
00075
00076 p = &buf[2];
00077
00078 if ( version != NULL ) {
00079 *version = atoi(p);
00080 }
00081
00082 if ( revision != NULL ) {
00083
00084 while (*p!='\0' && *p!='.' ) p++;
00085 if (*p == '.' ) {
00086 p++;
00087 *revision = atoi(p);
00088 }
00089 }
00090
00091 if ( pversion != NULL ) {
00092
00093 while (*p!='\0' && *p!=',' ) p++;
00094 if (*p == ',' ) {
00095 p++;
00096 *pversion = atoi(p);
00097 }
00098 }
00099
00100 if ( prevision != NULL ) {
00101
00102 while (*p!='\0' && *p!='.' ) p++;
00103 if (*p == '.' ) {
00104 p++;
00105 *prevision = atoi(p);
00106 }
00107 }
00108
00109 return 0;
00110 }
00111
00112
00123 int koa_setPidPosition(knet_dev_t * dev, int Kp, int Ki, int Kd)
00124 {
00125 char buf[32];
00126 int rc;
00127
00128 if ((rc=knet_printf( dev , "F,%d,%d,%d\r\n" , Kp, Ki, Kd)) < 0 )
00129 return rc;
00130
00131 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00132 return rc;
00133
00134
00135 if ( buf[0] != 'f' )
00136 return 0;
00137
00138 return 1;
00139 }
00140
00141
00151 int koa_setPositionCounter(knet_dev_t * dev, long pos_left, long pos_right)
00152 {
00153 char buf[32];
00154 int rc;
00155
00156 if ((rc=knet_printf( dev , "G,%d,%d\r\n" , pos_left, pos_right)) < 0 )
00157 return rc;
00158
00159 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00160 return rc;
00161
00162
00163 if ( buf[0] != 'g' )
00164 return 0;
00165
00166 return 1;
00167 }
00168
00169
00180 int koa_setPidSpeed(knet_dev_t * dev, int Kp, int Ki, int Kd)
00181 {
00182 char buf[32];
00183 int rc;
00184
00185 if ((rc=knet_printf( dev , "A,%d,%d,%d\r\n" , Kp, Ki, Kd)) < 0 )
00186 return rc;
00187
00188 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00189 return rc;
00190
00191
00192 if ( buf[0] != 'a' )
00193 return 0;
00194
00195 return 1;
00196 }
00197
00198
00217 int koa_setPosition(knet_dev_t * dev, long pos_left, long pos_right)
00218 {
00219 char buf[32];
00220 int rc;
00221
00222 if ((rc=knet_printf( dev , "C,%d,%d\r\n" , pos_left, pos_right)) < 0 )
00223 return rc;
00224
00225 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00226 return rc;
00227
00228
00229 if ( buf[0] != 'c' )
00230 return 0;
00231
00232 return 1;
00233 }
00234
00235
00247 int koa_readSpeed(knet_dev_t * dev, int * speedtable)
00248 {
00249 char buf[256];
00250 char val[8];
00251 char * scan;
00252 int rc;
00253
00254 if ((rc=knet_printf( dev , "E\r\n" )) < 0 )
00255 return rc;
00256
00257 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00258 return rc;
00259
00260
00261 if ( buf[0] != 'e' )
00262 return 0;
00263
00264 scan = buf+2;
00265 while(scan) {
00266 scan = koa_get_nextValue(scan,val);
00267 *speedtable= atoi(val);
00268 speedtable++;
00269 }
00270
00271 return 1;
00272 }
00273
00274
00285 int koa_readPosition(knet_dev_t * dev, int * postable)
00286 {
00287 char buf[256];
00288 char val[8];
00289 char * scan;
00290 int rc;
00291
00292 if ((rc=knet_printf( dev , "H\r\n" )) < 0 )
00293 return rc;
00294
00295 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00296 return rc;
00297
00298
00299 if ( buf[0] != 'h' )
00300 return 0;
00301
00302 scan = buf+2;
00303 while(scan) {
00304 scan = koa_get_nextValue(scan,val);
00305 *postable = atoi(val);
00306 postable++;
00307 }
00308
00309 return 1;
00310 }
00311
00312
00320 int koa_readInput(knet_dev_t * dev, int channel)
00321 {
00322 char buf[256];
00323 char val[8];
00324 char * scan;
00325 int rc;
00326
00327 if ((rc=knet_printf( dev , "Y,%d\r\n", channel)) < 0 )
00328 return rc;
00329
00330 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00331 return rc;
00332
00333
00334 if ( buf[0] != 'y' )
00335 return -1;
00336
00337 scan = buf+2;
00338 scan = koa_get_nextValue(scan,val);
00339 return atoi(val);
00340 }
00341
00342
00351 int koa_readAD(knet_dev_t * dev, int channel)
00352 {
00353 char buf[256];
00354 char val[8];
00355 char * scan;
00356 int rc;
00357
00358 if ((rc=knet_printf( dev , "I,%d\r\n", channel)) < 0 )
00359 return rc;
00360
00361 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00362 return rc;
00363
00364
00365 if ( buf[0] != 'i' )
00366 return -1;
00367
00368 scan = buf+2;
00369 scan = koa_get_nextValue(scan,val);
00370 return atoi(val);
00371 }
00372
00373
00389 int koa_setProfile( knet_dev_t * dev, int max_speed_left, int acc_left, int max_speed_right, int acc_right)
00390 {
00391 char buf[32];
00392 int rc;
00393
00394 if ((rc=knet_printf( dev ,"J,%d,%d,%d,%d\r\n",max_speed_left,acc_left,max_speed_right,acc_right)) < 0 )
00395 return rc;
00396
00397 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00398 return rc;
00399
00400
00401 if ( buf[0] != 'j' )
00402 return 0;
00403
00404 return 1;
00405 }
00406
00407
00419 int koa_readStatus(knet_dev_t * dev, int * statustable)
00420 {
00421 char buf[256];
00422 char val[8];
00423 char * scan;
00424 int rc;
00425
00426 if ((rc=knet_printf( dev , "K\r\n" )) < 0 )
00427 return rc;
00428
00429 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00430 return rc;
00431
00432
00433 if ( buf[0] != 'k' )
00434 return 0;
00435
00436 scan = buf+2;
00437 while(scan) {
00438 scan = koa_get_nextValue(scan,val);
00439 *statustable = atoi(val);
00440 statustable++;
00441 }
00442
00443 return 1;
00444 }
00445
00446
00456 int koa_changeLed( knet_dev_t * dev , short int led, short int action )
00457 {
00458 char buf[32];
00459 int rc;
00460
00461 if ((rc=knet_printf( dev , "L,%d,%d\r\n" , led , action)) < 0 )
00462 return rc;
00463
00464 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00465 return rc;
00466
00467
00468 if ( buf[0] != 'l' )
00469 return 0;
00470
00471 return 1;
00472 }
00473
00474
00475
00487 int koa_readSensor(knet_dev_t * dev, int sensor)
00488 {
00489 char buf[256];
00490 char val[8];
00491 char * scan;
00492 int rc;
00493
00494 if ((rc=knet_printf( dev , "I,%d\r\n", sensor)) < 0 )
00495 return rc;
00496
00497 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00498 return rc;
00499
00500
00501 if ( buf[0] != 'i' )
00502 return -1;
00503
00504 scan = buf+2;
00505 scan = koa_get_nextValue(scan,val);
00506 return atoi(val);
00507 }
00508
00509
00521 int koa_setSpeed( knet_dev_t * dev , short int left , short int right )
00522 {
00523 char buf[32];
00524 int rc;
00525
00526 if ((rc=knet_printf( dev , "D,%d,%d\r\n" , left , right )) < 0 )
00527 return rc;
00528
00529 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00530 return rc;
00531
00532
00533 if ( buf[0] != 'd' )
00534 return 0;
00535
00536 return 1;
00537 }
00538
00539
00548 int koa_readBattery(knet_dev_t * dev)
00549 {
00550 char buf[256];
00551 char val[8];
00552 char * scan;
00553 int rc;
00554
00555 if ((rc=knet_printf( dev , "S\r\n")) < 0 )
00556 return rc;
00557
00558 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00559 return rc;
00560
00561
00562 if ( buf[0] != 's' )
00563 return -1;
00564
00565 scan = buf+2;
00566 scan = koa_get_nextValue(scan,val);
00567 return atoi(val);
00568 }
00569
00570
00580 int koa_readAmbient(knet_dev_t * dev, int * senstable)
00581 {
00582 char buf[256];
00583 char val[8];
00584 char * scan;
00585 int rc;
00586
00587 if ((rc=knet_printf( dev , "O\r\n" )) < 0 )
00588 return rc;
00589
00590 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00591 return rc;
00592
00593
00594 if ( buf[0] != 'o' )
00595 return 0;
00596
00597 scan = buf+2;
00598 while(scan) {
00599 scan = koa_get_nextValue(scan,val);
00600 *senstable = atoi(val);
00601 senstable++;
00602 }
00603
00604 return 1;
00605 }
00606
00607
00619 int koa_setPWM( knet_dev_t * dev , int left , int right )
00620 {
00621 char buf[32];
00622 int rc;
00623
00624 if ((rc=knet_printf( dev , "P,%d,%d\r\n" , left , right )) < 0 )
00625 return rc;
00626
00627 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00628 return rc;
00629
00630
00631 if ( buf[0] != 'p' )
00632 return 0;
00633
00634 return 1;
00635 }
00636
00637
00650 int koa_setOutput(knet_dev_t * dev, int output, int action)
00651 {
00652 char buf[32];
00653 int rc;
00654
00655 if ((rc=knet_printf( dev , "Q,%d,%d\r\n" , output, action)) < 0 )
00656 return rc;
00657
00658 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00659 return rc;
00660
00661
00662 if ( buf[0] != 'q' )
00663 return 0;
00664
00665 return 1;
00666 }
00667
00668
00678 int koa_readProximity(knet_dev_t * dev, int * senstable)
00679 {
00680 char buf[256];
00681 char val[8];
00682 char * scan;
00683 int rc;
00684
00685 if ((rc=knet_printf( dev , "N\r\n" )) < 0 )
00686 return rc;
00687
00688 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00689 return rc;
00690
00691
00692 if ( buf[0] != 'n' )
00693 return 0;
00694
00695 scan = buf+2;
00696 while(scan) {
00697 scan = koa_get_nextValue(scan,val);
00698 *senstable = atoi(val);
00699 senstable++;
00700 }
00701
00702 return 1;
00703 }