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
00038 int kh3_init( void )
00039 {
00040 int rc;
00041
00042
00043
00044 if((rc = kb_init( 0 , NULL )) < 0 )
00045 {
00046
00047 KB_ERROR("kb_kh3_init",KB_ERROR_KH3KBINIT);
00048 return KH3_ERROR_KBINIT;
00049 }
00050
00051
00052
00053
00054 kb_gpio_init();
00055
00056
00057 kb_gpio_function(KNET_INT0,0);
00058
00059
00060 kb_gpio_dir(KNET_INT0, IN);
00061
00062 }
00063
00064
00065
00066
00067
00087 int kh3_getcommand( knet_dev_t *hDev, unsigned char *out )
00088 {
00089 char sizeMsg;
00090 int rc;
00091
00092 if( knet_llread( hDev, &sizeMsg, 1 ) == 1 )
00093 {
00094 rc = knet_llread( hDev, out, sizeMsg );
00095 if(rc == sizeMsg)
00096 return rc;
00097 else
00098 {
00099 KB_ERROR("knet_getCommand", KB_ERROR_KH3FRMSZERR, rc, sizeMsg);
00100 return KH3_ERROR_FRMSZ;
00101 }
00102 }
00103 else
00104 {
00105 KB_ERROR("knet_getCommand", KB_ERROR_KH3SZFMTERR);
00106 return KH3_ERROR_SZFMT;
00107 }
00108 }
00109
00110
00126 int kh3_sendcommand( knet_dev_t *hDev, unsigned char *in )
00127 {
00128 char sizeMsg;
00129
00130
00131 sizeMsg = in[0];
00132
00133 if( knet_llwrite( hDev, in, sizeMsg) == sizeMsg)
00134 return sizeMsg;
00135 else
00136 {
00137 KB_ERROR("knet_sendCommand", KB_ERROR_KH3FRMSNDERR);
00138 return KH3_ERROR_FRMSND;
00139 }
00140 }
00141
00142
00143
00153 int kh3_proximity_ir(char *outbuf, knet_dev_t *hDev)
00154 {
00155
00156 int rc , i;
00157
00158
00159
00160 char cmd[3] = { 2, 'N', 0};
00161
00162 if(hDev)
00163 {
00164
00165 kh3_sendcommand( hDev , cmd );
00166 #ifdef KH3_UNDER_16
00167
00168 usleep(K3_CMD_DELAY);
00169 while(!kb_gpio_get(KNET_INT0));
00170 #endif
00171 rc = kh3_getcommand( hDev, outbuf );
00172
00173 return rc;
00174
00175 }
00176
00177 return 0;
00178
00179 }
00180
00190 int kh3_ambiant_ir(char *outbuf, knet_dev_t *hDev)
00191 {
00192 int rc;
00193
00194
00195
00196 char cmd[3] = { 2, 'O', 0};
00197
00198 if(hDev)
00199 {
00200 kh3_sendcommand( hDev , cmd );
00201 #ifdef KH3_UNDER_16
00202
00203 usleep(K3_CMD_DELAY);
00204
00205 while(!kb_gpio_get(KNET_INT0));
00206 #endif
00207 rc = kh3_getcommand( hDev, outbuf );
00208
00209 return rc;
00210 }
00211
00212 return 0;
00213
00214 }
00215
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 }