00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include "korebot.h"
00012
00027
00030 int hem_getOSVersion( knet_dev_t * dev ,
00031 unsigned char *version ,
00032 unsigned char *revision )
00033 {
00034 char *p;
00035 char buf[16];
00036 int rc;
00037
00038 if ((rc=knet_printf( dev , "B\r\n")) < 0 )
00039 return rc;
00040
00041 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00042 return rc;
00043
00044
00045 if ( buf[0] != 'b' )
00046 return 0;
00047
00048 p = &buf[2];
00049
00050 if ( version != NULL ) {
00051 *version = atoi(p);
00052 }
00053
00054 if ( revision != NULL ) {
00055
00056 while (*p!='\0' && *p!='.' ) p++;
00057 if (*p == '.' ) {
00058 p++;
00059 *revision = atoi(p);
00060 }
00061 }
00062 return 0;
00063 }
00064
00065
00076 int hem_setSpeed( knet_dev_t * dev , int left , int right )
00077 {
00078 char buf[32];
00079 int rc;
00080
00081 if ( left > 9 ) left = 9;
00082 if ( left < -9 ) left = -9;
00083 if ( right > 9 ) right = 9;
00084 if ( right < -9 ) right = -9;
00085
00086 if ((rc=knet_printf( dev , "D,%d,%d\r\n" , left , right )) < 0 )
00087 return rc;
00088
00089 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00090 return rc;
00091
00092
00093 if ( buf[0] != 'd' )
00094 return 0;
00095
00096 return 1;
00097 }
00098
00099
00102 int hem_beep( knet_dev_t * dev , unsigned int state )
00103 {
00104 char buf[16];
00105 int rc;
00106
00107 if ((rc = knet_printf( dev , "H,%u\r\n" , (state!=0?1:0))) < 0 )
00108 return rc;
00109
00110 if ((rc = knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00111 return rc;
00112
00113
00114 if ( buf[0] != 'h' )
00115 return 0;
00116
00117 return 1;
00118 }
00119
00120
00123 int hem_readSwitches( knet_dev_t * dev , unsigned int * state )
00124 {
00125 char buf[32];
00126 int rc;
00127 unsigned int val = 0;
00128
00129 if ((rc = knet_printf( dev , "I\r\n")) < 0 )
00130 return rc;
00131
00132 if (( rc = knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00133 return rc;
00134
00135
00136 if ( buf[0] != 'i' ) {
00137 return 0;
00138 }
00139
00140 if ( buf[2] == '1' ) {
00141 val |= hem_Switch1;
00142 }
00143
00144 if ( buf[4] == '1' ) {
00145 val |= hem_Switch2;
00146 }
00147
00148 if ( buf[6] == '1' ) {
00149 val |= hem_Switch3;
00150 }
00151
00152 if ( buf[8] == '1' ) {
00153 val |= hem_Switch4;
00154 }
00155
00156 if ( state != NULL ) {
00157 *state = val;
00158 }
00159
00160 return 1;
00161 }
00162
00163
00169 int hem_setLEDs( knet_dev_t * dev , unsigned int state )
00170 {
00171 unsigned char buf[16];
00172 int rc;
00173
00174 if ((rc=knet_printf( dev , "L,%u,%u,%u,%u\r\n" ,
00175 ((state & hem_LedOnOff)?1:0) ,
00176 ((state & hem_LedPgmExec)?1:0) ,
00177 ((state & hem_LedFrontLeft)?1:0) ,
00178 ((state & hem_LedFrontRight)?1:0) )) < 0 )
00179 return rc;
00180
00181 if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00182 return rc;
00183
00184 if ( buf[0] != 'l' )
00185 return 0;
00186
00187 return 1;
00188 }
00189
00190
00201 int hem_readProximitySensors( knet_dev_t * dev ,
00202 unsigned char * sensors )
00203 {
00204 unsigned char buf[64];
00205 char * p;
00206 int rc;
00207 int pos;
00208
00209 if ((rc = knet_printf( dev , "N\r\n")) < 0 )
00210 return rc;
00211
00212 if ((rc = knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00213 return rc;
00214
00215 if ( buf[0] != 'n' )
00216 return 0;
00217
00218 pos = 0;
00219
00220 for ( p = strtok(buf,",") ;
00221 p != NULL && pos < hem_SensorMax ;
00222 p = strtok(NULL,",")) {
00223
00224 sensors[pos++] = atoi(p);
00225 }
00226
00227 return 1;
00228 }
00229
00230
00241 int hem_readAmbientLightSensors( knet_dev_t * dev ,
00242 unsigned char * sensors )
00243 {
00244 unsigned char buf[64];
00245 char * p;
00246 int rc;
00247 int pos;
00248
00249 if ((rc = knet_printf( dev , "O\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 if ( buf[0] != 'o' )
00256 return 0;
00257
00258 pos = 0;
00259
00260 for ( p = strtok(buf,",") ;
00261 p != NULL && pos < hem_SensorMax ;
00262 p = strtok(NULL,",")) {
00263
00264 sensors[pos++] = atoi(p);
00265 }
00266
00267 return 1;
00268 }
00269
00270
00275 int hem_readTVRemote( knet_dev_t * dev ,
00276 unsigned char * value )
00277 {
00278 unsigned char buf[16];
00279 int rc;
00280
00281 if ((rc = knet_printf( dev , "T\r\n")) < 0 )
00282 return rc;
00283
00284 if ((rc = knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00285 return rc;
00286
00287 if ( buf[0] != 't' )
00288 return 0;
00289
00290
00291 if ( value != NULL ){
00292 *value = atoi( &buf[2] );
00293 }
00294
00295 return 1;
00296 }
00297
00298
00310 int hem_readI2C( knet_dev_t * dev ,
00311 unsigned char slave ,
00312 unsigned char reg ,
00313 unsigned char * data,
00314 unsigned char count )
00315 {
00316 unsigned char buf[ 130 ];
00317 int rc;
00318 unsigned int pos;
00319
00320 if ( count > (sizeof(buf)-3)/2 ) {
00321 count = (sizeof(buf)-3)/2;
00322 }
00323
00324 if ((rc=knet_printf( dev ,
00325 "R,%02X,%02X,%02X\r\n" ,
00326 slave , reg , count )) < 0 )
00327 return rc;
00328
00329 if ((rc=knet_read_string( dev ,
00330 buf , sizeof(buf-1) , '\n' )) < 0)
00331 return rc;
00332
00333 if ( buf[0] != 'r' )
00334 return 0;
00335
00336 for (pos=0; pos<count; pos++) {
00337 data[pos] = strtoul( &buf[2+pos*2] , NULL , 16 );
00338 }
00339
00340 return count;
00341 }
00342
00343
00347 int hem_writeI2C( knet_dev_t * dev ,
00348 unsigned char slave ,
00349 unsigned char reg ,
00350 unsigned char *data,
00351 unsigned char count )
00352 {
00353 unsigned char buf[16];
00354 int rc;
00355 unsigned int pos;
00356
00357 for (pos=0; pos<count; pos++) {
00358
00359 if ((rc=knet_printf( dev ,
00360 "W,%02X,%02X,%02X" ,
00361 slave ,
00362 (unsigned char)(reg+pos) ,
00363 data[pos] )) < 0)
00364 return rc;
00365
00366 if ((rc=knet_read_string( dev ,
00367 buf , sizeof(buf)-1 , '\n' )) < 0 )
00368 return rc;
00369
00370 if ( buf[0] != 'w' )
00371 return 0;
00372 }
00373
00374 return count;
00375 }
00376
00377
00397 int hem_fastRead( knet_dev_t * dev ,
00398 unsigned char *motorLeft ,
00399 unsigned char *motorRight ,
00400 unsigned char *proximitySensors ,
00401 unsigned char *ambientLightSensors ,
00402 unsigned char *switches ,
00403 unsigned char *tvRemoteData ,
00404 unsigned char *version ,
00405 unsigned char *revision )
00406 {
00407 unsigned char buf[64];
00408 int rc;
00409 unsigned int pos;
00410
00411 if ((rc = knet_printf( dev , "&\r\n" )) < 0 )
00412 return rc;
00413
00414 if ((rc=knet_llread( dev , buf , 25 )) < 0 )
00415 return rc;
00416
00417
00418 if ( buf[0] != '&' )
00419 return 0;
00420
00421 if ( motorLeft != NULL ) {
00422 *motorLeft = buf[1];
00423 }
00424
00425 if ( motorRight != NULL ) {
00426 *motorRight = buf[2];
00427 }
00428
00429 if ( proximitySensors != NULL ) {
00430 for (pos=0; pos<hem_SensorMax; pos++) {
00431 proximitySensors[pos] = buf[3+pos];
00432 }
00433 }
00434
00435 if ( ambientLightSensors != NULL ) {
00436 for (pos=0; pos<hem_SensorMax; pos++) {
00437 ambientLightSensors[pos] = buf[11+pos];
00438 }
00439 }
00440
00441 if ( switches != NULL ) {
00442 *switches = buf[19];
00443 }
00444
00445 if ( tvRemoteData != NULL ) {
00446 *tvRemoteData = buf[20];
00447 }
00448
00449 if ( version != NULL ) {
00450 *version = buf[21];
00451 }
00452
00453 if ( revision != NULL ) {
00454 *revision = buf[22];
00455 }
00456
00457 return 1;
00458 }
00459
00460
00470 int hem_fastWrite( knet_dev_t * dev ,
00471 unsigned char motorLeft ,
00472 unsigned char motorRight ,
00473 unsigned char ledState )
00474 {
00475 int rc;
00476 char buf[16];
00477
00478 if ((rc = knet_printf( dev , "*%c%c%c\r\n" ,
00479 motorLeft ,
00480 motorRight ,
00481 ledState )) < 0 )
00482 return rc;
00483
00484
00485 if ((rc = knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
00486 return rc;
00487
00488 if ( buf[0] != '*' )
00489 return 0;
00490
00491 return 1;
00492 }