00001
00016 #include "korebot.h"
00017
00018
00019
00020
00021 #define PULSE_TO_MM_FIRMWARE_S_3 0.04629 // for version <3.0
00022 #define PULSE_TO_MM_FIRMWARE_BE_3 0.03068
00023
00024
00025 #define MM_S_TO_SPEED_FIRMWARE_S_3 144.01 // for version <3.0
00026 #define MM_S_TO_SPEED_FIRMWARE_BE_3 218.72
00027
00028
00029 double kh3_pulses_to_mm=PULSE_TO_MM_FIRMWARE_S_3 ;
00030 double kh3_mms_to_speed=MM_S_TO_SPEED_FIRMWARE_S_3 ;
00031
00032
00033
00043 int kh3_init( void )
00044 {
00045 int rc;
00046
00047
00048
00049 if((rc = kb_init( 0 , NULL )) < 0 )
00050 {
00051
00052 KB_ERROR("kb_kh3_init",KB_ERROR_KH3KBINIT);
00053 return KH3_ERROR_KBINIT;
00054 }
00055
00056
00057
00058 #ifdef KH3_UNDER_16
00059
00060 if ((rc=kb_gpio_init())<0)
00061 {
00062
00063 return KH3_ERROR_GPIO;
00064 }
00065
00066
00067
00068 kb_gpio_function(KNET_INT0,0);
00069
00070
00071 kb_gpio_dir(KNET_INT0, IN);
00072
00073 #endif
00074
00075 return KH3_ERROR_SUCCESS;
00076 }
00077
00078
00079
00099 int kh3_getcommand( knet_dev_t *hDev, unsigned char *out )
00100 {
00101 char sizeMsg;
00102 int rc;
00103
00104 if( knet_llread( hDev, &sizeMsg, 1 ) == 1 )
00105 {
00106 rc = knet_llread( hDev, out, sizeMsg );
00107 if(rc == sizeMsg)
00108 return rc;
00109 else
00110 {
00111 KB_ERROR("knet_getCommand", KB_ERROR_KH3FRMSZERR, rc, sizeMsg);
00112 return KH3_ERROR_FRMSZ;
00113 }
00114 }
00115 else
00116 {
00117 KB_ERROR("knet_getCommand", KB_ERROR_KH3SZFMTERR);
00118 return KH3_ERROR_SZFMT;
00119 }
00120 }
00121
00122
00138 int kh3_sendcommand( knet_dev_t *hDev, unsigned char *in )
00139 {
00140 char sizeMsg;
00141
00142
00143 sizeMsg = in[0];
00144
00145 if( knet_llwrite( hDev, in, sizeMsg) == sizeMsg)
00146 return sizeMsg;
00147 else
00148 {
00149 KB_ERROR("knet_sendCommand", KB_ERROR_KH3FRMSNDERR);
00150 return KH3_ERROR_FRMSND;
00151 }
00152 }
00153
00154
00155
00167 int kh3_proximity_ir(char *outbuf, knet_dev_t *hDev)
00168 {
00169
00170 int rc , i;
00171
00172
00173
00174 char cmd[3] = { 2, 'N', 0};
00175
00176 if(hDev)
00177 {
00178
00179 kh3_sendcommand( hDev , cmd );
00180 #ifdef KH3_UNDER_16
00181
00182 usleep(K3_CMD_DELAY);
00183 while(!kb_gpio_get(KNET_INT0));
00184 #endif
00185 rc = kh3_getcommand( hDev, outbuf );
00186
00187 return rc;
00188
00189 }
00190
00191 return 0;
00192
00193 }
00194
00206 int kh3_ambiant_ir(char *outbuf, knet_dev_t *hDev)
00207 {
00208 int rc;
00209
00210
00211
00212 char cmd[3] = { 2, 'O', 0};
00213
00214 if(hDev)
00215 {
00216 kh3_sendcommand( hDev , cmd );
00217 #ifdef KH3_UNDER_16
00218
00219 usleep(K3_CMD_DELAY);
00220
00221 while(!kb_gpio_get(KNET_INT0));
00222 #endif
00223 rc = kh3_getcommand( hDev, outbuf );
00224
00225 return rc;
00226 }
00227
00228 return 0;
00229
00230 }
00231
00244 int kh3_battery_voltage(char *outbuf, unsigned char argument, knet_dev_t *hDev)
00245 {
00246 int rc;
00247
00248
00249
00250 char cmd[4] = { 3, 'V', 0, 0};
00251 cmd[2] = argument;
00252
00253 if(hDev)
00254 {
00255 kh3_sendcommand( hDev , cmd );
00256 #ifdef KH3_UNDER_16
00257
00258 usleep(K3_CMD_DELAY);
00259 while(!kb_gpio_get(KNET_INT0));
00260 #endif
00261
00262 rc = kh3_getcommand( hDev, outbuf );
00263 return rc;
00264 }
00265
00266 return 0;
00267
00268 }
00269
00283 int kh3_reset_tstamp(char *outbuf, knet_dev_t *hDev)
00284 {
00285 int rc;
00286
00287
00288
00289 char cmd[3] = { 2, 'Z', 0};
00290
00291 if(hDev)
00292 {
00293 kh3_sendcommand( hDev , cmd );
00294 #ifdef KH3_UNDER_16
00295
00296 usleep(K3_CMD_DELAY);
00297
00298 while(!kb_gpio_get(KNET_INT0));
00299 #endif
00300 rc = kh3_getcommand( hDev, outbuf );
00301
00302 return rc;
00303 }
00304
00305 return 0;
00306
00307 }
00308
00319 int kh3_revision(char *outbuf, knet_dev_t *hDev)
00320 {
00321 int rc;
00322
00323
00324
00325 char cmd[3] = { 2, 'B', 0};
00326
00327 if(hDev)
00328 {
00329
00330
00331 kh3_sendcommand( hDev , cmd );
00332
00333
00334
00335 #ifdef KH3_UNDER_16
00336
00337 usleep(K3_CMD_DELAY);
00338
00339 while(!kb_gpio_get(KNET_INT0));
00340 #endif
00341
00342
00343
00344 rc = kh3_getcommand( hDev, outbuf );
00345
00346
00347 return rc;
00348
00349
00350 }
00351
00352 return 0;
00353
00354 }
00355
00387 int kh3_configure_os(char *outbuf, unsigned char index, unsigned char value, knet_dev_t *hDev)
00388 {
00389 int rc;
00390
00391
00392
00393 char cmd[6] = { 5, 'C', 0, 0, 0, 0};
00394
00395 cmd[2] = index;
00396 cmd[4] = value;
00397
00398 if(hDev)
00399 {
00400 kh3_sendcommand( hDev , cmd );
00401 #ifdef KH3_UNDER_16
00402
00403 usleep(K3_CMD_DELAY);
00404
00405 while(!kb_gpio_get(KNET_INT0));
00406 #endif
00407 rc = kh3_getcommand( hDev, outbuf );
00408 return rc;
00409 }
00410 return 0;
00411
00412 }
00413
00425 int kh3_measure_us(char *outbuf, unsigned char usnbr, knet_dev_t *hDev)
00426 {
00427 int rc;
00428
00429
00430
00431 char cmd[4] = { 3, 'G', 0, 0};
00432
00433 cmd[2] = usnbr;
00434 if(hDev)
00435 {
00436 kh3_sendcommand( hDev , cmd );
00437 #ifdef KH3_UNDER_16
00438
00439 usleep(K3_CMD_DELAY);
00440
00441 while(!kb_gpio_get(KNET_INT0));
00442 #endif
00443 rc = kh3_getcommand( hDev, outbuf );
00444
00445 return rc;
00446 }
00447 return 0;
00448
00449 }