00001
00016 #include <korebot/korebot.h>
00017 #include <signal.h>
00018 #include <limits.h>
00019
00020 #define MAXBUFFERSIZE 128
00021
00022
00023
00024
00025 #define BAUDRATE B115200
00026 #define VAL_BAUDRATE 115200
00027 #define SERIAL_PORT "/dev/ttyS1" // Bluetooth
00028
00029 enum {
00030 Timeout = 800,
00031 LineLength = 16 + 64 + 1 + 1 + 1,
00032 };
00033
00034 #define ERROR_CMD_CHAR '$'
00035
00036
00037 #define MAX_ARG 4
00038 #define ARG_LENGTH 10
00039 typedef char string[ARG_LENGTH];
00040
00041 static int porthandle=-1;
00042
00043 static int quitReq = 0;
00044 static char buf[1024];
00045
00046
00049 static knet_dev_t * dsPic;
00050 static knet_dev_t * mot1;
00051 static knet_dev_t * mot2;
00052
00053
00054 int process_command ();
00055
00056
00064 static void ctrlc_handler( int sig )
00065 {
00066 quitReq = 1;
00067
00068 }
00069
00070
00071
00079 int kbhit(void)
00080 {
00081 struct timeval tv;
00082 fd_set read_fd;
00083
00084 tv.tv_sec=0;
00085 tv.tv_usec=0;
00086 FD_ZERO(&read_fd);
00087
00088 FD_SET(0,&read_fd);
00089
00090 if(select(1, &read_fd, NULL, NULL,
00091 &tv) == -1)
00092 return -1;
00093
00094
00095
00096
00097 if(FD_ISSET(0,&read_fd))
00098 return 1;
00099
00100
00101
00102
00103 return 0;
00104
00105 }
00106
00107
00113 void clear_screen()
00114 {
00115 printf ("\33[2J" );
00116 }
00117
00118
00126 static int com_connect(const char* device, speed_t baudrate) {
00127
00128 struct termios options;
00129 int HComm=-1;
00130
00131 if ((HComm=open(device,O_RDWR | O_NOCTTY | O_NDELAY))<0) {
00132 printf("ERROR: can't open serial port %s\n",device);
00133 return -1;
00134 }
00135
00136 fcntl(HComm, F_SETFL, 0);
00137
00138
00139 tcgetattr(HComm, &options);
00140
00141
00142 options.c_cflag |= (CLOCAL | CREAD);
00143
00144
00145
00146 options.c_lflag = ICANON;
00147
00148
00149 options.c_oflag &= ~OPOST;
00150 options.c_cc[VMIN] = 1;
00151 options.c_cc[VTIME] = 0;
00152
00153
00154 cfsetispeed(&options, baudrate);
00155 cfsetospeed(&options, baudrate);
00156
00157
00158 tcsetattr(HComm, TCSANOW, &options);
00159
00160 return HComm;
00161 }
00162
00163
00164
00173 static int com_send(const char* data, int size) {
00174 int n;
00175
00176 tcflush (porthandle, TCIFLUSH);
00177 n=write( porthandle , data , size );
00178
00179 return n;
00180 }
00181
00182
00183
00193 static int com_recv(char* data, int max_size, int timeout) {
00194
00195 int filled = 0;
00196 int readable_size = 0;
00197 struct termios options;
00198
00199
00200
00201 memset (data, 0,max_size);
00202
00203 do {
00204
00205
00206
00207
00208 ioctl(porthandle, FIONREAD, &readable_size);
00209
00210 int read_n = (readable_size > max_size) ? max_size : readable_size;
00211
00212 int n;
00213
00214 n=read( porthandle , &data[filled] ,read_n );
00215 filled += n;
00216 readable_size -= n;
00217
00218 if (filled >= max_size) {
00219 return filled;
00220 }
00221 } while (readable_size > 0);
00222
00223
00224
00225
00226 if (timeout > 0) {
00227
00228
00229
00230 options.c_cc[VTIME] = timeout/100;
00231
00232
00233
00234
00235
00236
00237
00238 int n;
00239 while (1) {
00240
00241 n=read(porthandle, &data[filled], 1);
00242 if (n < 1) {
00243
00244 tcflush (porthandle, TCIFLUSH);
00245
00246 return filled;
00247 }
00248 filled += n;
00249 if (filled >= max_size) {
00250 return filled;
00251 }
00252
00253 }
00254 }
00255 }
00256
00257
00258
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00300 int initMot(knet_dev_t *hDev)
00301 {
00302 if(hDev)
00303 {
00304 kmot_SetMode( hDev , kMotModeIdle );
00305 kmot_SetSampleTime( hDev , 1550 );
00306 kmot_SetMargin( hDev , 6 );
00307 if(hDev == mot1)
00308 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk | kMotSWOptDirectionInv);
00309 else
00310 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00311 kmot_ResetError( hDev );
00312 kmot_SetBlockedTime( hDev , 10 );
00313 kmot_SetLimits( hDev , kMotRegCurrent , 0 , 500 );
00314 kmot_SetLimits( hDev , kMotRegPos , -10000 , 10000 );
00315
00316
00317 kmot_ConfigurePID( hDev , kMotRegSpeed , 620 , 3 , 10 );
00318 kmot_ConfigurePID( hDev,kMotRegPos,600,20,30);
00319 kmot_SetSpeedProfile(hDev,15000,10);
00320
00321 return 1;
00322 }
00323 else
00324 {
00325 printf("initMot error, handle cannot be null\r\n");
00326 return 0;
00327 }
00328 }
00329
00330
00331
00342 int initKH3( void )
00343 {
00344
00345 kh3_init();
00346
00347
00348 dsPic = knet_open( "Khepera3:dsPic" , KNET_BUS_I2C , 0 , NULL );
00349 mot1 = knet_open( "Khepera3:mot1" , KNET_BUS_I2C , 0 , NULL );
00350 mot2 = knet_open( "Khepera3:mot2" , KNET_BUS_I2C , 0 , NULL );
00351
00352 if(dsPic!=0)
00353 {
00354 if((mot1!=0)&&(mot2!=0))
00355 {
00356 initMot(mot1);
00357 initMot(mot2);
00358 return 0;
00359 }
00360 else
00361 return -1;
00362 }
00363
00364 return -2;
00365
00366 }
00367
00368
00378 int InitMot(int narg,string *larg)
00379 {
00380 int rc;
00381 char Buffer[MAXBUFFERSIZE];
00382
00383 if(dsPic)
00384 {
00385 if((mot1!=0)&&(mot2!=0))
00386 {
00387 initMot(mot1);
00388 initMot(mot2);
00389 sprintf(Buffer,"m\r\n");
00390 com_send(Buffer,strlen(Buffer));
00391 return 0;
00392 }
00393 }
00394
00395 return -1;
00396 }
00397
00398
00399
00409 int ReadProxSensors(int narg, string *larg)
00410 {
00411 char Buffer[MAXBUFFERSIZE];
00412 char BufferOut[MAXBUFFERSIZE];
00413 int sensor,i;
00414
00415
00416 sprintf(BufferOut,"n");
00417
00418 if(kh3_proximity_ir((char *)Buffer, dsPic))
00419 {
00420 for (i=0;i<11;i++)
00421 {
00422 sensor=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
00423 sprintf(BufferOut+strlen(BufferOut),",%d",sensor);
00424
00425 }
00426 sprintf(BufferOut+strlen(BufferOut),",%lu\r\n",((Buffer[23] | Buffer[24]<<8 ) | ( Buffer[25] | Buffer[26]<<8)<<16) );
00427 com_send(BufferOut, strlen(BufferOut));
00428
00429
00430 #ifdef DEBUG
00431 printf("\nReadProxSensors : %s\n",BufferOut);
00432 #endif
00433 return 0;
00434 }
00435
00436 return -1;
00437 }
00438
00439
00449 int ReadAmbSensors(int narg, string *larg)
00450 {
00451 char Buffer[MAXBUFFERSIZE];
00452 char BufferOut[MAXBUFFERSIZE];
00453 int sensor,i;
00454
00455
00456 sprintf(BufferOut,"o");
00457
00458 if(kh3_ambiant_ir((char *)Buffer, dsPic))
00459 {
00460 for (i=0;i<11;i++)
00461 {
00462 sensor=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
00463
00464 sprintf(BufferOut+strlen(BufferOut),",%d",sensor);
00465
00466 }
00467
00468 sprintf(BufferOut+strlen(BufferOut),",%lu\r\n",((Buffer[23] | Buffer[24]<<8 ) | ( Buffer[25] | Buffer[26]<<8)<<16));
00469 com_send(BufferOut, strlen(BufferOut));
00470 #ifdef DEBUG
00471 printf("\nReadAmbSensors : %s\n",BufferOut);
00472 #endif
00473 return 0;
00474 }
00475
00476 return -1;
00477 }
00478
00479
00489 int batStatus(int narg, string *larg)
00490 {
00491 char Buffer[MAXBUFFERSIZE];
00492 char BufferOut[MAXBUFFERSIZE];
00493 short argument;
00494
00495 if (narg != 1)
00496 {
00497 sprintf(Buffer,"%c\r\n",ERROR_CMD_CHAR);
00498 com_send(Buffer, strlen(Buffer));
00499 return -1;
00500 }
00501
00502 argument = atoi(larg[0]);
00503 if(kh3_battery_voltage((char *)Buffer, argument, dsPic)){
00504 switch(argument){
00505 case 0:
00506 sprintf(BufferOut,"v,%d,%d\r\n",(Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00507 break;
00508 case 1:
00509 if ((Buffer[2] & 0x80) != 0){
00510 Buffer[2] = Buffer[2] & 0x7F;
00511 sprintf(BufferOut,"v,-%d,%d\r\n",(Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00512 }
00513 else{
00514 sprintf(BufferOut,"v,%d,%d\r\n",(Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00515 }
00516 break;
00517 case 2:
00518 if ((Buffer[2] & 0x80) != 0){
00519 Buffer[2] = Buffer[2] & 0x7F;
00520
00521 sprintf(BufferOut,"v,-%d,%d\r\n",(Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00522 }
00523 else{
00524 sprintf(BufferOut,"v,%d,%d\r\n",(Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00525 }
00526 break;
00527 case 3:
00528 sprintf(BufferOut,"v,%3.3u\r\n",(Buffer[1] | Buffer[2]<<8));
00529 break;
00530 case 4:
00531 sprintf(BufferOut,"v,%d,%d\r\n",(Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00532 break;
00533 case 5:
00534 sprintf(BufferOut,"v,%d\r\n",(Buffer[1] | Buffer[2]<<8));
00535 break;
00536
00537
00538 default:
00539 sprintf(BufferOut,"%c\r\n",ERROR_CMD_CHAR);
00540
00541
00542 }
00543 com_send(BufferOut,strlen(BufferOut));
00544 return 0;
00545 }
00546
00547 return -1;
00548
00549
00550 }
00551
00552
00562 int tstampRST(int narg, string *larg)
00563 {
00564 char Buffer[MAXBUFFERSIZE];
00565
00566 if(kh3_reset_tstamp((char *)Buffer, dsPic))
00567 {
00568 sprintf(Buffer,"z\r\n");
00569 com_send(Buffer,strlen(Buffer));
00570
00571 return 0;
00572 }
00573
00574 return -1;
00575 }
00576
00577
00587 int revisionOS(int narg, string *larg)
00588 {
00589 char Buffer[MAXBUFFERSIZE];
00590 unsigned int ver ;
00591
00592
00593 if(kh3_revision((char *)Buffer, dsPic)){
00594
00595 sprintf(Buffer,"b,%d,%d\r\n",(Buffer[1] | Buffer[2]<<8),(Buffer[3] | Buffer[4]<<8));
00596 com_send(Buffer, strlen(Buffer));
00597
00598 return 0;
00599 }
00600
00601 return -1;
00602 }
00603
00604
00613 int configureOS( int argc, string * argv)
00614 {
00615 char Buffer[MAXBUFFERSIZE];
00616 short index;
00617 short value;
00618
00619
00620 index = atoi(argv[0]);
00621
00622 argv[1][0]=' ';
00623 value = atoi(argv[1]);
00624
00625
00626 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00627 {
00628 sprintf(Buffer,"c\r\n");
00629 com_send(Buffer, strlen(Buffer));
00630 return 0;
00631 }
00632
00633
00634
00635 return -1;
00636 }
00637
00638
00649 int GetUS( int argc, string * argv)
00650 {
00651 char Buffer[MAXBUFFERSIZE],buf[MAXBUFFERSIZE];
00652
00653 int i;
00654
00655
00656 if(kh3_measure_us((char *)Buffer, atoi(argv[0]), dsPic))
00657 {
00658 sprintf(buf,"g,%d",(Buffer[1] | Buffer[2]<<8));
00659
00660 for(i=0;i<5;i++)
00661 {
00662 sprintf(buf+strlen(buf),",%d,%d,%ld",(Buffer[3+8*i] | Buffer[4+8*i]<<8),(Buffer[5+8*i] | Buffer[6+8*i]<<8),(long int)(Buffer[7+8*i] | Buffer[8+8*i]<<8 | Buffer[9+8*i]<<16 | Buffer[10+8*i]<<24));
00663 }
00664
00665 sprintf(buf+strlen(buf),",%d\r\n",(Buffer[43] | Buffer[44]<<8));
00666 com_send(buf, strlen(buf));
00667
00668 return 0;
00669 }
00670
00671 return -1;
00672
00673 }
00674
00675
00676
00687 int SetSpeed( int argc, string *argv)
00688 {
00689 char Buffer[MAXBUFFERSIZE];
00690 if(mot1!=0 && mot2!=0)
00691 {
00692
00693 argv[0][0]=' ';
00694 argv[1][0]=' ';
00695
00696
00697 #ifdef DEBUG
00698 printf("\nSetSpeed : argv[0] %s %d argv[1] %s %d",argv[0],atoi(argv[0]),argv[1],atoi(argv[1]));
00699 #endif
00700
00701 kmot_SetPoint( mot1 , kMotRegSpeed , atol(argv[0]));
00702 kmot_SetPoint( mot2 , kMotRegSpeed , atol(argv[1]));
00703
00704 sprintf(Buffer,"d\r\n");
00705 com_send(Buffer, strlen(Buffer));
00706
00707 return 0;
00708 }
00709
00710 return -1;
00711 }
00712
00713
00724 int SetSpeedOpenLoop( int argc, string *argv)
00725 {
00726 char Buffer[MAXBUFFERSIZE];
00727 if(mot1!=0 && mot2!=0)
00728 {
00729
00730 argv[0][0]=' ';
00731 argv[1][0]=' ';
00732
00733 kmot_SetPoint( mot1 , kMotRegOpenLoop , atol(argv[0]));
00734 kmot_SetPoint( mot2 , kMotRegOpenLoop , atol(argv[1]));
00735
00736 sprintf(Buffer,"l\r\n");
00737 com_send(Buffer, strlen(Buffer));
00738
00739 return 0;
00740 }
00741
00742
00743 return -1;
00744 }
00745
00746
00747
00757 int GetSpeed( int argc, string *argv)
00758 {
00759 char Buffer[MAXBUFFERSIZE];
00760 int left,right;
00761
00762 if(mot1!=0 && mot2!=0)
00763 {
00764
00765
00766 left = kmot_GetMeasure( mot1 , kMotMesSpeed );
00767 right = kmot_GetMeasure( mot2 , kMotMesSpeed );
00768
00769 #ifdef DEBUG
00770 printf("\nGetSpeed : left %d right %d",left,right);
00771 #endif
00772
00773
00774 sprintf(Buffer,"e,%d,%d\r\n",left,right);
00775 com_send(Buffer, strlen(Buffer));
00776
00777 return 0;
00778 }
00779
00780
00781 return -1;
00782 }
00783
00784
00795 int SetTargetProfile( int argc, string *argv)
00796 {
00797 char Buffer[MAXBUFFERSIZE];
00798 if(mot1!=0 && mot2!=0)
00799 {
00800
00801 argv[0][0]=' ';
00802 argv[1][0]=' ';
00803 kmot_SetPoint( mot1 , kMotRegPosProfile, atol(argv[0]));
00804 kmot_SetPoint( mot2 , kMotRegPosProfile, atol(argv[1]));
00805
00806 sprintf(Buffer,"f\r\n");
00807 com_send(Buffer, strlen(Buffer));
00808 return 0;
00809 }
00810
00811
00812 return -1;
00813 }
00814
00815
00826 int SetTargetPosition( int argc, string *argv)
00827 {
00828 char Buffer[MAXBUFFERSIZE];
00829 if(mot1!=0 && mot2!=0)
00830 {
00831
00832 argv[0][0]=' ';
00833 argv[1][0]=' ';
00834
00835 #ifdef DEBUG
00836 printf("\nSetPosition : argv[0] %s %d argv[1] %s %d",argv[0],atoi(argv[0]),argv[1],atoi(argv[1]));
00837 #endif
00838
00839 kmot_SetPoint( mot1 , kMotRegPos , atol(argv[0]));
00840 kmot_SetPoint( mot2 , kMotRegPos, atol(argv[1]));
00841
00842 sprintf(Buffer,"p\r\n");
00843 com_send(Buffer, strlen(Buffer));
00844 return 0;
00845 }
00846
00847 return -1;
00848 }
00849
00850
00861 int SetEncPosition( int argc, string *argv)
00862 {
00863 char Buffer[MAXBUFFERSIZE];
00864
00865 if(mot1!=0 && mot2!=0)
00866 {
00867
00868 argv[0][0]=' ';
00869 argv[1][0]=' ';
00870
00871 #ifdef DEBUG
00872 printf("\nSetEncPosition : argv[0] str:%s nb:%ld | argv[1] str:%s nb:%ld",argv[0],atol(argv[0]),argv[1],atol(argv[1]));
00873 #endif
00874
00875 kmot_SetPosition( mot1 , atol(argv[0]));
00876 kmot_SetPosition( mot2 , atol(argv[1]));
00877
00878 sprintf(Buffer,"i\r\n");
00879 com_send(Buffer, strlen(Buffer));
00880 return 0;
00881 }
00882
00883 return -1;
00884 }
00885
00886
00897 int ReadPos( int argc, string *argv)
00898 {
00899 char Buffer[MAXBUFFERSIZE];
00900 int left,right;
00901
00902
00903 if(mot1!=0 && mot2!=0)
00904 {
00905
00906
00907 left = kmot_GetMeasure( mot1 , kMotMesPos );
00908 right = kmot_GetMeasure( mot2 , kMotMesPos );
00909
00910
00911 #ifdef DEBUG
00912 printf("\nReadPos : left %d right %d",left,right);
00913 #endif
00914
00915 sprintf(Buffer,"r,%d,%d\r\n",left,right);
00916 com_send(Buffer, strlen(Buffer));
00917
00918 return 0;
00919 }
00920
00921 return -1;
00922 }
00923
00924
00925
00938 int ConfigPID( int argc, string *argv)
00939 {
00940 char Buffer[MAXBUFFERSIZE];
00941 if(mot1!=0 && mot2!=0)
00942 {
00943
00944 argv[0][0]=' ';
00945 argv[1][0]=' ';
00946 argv[2][0]=' ';
00947 argv[3][0]=' ';
00948
00949 if (atoi(argv[0]))
00950 {
00951
00952 kmot_ConfigurePID( mot1,kMotRegPos,atoi(argv[1]),atoi(argv[3]),atoi(argv[2]));
00953 kmot_ConfigurePID( mot2,kMotRegPos,atoi(argv[1]),atoi(argv[3]),atoi(argv[2]));
00954 }
00955 else
00956 {
00957
00958 kmot_ConfigurePID( mot1,kMotRegSpeed,atoi(argv[1]),atoi(argv[3]),atoi(argv[2]));
00959 kmot_ConfigurePID( mot2,kMotRegSpeed,atoi(argv[1]),atoi(argv[3]),atoi(argv[2]));
00960 }
00961
00962
00963 sprintf(Buffer,"h\r\n");
00964 com_send(Buffer, strlen(Buffer));
00965 return 0;
00966 }
00967
00968 return -1;
00969 }
00970
00971
00981 int ConfigSpeedProfile( int argc, string *argv)
00982 {
00983 char Buffer[MAXBUFFERSIZE];
00984 if(mot1!=0 && mot2!=0)
00985 {
00986
00987 argv[0][0]=' ';
00988 argv[1][0]=' ';
00989 kmot_SetSpeedProfile(mot1,atoi(argv[0]),atoi(argv[1]));
00990 kmot_SetSpeedProfile(mot2,atoi(argv[0]),atoi(argv[1]));
00991
00992
00993 sprintf(Buffer,"j\r\n");
00994 com_send(Buffer, strlen(Buffer));
00995 return 0;
00996 }
00997
00998 return -1;
00999 }
01000
01001
01012 int SetLED(int argc,string * argv)
01013 {
01014 int rc;
01015 char buf[MAXBUFFERSIZE];
01016
01017
01018 char cmd[5] = { 4, 'K', 0, 0, 0};
01019 cmd[2] = (char)atoi(argv[0]);
01020 cmd[3] = (char)atoi(argv[1]);
01021
01022
01023
01024 if(dsPic)
01025 {
01026 kh3_sendcommand( dsPic , cmd );
01027 rc = kh3_getcommand( dsPic, buf );
01028
01029
01030 sprintf(buf,"k\r\n");
01031 com_send(buf, strlen(buf));
01032
01033 #ifdef DEBUG
01034 printf("\nSetLED : number %d state %d",cmd[2],cmd[3]);
01035 #endif
01036
01037 return rc;
01038 }
01039
01040 return 0;
01041 }
01042
01043
01044
01045
01056 #define BR_IRGAIN 30
01057 #define fwSpeed 20
01058 #define MIN_SPEED 10
01059 #define RotSpeedL 50
01060 #define RotSpeedR -50
01061
01062 int braitenberg( int argc , string * argv)
01063 {
01064 int Connections_B[9] = { 2, -2, -4, -12, 5, 2, 2, 2, 4};
01065 int Connections_A[9] = { 2, 2, 2, 5, -12, -4, -2, 2, 4};
01066 int i, buflen, sensval;
01067 char buffer[MAXBUFFERSIZE];
01068 char * scan;
01069 long int lspeed16, rspeed16;
01070 int tabsens[9];
01071 int left_speed, right_speed;
01072 unsigned int immobility = 0;
01073 unsigned int prevpos_left, pos_left, prevpos_right, pos_right;
01074 u_int8_t valueLL,valueLH,valueHL,valueHH;
01075
01076 int readable_size=0;
01077 int out=0;
01078 char Buffer[MAXBUFFERSIZE];
01079
01080
01081 if (argc != 1)
01082 {
01083 sprintf(buf,"%c\r\n",ERROR_CMD_CHAR);
01084 com_send(buf, strlen(buf));
01085 return -1;
01086 }
01087
01088 if (atoi(argv[0]) == 2 )
01089 {
01090 sprintf(buf,"a\r\n");
01091 com_send(buf, strlen(buf));
01092 return 1;
01093 } else
01094 {
01095 if ((atoi(argv[0]) == 0) || (atoi(argv[0]) == 1) )
01096 {
01097
01098 }
01099 else
01100 {
01101 sprintf(buf,"%c\r\n",ERROR_CMD_CHAR);
01102 com_send(buf, strlen(buf));
01103 return -1;
01104 }
01105 }
01106
01107 sprintf(buf,"a\r\n");
01108 com_send(buf, strlen(buf));
01109
01110
01111 prevpos_left = kmot_GetMeasure( mot1 , kMotRegPos );
01112 prevpos_right = kmot_GetMeasure( mot2 , kMotRegPos );
01113
01114
01115 printf("\nBraitenberg mode; enter any serial command to stop\n");
01116
01117 fcntl(porthandle, F_SETFL, FNDELAY);
01118
01119 do
01120 {
01121 lspeed16 = 0; rspeed16 = 0;
01122
01123 #if 0
01124 kh3_sendcommand( dsPic, cmd );
01125 while(!kb_gpio_get(KNET_INT0));
01126
01127 buflen = knet_llread( dsPic, buffer, 30);
01128 #endif
01129
01130 kh3_proximity_ir((char *)buffer, dsPic);
01131
01132
01133 scan = buffer+3;
01134
01135
01136 for (i = 0; i < 9; i++)
01137 {
01138 sensval = *(scan) | (*(scan+1))<<8;
01139 if(sensval > 1000)
01140 tabsens[i] = 450;
01141 else if (sensval < 100)
01142 tabsens[i] = 0;
01143 else
01144 tabsens[i] = (sensval - 100) >> 1;
01145 scan = scan + 2;
01146 }
01147
01148 for (i = 0; i < 9; i++)
01149 {
01150 lspeed16 += Connections_A[i] * tabsens[i];
01151 rspeed16 += Connections_B[i] * tabsens[i];
01152 }
01153
01154 left_speed = ((lspeed16 / BR_IRGAIN) + fwSpeed);
01155 right_speed = ((rspeed16 / BR_IRGAIN) + fwSpeed);
01156
01157 if(left_speed > 0 && left_speed < MIN_SPEED)
01158 left_speed = MIN_SPEED;
01159 if(left_speed < 0 && left_speed > -MIN_SPEED)
01160 left_speed = -MIN_SPEED;
01161 if(right_speed > 0 && right_speed < MIN_SPEED)
01162 right_speed = MIN_SPEED;
01163 if(right_speed < 0 && right_speed > -MIN_SPEED)
01164 right_speed = -MIN_SPEED;
01165
01166
01167 #ifndef SHIFT_SPEED
01168 left_speed *= 256;
01169 right_speed *= 256;
01170 #endif
01171
01172
01173 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
01174 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
01175
01176
01177
01178 left_speed = kmot_GetMeasure( mot1 , kMotMesSpeed );
01179 right_speed = kmot_GetMeasure( mot2 , kMotMesSpeed );
01180
01181
01182 pos_left = kmot_GetMeasure( mot1 , kMotRegPos );
01183 pos_right = kmot_GetMeasure( mot2 , kMotRegPos );
01184
01185
01186 if((pos_left < (prevpos_left + 700)) && (pos_left > (prevpos_left -700)) && (pos_right < (prevpos_right + 700)) && (pos_right > (prevpos_right -700)))
01187 {
01188
01189
01190
01191 if(++immobility > 5)
01192 {
01193 left_speed = RotSpeedL;
01194 right_speed = RotSpeedR;
01195 #ifndef SHIFT_SPEED
01196 left_speed *= 256;
01197 right_speed *= 256;
01198 #endif
01199 kmot_SetPoint( mot1, kMotRegSpeed, left_speed);
01200 kmot_SetPoint( mot2, kMotRegSpeed, right_speed);
01201
01202 do{
01203 usleep(10);
01204 kh3_proximity_ir((char *)buffer, dsPic);
01205 }while (((buffer[7] | (buffer[8]<<8) ) >250) || ((buffer[9] | (buffer[10]<<8)) >250));
01206
01207 immobility = 0;
01208 prevpos_left = pos_left;
01209 prevpos_right = pos_right;
01210 }
01211 }
01212 else
01213 {
01214 immobility = 0;
01215 prevpos_left = pos_left;
01216 prevpos_right = pos_right;
01217 }
01218
01219 usleep(20000);
01220
01221
01222 ioctl(porthandle, FIONREAD, &readable_size);
01223
01224 if (readable_size>0)
01225 {
01226 out=process_command();
01227 }
01228
01229 } while(!out);
01230
01231
01232 kmot_SetMode( mot1 , kMotModeStopMotor );
01233 kmot_SetMode( mot2 , kMotModeStopMotor );
01234
01235 tcflush (porthandle, TCIFLUSH);
01236 fcntl(porthandle, F_SETFL, 0);
01237
01238 printf("Braitenberg mode exit\n");
01239
01240 return 0;
01241 }
01242
01243
01244 #define RS232_EOA ','
01245 #define EOL_TEST(data) (data == '\r' || data == '\n' || data == '\0')
01246
01247
01255 int getArgs(char *buf,string *larg)
01256 {
01257 int narg=0;
01258
01259 char delim[2]= {RS232_EOA , '\0'};
01260 char * pch;
01261
01262
01263
01264 pch = strtok (buf,delim);
01265 while (pch != NULL)
01266 {
01267
01268 strcpy(larg[narg],pch);
01269 pch = strtok (NULL, delim);
01270
01271 #ifdef DEBUG
01272 printf(", | arg nb %d arg val: %s",narg,larg[narg]);
01273 #endif
01274
01275 narg++;
01276
01277 if (narg == MAX_ARG)
01278 break;
01279 }
01280
01281
01282 return narg;
01283 }
01284
01285
01286
01296 int BinaryRead(int narg,string *larg) {
01297 char Buffer[MAXBUFFERSIZE],buf[MAXBUFFERSIZE];
01298 int i, data;
01299
01300
01301
01302 Buffer[0]='x';
01303
01304 kh3_proximity_ir((char *)buf, dsPic);
01305 for (i=0;i<22;i++)
01306 {
01307 Buffer[i+1]=buf[i+1];
01308 }
01309
01310
01311
01312 kh3_ambiant_ir((char *)buf, dsPic);
01313 for (i=0;i<22;i++)
01314 {
01315 Buffer[i+23]=buf[i+1];
01316 }
01317
01318
01319
01320
01321 data = kmot_GetMeasure( mot1 , kMotMesSpeed );
01322 Buffer[45]=(u_int8_t )(data & 0x00FF);
01323 Buffer[46]=(u_int8_t )((data>>8) & 0x00FF);
01324 Buffer[47]=(u_int8_t )((data>>16) & 0x00FF);
01325 Buffer[48]=(u_int8_t )((data>>24) & 0x00FF);
01326
01327 data = kmot_GetMeasure( mot2 , kMotMesSpeed );
01328 Buffer[49]=(u_int8_t )(data & 0x00FF);
01329 Buffer[50]=(u_int8_t )((data>>8) & 0x00FF);
01330 Buffer[51]=(u_int8_t )((data>>16) & 0x00FF);
01331 Buffer[52]=(u_int8_t )((data>>24) & 0x00FF);
01332
01333
01334
01335
01336
01337 data = kmot_GetMeasure( mot1 , kMotRegPos );
01338 Buffer[53]=(u_int8_t )(data & 0x00FF);
01339 Buffer[54]=(u_int8_t )((data>>8) & 0x00FF);
01340 Buffer[55]=(u_int8_t )((data>>16) & 0x00FF);
01341 Buffer[56]=(u_int8_t )((data>>24) & 0x00FF);
01342 data = kmot_GetMeasure( mot2 , kMotRegPos );
01343 Buffer[57]=(u_int8_t )(data & 0x00FF);
01344 Buffer[58]=(u_int8_t )((data>>8) & 0x00FF);
01345 Buffer[59]=(u_int8_t )((data>>16) & 0x00FF);
01346 Buffer[60]=(u_int8_t )((data>>24) & 0x00FF);
01347
01348
01349
01350
01351 Buffer[61]=buf[23];
01352 Buffer[62]=buf[24];
01353 Buffer[63]=buf[25];
01354 Buffer[64]=buf[26];
01355
01356 Buffer[65]='\n';
01357 Buffer[66]='\r';
01358 Buffer[67]='\0';
01359
01360 com_send(Buffer, 67);
01361
01362 #ifdef DEBUG
01363 printf("\nBinaryRead (hexa): ");
01364 for (i=0; i<67; i++)
01365 printf(" %02x",Buffer[i]);
01366 #endif
01367
01368
01369 return 0;
01370 }
01371
01372
01379 int process_command ()
01380 {
01381 char Buffer[MAXBUFFERSIZE];
01382 int narg;
01383
01384 int out=0;
01385
01386 string larg[MAX_ARG];
01387 char *bptr;
01388
01389
01390
01391 if ( readLine(Buffer) >0 )
01392 {
01393 #ifdef DEBUG
01394 printf("%c length %d |%s|",Buffer[0],strlen(Buffer),Buffer);
01395 #endif
01396
01397 if (strlen(Buffer)>2)
01398 {
01399
01400 bptr = Buffer + 2;
01401
01402 narg=getArgs(bptr,larg);
01403
01404 }
01405
01406 switch(Buffer[0])
01407 {
01408 case 'A':
01409 out=braitenberg(narg,larg);
01410 break;
01411 case 'B':
01412 revisionOS(narg,larg);
01413 break;
01414 case 'C':
01415 configureOS(narg,larg);
01416 break;
01417 case 'D':
01418 SetSpeed(narg,larg);
01419 break;
01420 case 'E':
01421 GetSpeed(narg,larg);
01422 break;
01423 case 'F':
01424 SetTargetProfile(narg,larg);
01425 break;
01426 case 'G':
01427 GetUS(narg,larg);
01428 break;
01429 case 'H':
01430 ConfigPID(narg,larg);
01431 break;
01432 case 'I':
01433 SetEncPosition(narg,larg);
01434 break;
01435 case 'J':
01436 ConfigSpeedProfile(narg,larg);
01437 break;
01438 case 'K':
01439 SetLED(narg,larg);
01440 break;
01441 case 'L':
01442 SetSpeedOpenLoop(narg,larg);
01443 break;
01444 case 'M':
01445 InitMot(narg,larg);
01446 break;
01447 case 'N':
01448 ReadProxSensors(narg,larg);
01449 break;
01450 case 'O':
01451 ReadAmbSensors(narg,larg);
01452 break;
01453 case 'P':
01454 SetTargetPosition(narg,larg);
01455 break;
01456 case 'R':
01457 ReadPos(narg,larg);
01458 break;
01459 case 'V':
01460 batStatus(narg,larg);
01461 break;
01462 case 'X':
01463 BinaryRead(narg,larg);
01464 break;
01465 case 'Z':
01466 tstampRST(narg,larg);
01467 break;
01468 default:
01469 sprintf(Buffer,"%c\r\n",ERROR_CMD_CHAR);
01470 com_send(Buffer, strlen(Buffer));
01471
01472 }
01473 }
01474
01475 return out;
01476 }
01477
01478
01479
01480
01481 int main( int argc, char *argv[])
01482 {
01483 char Buffer[MAXBUFFERSIZE];
01484
01485
01486
01487 printf("Khepera3 server program (C) K-Team S.A\r\n");
01488
01489
01490
01491
01492 if(!initKH3())
01493 {
01494 printf("Init ok...\r\n");
01495
01496 kh3_revision((char *)Buffer, dsPic);
01497 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
01498 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
01499 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
01500
01501
01502 if (argc == 2)
01503 {
01504 strcpy(Buffer,argv[1]);
01505 }
01506 else
01507 {
01508 strcpy(Buffer,SERIAL_PORT);
01509 }
01510
01511
01512 if ((porthandle=com_connect(Buffer,BAUDRATE))<0)
01513 {
01514 printf("\nError: Serial port %s could not be open!\n",Buffer);
01515 return -1;
01516 }
01517
01518 printf("\nParsing commands from serial port %s, baudrate %ld.\nPush CTRL-C for quitting!\n",Buffer,VAL_BAUDRATE);
01519
01520
01521 while (!quitReq)
01522 {
01523
01524
01525 #ifdef DEBUG
01526 printf("\n> ");
01527 #endif
01528 process_command();
01529
01530
01531 }
01532
01533 printf("Exiting...\r\n");
01534 }
01535 else
01536 printf("Fatal error, unable to initialize\r\n");
01537
01538
01539 if (porthandle>=0)
01540 close(porthandle);
01541
01542 return 0;
01543 }
01544
01545