00001
00016 #include "korebot.h"
00017
00018
00019
00023 static knet_dev_t *dspic;
00024 static knet_dev_t *mot1;
00025 static knet_dev_t *mot2;
00026
00027
00037 int kh3_init( void )
00038 {
00039 int rc;
00040
00041
00042
00043 if((rc = kb_init( 0 , NULL )) < 0 )
00044 {
00045
00046 KB_ERROR("kb_kh3_init",KB_ERROR_KH3KBINIT);
00047 return KH3_ERROR_KBINIT;
00048 }
00049
00050
00051
00052
00053 kb_gpio_init();
00054
00055
00056 kb_gpio_function(KNET_INT0,0);
00057
00058
00059 kb_gpio_dir(KNET_INT0, IN);
00060
00061 }
00062
00063
00064
00065
00066
00086 int kh3_getcommand( knet_dev_t *hDev, unsigned char *out )
00087 {
00088 char sizeMsg;
00089 int rc;
00090
00091 if( knet_llread( hDev, &sizeMsg, 1 ) == 1 )
00092 {
00093 rc = knet_llread( hDev, out, sizeMsg );
00094 if(rc == sizeMsg)
00095 return rc;
00096 else
00097 {
00098 KB_ERROR("knet_getCommand", KB_ERROR_KH3FRMSZERR, rc, sizeMsg);
00099 return KH3_ERROR_FRMSZ;
00100 }
00101 }
00102 else
00103 {
00104 KB_ERROR("knet_getCommand", KB_ERROR_KH3SZFMTERR);
00105 return KH3_ERROR_SZFMT;
00106 }
00107 }
00108
00109
00125 int kh3_sendcommand( knet_dev_t *hDev, unsigned char *in )
00126 {
00127 char sizeMsg;
00128
00129
00130 sizeMsg = in[0];
00131
00132 if( knet_llwrite( hDev, in, sizeMsg) == sizeMsg)
00133 return sizeMsg;
00134 else
00135 {
00136 KB_ERROR("knet_sendCommand", KB_ERROR_KH3FRMSNDERR);
00137 return KH3_ERROR_FRMSND;
00138 }
00139 }
00140
00141
00142
00152 int kh3_proximity_ir(char *outbuf, knet_dev_t *hDev)
00153 {
00154
00155 int rc , i;
00156
00157
00158
00159 char cmd[3] = { 2, 'N', 0};
00160
00161 if(hDev)
00162 {
00163
00164 kh3_sendcommand( hDev , cmd );
00165 #ifdef KH3_UNDER_16
00166
00167 usleep(K3_CMD_DELAY);
00168 while(!kb_gpio_get(KNET_INT0));
00169 #endif
00170 rc = kh3_getcommand( hDev, outbuf );
00171
00172 return rc;
00173
00174 }
00175
00176 return 0;
00177
00178 }
00179
00189 int kh3_ambiant_ir(char *outbuf, knet_dev_t *hDev)
00190 {
00191 int rc;
00192
00193
00194
00195 char cmd[3] = { 2, 'O', 0};
00196
00197 if(hDev)
00198 {
00199 kh3_sendcommand( hDev , cmd );
00200 #ifdef KH3_UNDER_16
00201
00202 usleep(K3_CMD_DELAY);
00203
00204 while(!kb_gpio_get(KNET_INT0));
00205 #endif
00206 rc = kh3_getcommand( hDev, outbuf );
00207
00208 return rc;
00209 }
00210
00211 return 0;
00212
00213 }
00214
00225 int kh3_battery_voltage(char *outbuf, unsigned char argument, knet_dev_t *hDev)
00226 {
00227 int rc;
00228
00229
00230
00231 char cmd[4] = { 3, 'V', 0, 0};
00232 cmd[2] = argument;
00233
00234 if(hDev)
00235 {
00236 kh3_sendcommand( hDev , cmd );
00237 #ifdef KH3_UNDER_16
00238
00239 usleep(K3_CMD_DELAY);
00240 while(!kb_gpio_get(KNET_INT0));
00241 #endif
00242
00243 rc = kh3_getcommand( hDev, outbuf );
00244 return rc;
00245 }
00246
00247 return 0;
00248
00249 }
00250
00262 int kh3_reset_tstamp(char *outbuf, knet_dev_t *hDev)
00263 {
00264 int rc;
00265
00266
00267
00268 char cmd[3] = { 2, 'Z', 0};
00269
00270 if(hDev)
00271 {
00272 kh3_sendcommand( hDev , cmd );
00273 #ifdef KH3_UNDER_16
00274
00275 usleep(K3_CMD_DELAY);
00276
00277 while(!kb_gpio_get(KNET_INT0));
00278 #endif
00279 rc = kh3_getcommand( hDev, outbuf );
00280
00281 return rc;
00282 }
00283
00284 return 0;
00285
00286 }
00287
00296 int kh3_revision(char *outbuf, knet_dev_t *hDev)
00297 {
00298 int rc;
00299
00300
00301
00302 char cmd[3] = { 2, 'B', 0};
00303
00304 if(hDev)
00305 {
00306 kh3_sendcommand( hDev , cmd );
00307 #ifdef KH3_UNDER_16
00308
00309 usleep(K3_CMD_DELAY);
00310
00311 while(!kb_gpio_get(KNET_INT0));
00312 #endif
00313 rc = kh3_getcommand( hDev, outbuf );
00314
00315 return rc;
00316 }
00317 return 0;
00318
00319 }
00320
00350 int kh3_configure_os(char *outbuf, unsigned char index, unsigned char value, knet_dev_t *hDev)
00351 {
00352 int rc;
00353
00354
00355
00356 char cmd[6] = { 5, 'C', 0, 0, 0, 0};
00357
00358 cmd[2] = index;
00359 cmd[4] = value;
00360
00361 if(hDev)
00362 {
00363 kh3_sendcommand( hDev , cmd );
00364 #ifdef KH3_UNDER_16
00365
00366 usleep(K3_CMD_DELAY);
00367
00368 while(!kb_gpio_get(KNET_INT0));
00369 #endif
00370 rc = kh3_getcommand( hDev, outbuf );
00371 return rc;
00372 }
00373 return 0;
00374
00375 }
00376
00386 int kh3_measure_us(char *outbuf, unsigned char usnbr, knet_dev_t *hDev)
00387 {
00388 int rc;
00389
00390
00391
00392 char cmd[4] = { 3, 'G', 0, 0};
00393
00394 cmd[2] = usnbr;
00395 if(hDev)
00396 {
00397 kh3_sendcommand( hDev , cmd );
00398 #ifdef KH3_UNDER_16
00399
00400 usleep(K3_CMD_DELAY);
00401
00402 while(!kb_gpio_get(KNET_INT0));
00403 #endif
00404 rc = kh3_getcommand( hDev, outbuf );
00405
00406 return rc;
00407 }
00408 return 0;
00409
00410 }