00001
00014 #include <termios.h>
00015 #include <stdio.h>
00016 #include <signal.h>
00017 #include <korebot/korebot.h>
00018 #include <string.h>
00019 #include <math.h>
00020
00021 #define MAX_SENSOR_NUMBER 16
00022 #define RANGE (1024 / 2)
00023
00024 #define TIME_STEP 64
00025 #define RESET_BUT 65
00026
00027
00028 #undef DEBUG
00029
00030
00031 knet_dev_t * BTComm;
00032 knet_dev_t *koala;
00033 int fd;
00034 static int quitReq = 0;
00035 #define BAUDRATE B115200
00036
00037
00038
00039
00040
00041
00042 static int sensors[MAX_SENSOR_NUMBER];
00043
00044
00045
00046
00047
00048 static float matrix[16][2] =
00049 { {-1, 15}, {-1, 5}, {-3, 4}, {-7, 17}, {0, 0}, {0, 0}, {0, 0},
00050 {-10, -10}, {17, -1}, {5, -1}, {4,-3 }, {17, -7}, {0, 0}, {0, 0},
00051 {0, 0}, {-10, -10} };
00052
00053
00054
00055 static int sensor_number;
00056 static int range;
00057 static int phase;
00058 static int position;
00059 static int goal;
00060 static int oldEncoder[2];
00061 static float X,Y,Theta;
00062 static int Start;
00063
00064
00065 typedef struct {
00066
00067 pthread_t thread;
00068 pthread_mutex_t cmdWaitLock;
00069 pthread_cond_t cmdWait;
00070 int busy;
00071 int done;
00072 int state;
00073 }
00074 koala_t;
00075
00076 koala_t BTkoala_struct ;
00077
00078
00079
00080
00081 static void init(void)
00082 {
00083
00084 int i, j;
00085
00086
00087 range = RANGE;
00088 sensor_number = 16;
00089 #ifdef DEBUG
00090 printf("The koala robot is reset, it uses 16 sensors.\n\r");
00091 #endif
00092
00093 phase = 2;
00094
00095 position = 0;
00096 goal = 0;
00097 X=0; Y=0; Theta=0.0;
00098 oldEncoder[0]=0;oldEncoder[1]=0;
00099 koa_setPositionCounter(koala, 0, 0);
00100
00101 Start=1;
00102
00103 return;
00104 }
00105
00106
00107
00108 int cmd_received = 0;
00109
00110
00111 void * thread(void * arg)
00112 {
00113 koala_t *BT_koala = (koala_t *)arg;
00114
00115 int i, j, button;
00116 float speed[2];
00117 int leftspeed, rightspeed;
00118 unsigned short sensors_value[MAX_SENSOR_NUMBER];
00119 float encoder_temp[2];
00120 int encoder[2];
00121 int dSl, dSr;
00122 char sensors_val[6], pos_val[8];
00123 long left,right;
00124
00125 for(;;)
00126 {
00127
00128
00129
00130
00131
00132
00133
00134
00135 koa_readPosition(koala, encoder);
00136 encoder[0] = encoder[0] * 0.045;
00137 encoder[1] = encoder[1] * 0.045;
00138 #ifdef DEBUG
00139 printf("encoder = %d, %d\n\r", encoder[0], encoder[1]);
00140 #endif
00141
00142
00143 dSl=encoder[0]-oldEncoder[0];
00144 dSr=encoder[1]-oldEncoder[1];
00145
00146
00147 X+= (dSr+dSl)*cos(Theta)/2;
00148 Y+= (dSr+dSl)*sin(Theta)/2;
00149 Theta+=(dSr-dSl) /320.0;
00150
00151
00152
00153 Theta=fmod(Theta,2*M_PI);
00154 if (Theta > M_PI)
00155 Theta=Theta-2*M_PI;
00156 if (Theta < -M_PI)
00157 Theta=Theta+2*M_PI;
00158
00159
00160 oldEncoder[0]=encoder[0];
00161 oldEncoder[1]=encoder[1];
00162
00163
00164
00165
00166 koa_readProximity(koala, sensors);
00167
00168 if( kb_gpio_get(RESET_BUT))
00169 {
00170
00171
00172
00173 if (phase==0) {
00174 position = 0;
00175
00176
00177
00178
00179
00180
00181 for (i = 0; i < 2; i++) {
00182 speed[i] = 20;
00183
00184 for (j = 0; j < sensor_number; j++) {
00185
00186
00187
00188
00189
00190
00191 speed[i] += matrix[j][i] *
00192 (1 - ((float) sensors[j] / range));
00193 }
00194 }
00195
00196 if ((sensors[0] + sensors[1] + sensors[2]+ sensors[3] +
00197 sensors[8] + sensors[9] + sensors[10] + sensors[11] )>3500) {
00198 speed[0] = 0;
00199 speed[1] = 0;
00200 #ifdef DEBUG
00201 printf("STOP phase. sens:%d,%d,%d,%d,%d,%d\n",
00202 sensors[0],sensors[1],sensors[2],
00203 sensors[8],sensors[9],sensors[10]);
00204 printf("position: %d,%d,%d\n", (int)X, (int)Y, (int)((Theta/3.14)*180));
00205 #endif
00206 phase=1;
00207 if (X<200 && X>-200 && Y<200 && Y>-200) {
00208 #ifdef DEBUG
00209 write(fd,"Back at START\n", 14);
00210 #endif
00211 Start=1;
00212
00213
00214
00215 X=X*0.5;
00216
00217
00218 Y=Y*0.5;
00219
00220
00221 Theta=Theta*0.5;
00222
00223 if(goal)
00224 {
00225 phase = 2;
00226 goal = 0;
00227 position = 1;
00228 }
00229 }
00230 }
00231 }
00232
00233
00234
00235 else if (phase == 1) {
00236 position = 0;
00237 if (Start) {
00238 speed[0] = 40;speed[1] = -40;
00239 }
00240 else {
00241 speed[0] = -40;speed[1] = 40;
00242 }
00243
00244 if ((sensors[0] + sensors[1] + sensors[2] +
00245 sensors[8] + sensors[9] + sensors[10])<1400)
00246 {
00247 phase = 0;
00248 Start = 0;
00249 speed[0] = 0;
00250 speed[1] = 0;
00251 }
00252 }
00253
00254
00255
00256 else if (phase==2) {
00257 Start=0;
00258 speed[0] = 0;speed[1] = 0;
00259 }
00260
00261
00262
00263
00264 else if(phase<63)
00265 {
00266 phase++;
00267 speed[0] = 0;
00268 speed[1] = 0;
00269 }
00270
00271
00272
00273 else if(phase >= 63)
00274 phase = 0;
00275 }
00276
00277
00278
00279 else
00280 {
00281 speed[0] = 0;
00282 speed[1] = 0;
00283 goal = 0;
00284 position = 0;
00285 phase = 30;
00286 X=0; Y=0; Theta=0.0;
00287 oldEncoder[0]=0;oldEncoder[1]=0;
00288 koa_setPositionCounter(koala, 0, 0);
00289 Start=1;
00290 }
00291
00292 leftspeed = speed[0];
00293 rightspeed = speed[1];
00294 #ifdef DEBUG
00295 printf("left %d, right %d\n\r", leftspeed, rightspeed);
00296
00297 write(fd,"\n\rPos: ",6);
00298 sprintf(pos_val, ",%6d", (int)X );
00299 write(fd,pos_val,7);
00300 sprintf(pos_val, ",%6d", (int)Y );
00301 write(fd,pos_val,7);
00302
00303 sprintf(pos_val, ",%6d", (int)(Theta*180/M_PI) );
00304 write(fd,pos_val,7);
00305
00306 write(fd,"\n\r",2);
00307 #endif
00308
00309 koa_setSpeed(koala, leftspeed, rightspeed);
00310
00311
00312
00313
00314 usleep(64000);
00315 }
00316
00317 }
00318
00319
00320
00321
00322
00325 int thread_create( koala_t *koala )
00326 {
00327 int i;
00328
00329 pthread_mutex_init( &koala->cmdWaitLock , NULL );
00330 pthread_cond_init( &koala->cmdWait , NULL );
00331
00332 koala->busy = 0;
00333 koala->done = 0;
00334 koala->state = 0;
00335
00336 if (pthread_create( &koala->thread , NULL , thread , koala ))
00337 return -1;
00338
00339 return 0;
00340 }
00341
00342
00343
00344
00345
00348 int quit( int argc , char * argv[] , void * data)
00349 {
00350 write(fd,"q\n\r", 3);
00351 koa_setSpeed(koala,0,0);
00352 quitReq = 1;
00353 return 0;
00354 }
00355
00356
00357
00360 int start( int argc , char * argv[] , void * data)
00361 {
00362 phase = 0;
00363 goal = 0;
00364 write(fd,"r\n\r", 3);
00365 return 0;
00366 }
00367
00368
00371 int stop( int argc , char * argv[] , void * data)
00372 {
00373 phase = 2;
00374 write(fd,"s\n\r", 3);
00375
00376 return 0;
00377 }
00378
00379
00382 int reset( int argc , char * argv[] , void * data)
00383 {
00384
00385 phase = 50;
00386 X=0; Y=0; Theta=0.0;
00387 oldEncoder[0]=0;oldEncoder[1]=0;
00388 koa_setPositionCounter(koala, 0, 0);
00389 Start=1;
00390
00391 position = 0;
00392 goal = 0;
00393 write(fd,"i\n\r", 3);
00394
00395 return 0;
00396 }
00397
00398
00401 int gotogoal( int argc , char * argv[] , void * data)
00402 {
00403 phase = 0;
00404 goal = 1;
00405 write(fd,"g\n\r", 3);
00406
00407 if(position)
00408 {
00409 phase = 1;
00410 }
00411 return 0;
00412 }
00413
00414
00417 int askposition( int argc , char * argv[] , void * data)
00418 {
00419 write(fd,"p,", 2);
00420 if(position)
00421 write(fd,"1\r\n",3);
00422 else
00423 write(fd,"0\r\n",3);
00424 return 0;
00425 }
00426
00427
00428 void handle_kill(int arg)
00429 {
00430 fprintf(stderr, "arg... killed\r\n");
00431
00432 koa_setSpeed(koala,0,0);
00433
00434 exit(1);
00435 }
00436
00437
00438
00439 static kb_command_t cmds[] = {
00440 { "R" , 0 , 0 , start },
00441 { "Q" , 0 , 0 , quit },
00442 { "S" , 0 , 0 , stop },
00443 { "I" , 0 , 0 , reset },
00444 { "G" , 0 , 0 , gotogoal },
00445 { "P" , 0 , 0 , askposition },
00446 { NULL , 0 , 0 , NULL }
00447 };
00448
00449
00450
00453 int help( int argc , char * argv[] , void * data)
00454 {
00455 kb_command_t * scan = cmds;
00456 while(scan->name != NULL)
00457 {
00458 write(fd,"TEST\n\r", 6);
00459
00460 scan++;
00461 }
00462 return 0;
00463 }
00464
00465
00466
00467 int main( int argc , char * argv[] )
00468 {
00469 static char buf[1024];
00470 char command[40];
00471 int rc;
00472 unsigned char a,b,c,d;
00473 int len, n;
00474 int res;
00475 struct termios oldtio,newtio;
00476
00477
00478
00479 signal(SIGINT, handle_kill);
00480
00481
00482 if(kb_init( argc , argv ) < 0 ){
00483 perror("global libkorebot init Problem \n\r");
00484 return -1;
00485 }
00486
00487
00488
00489 kb_set_debug_level(2);
00490
00491 if((rc = kb_init( argc , argv )) < 0 )
00492 return 1;
00493
00494 printf("Koala GoAutomation Program\r\n");
00495
00496
00497 koala = knet_open( "Koala:Robot", KNET_BUS_ANY, 0 , NULL );
00498 if(!koala)
00499 {
00500 printf("Open Koala device failed\r\n");
00501 return 1;
00502 }
00503
00504
00505 koa_getOSVersion(koala,&a,&b,&c,&d);
00506 printf("OS revision %d.%d protocol %d.%d\r\n",a,b,c,d);
00507 koa_setSpeed(koala,0,0);
00508
00509
00510
00511 fd = open("/dev/tts/2", O_RDWR | O_NOCTTY | O_NDELAY);
00512 if ( fd < 0 )
00513 {
00514 printf( "Unable to open /dev/tts/2 !" );
00515 return -1;
00516 }
00517
00518 fcntl(fd, F_SETFL, 0);
00519 tcgetattr(fd,&oldtio);
00520 bzero(&newtio, sizeof(newtio));
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531 newtio.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;
00532
00533
00534
00535
00536
00537
00538
00539 newtio.c_iflag = IGNPAR | ICRNL;
00540
00541
00542
00543
00544 newtio.c_oflag = 0;
00545
00546
00547
00548
00549
00550
00551 newtio.c_lflag = ICANON;
00552
00553
00554
00555
00556
00557
00558
00559 newtio.c_cc[VINTR] = 0;
00560 newtio.c_cc[VQUIT] = 0;
00561 newtio.c_cc[VERASE] = 0;
00562 newtio.c_cc[VKILL] = 0;
00563 newtio.c_cc[VEOF] = 4;
00564 newtio.c_cc[VTIME] = 0;
00565 newtio.c_cc[VMIN] = 1;
00566 newtio.c_cc[VSWTC] = 0;
00567 newtio.c_cc[VSTART] = 0;
00568 newtio.c_cc[VSTOP] = 0;
00569 newtio.c_cc[VSUSP] = 0;
00570 newtio.c_cc[VEOL] = 0;
00571 newtio.c_cc[VREPRINT] = 0;
00572 newtio.c_cc[VDISCARD] = 0;
00573 newtio.c_cc[VWERASE] = 0;
00574 newtio.c_cc[VLNEXT] = 0;
00575 newtio.c_cc[VEOL2] = 0;
00576
00577
00578
00579
00580
00581 tcflush(fd, TCIFLUSH);
00582 tcsetattr(fd,TCSANOW,&newtio);
00583
00584
00585 kb_gpio_init();
00586
00587
00588 kb_gpio_function(RESET_BUT ,0);
00589
00590
00591 kb_gpio_dir( RESET_BUT ,IN);
00592
00593
00594
00595
00596 init();
00597
00598
00599 thread_create(&BTkoala_struct);
00600
00601 while(!quitReq){
00602 res = read(fd,buf,255);
00603 buf[res-1]='\0';
00604
00605
00606 if (kb_parse_command(buf,cmds,NULL)!=0){
00607 write(fd,"ERROR\n\r", 7);
00608 }
00609
00610 }
00611
00612 tcsetattr(fd,TCSANOW,&oldtio);
00613 close(fd);
00614
00615
00616 }