00001
00020 #include <korebot/korebot.h>
00021 #include <math.h>
00022
00023
00024
00025 #define NB_COLUMNS_MAX 80
00026 #define NB_LINES_MAX 50
00027 #define KEYS_TEXT_EXT_LENGTH (158-NB_COLUMNS_MAX)
00028
00029
00030 #define DEFAULT_X_FACTOR (2.0*100.0)
00031 #define DEFAULT_Y_FACTOR (2.0*100.0)
00032 #define MAX_X_FACTOR (2.0*400.0)
00033 #define MAX_Y_FACTOR (2.0*400.0)
00034 #define MIN_X_FACTOR (2.0*20.0)
00035 #define MIN_Y_FACTOR (2.0*20.0)
00036
00037
00038 #define MEDIUM_SCALE 80
00039 #define BIGGEST_SCALE 240
00040
00041
00042 static double x_factor = DEFAULT_X_FACTOR;
00043 static double y_factor = DEFAULT_Y_FACTOR;
00044
00045
00046 #define ZOOM_FACTOR 1.25
00047
00048
00049
00050 #define MOVE_ANGLE_DIR_THRESH 10.0
00051 #define END_ANGLE_GOAL_THRESH 5.0
00052 #define END_POS_GOAL_THRESH 4.5
00053 #define FAST_SPEED_POS_THRESH 15.0
00054 #define MOVE_SLOW_SPEED 5000
00055 #define MOVE_FAST_SPEED 15000
00056 #define ROTATE_LOW_SPEED 2500
00057 #define ROTATE_HIGH_SPEED 5500
00058 #define ROTATE_ANGLE_TRESH 30.0
00059
00060
00061 #define ROUND(dbl) dbl >= 0.0 ? (int)(dbl + 0.5) : ((dbl - (double)(int)dbl) <= -0.5 ? (int)dbl : (int)(dbl - 0.5))
00062
00063
00064
00065 #define DEFAULT_SPEED 10000
00066 #define MAX_SPEED 43000
00067 #define MIN_SPEED 2000
00068 #define SPEED_FACTOR 1.25
00069
00070
00071 #define DATA_FILE_NAME "data_corr.csv"
00072
00073
00074
00075
00076
00077
00078 static knet_dev_t * mot1;
00079 static knet_dev_t * mot2;
00080 static knet_dev_t * dsPic;
00081
00082
00092 int initMot(knet_dev_t *hDev)
00093 {
00094 if(hDev)
00095 {
00096 kmot_SetMode( hDev , kMotModeIdle );
00097 kmot_SetSampleTime( hDev , 1550 );
00098 kmot_SetMargin( hDev , 6 );
00099 if(hDev == mot1)
00100 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk | kMotSWOptDirectionInv);
00101 else
00102 kmot_SetOptions( hDev , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00103 kmot_ResetError( hDev );
00104 kmot_SetBlockedTime( hDev , 10 );
00105 kmot_SetLimits( hDev , kMotRegCurrent , 0 , 500 );
00106 kmot_SetLimits( hDev , kMotRegPos , -10000 , 10000 );
00107
00108
00109 kmot_ConfigurePID( hDev , kMotRegSpeed , 620, 3 , 10 );
00110 kmot_ConfigurePID( hDev,kMotRegPos,600,20,30);
00111 kmot_SetSpeedProfile(hDev,15000,30);
00112
00113 return 1;
00114 }
00115 else
00116 {
00117 printf("initMot error, handle cannot be null\r\n");
00118 return 0;
00119 }
00120 }
00121
00122
00123
00134 int initKH3( void )
00135 {
00136
00137 kh3_init();
00138
00139
00140 dsPic = knet_open( "Khepera3:dsPic" , KNET_BUS_I2C , 0 , NULL );
00141 mot1 = knet_open( "Khepera3:mot1" , KNET_BUS_I2C , 0 , NULL );
00142 mot2 = knet_open( "Khepera3:mot2" , KNET_BUS_I2C , 0 , NULL );
00143
00144 if(dsPic!=0)
00145 {
00146 if((mot1!=0)&&(mot2!=0))
00147 {
00148 initMot(mot1);
00149 initMot(mot2);
00150 return 0;
00151 }
00152 else
00153 return -1;
00154 }
00155
00156 return -2;
00157
00158 }
00159
00160
00171 int set_Stargazer_parameters(int *nb_land,int *ref_land,int *land_type,int *land_mode )
00172 {
00173 int c,i,ret;
00174 char text[256];
00175
00176
00177
00178 printf("\nRead current Stargazer parameters:\n");
00179
00180 kb_gazer_get_landmark_number(&c);
00181 printf(" landmark number: %d\n",c);
00182 *nb_land=c;
00183
00184 kb_gazer_get_ref_id(&c);
00185 printf(" reference id : %d\n",c);
00186 *ref_land=c;
00187
00188 kb_gazer_get_landmark_type(&c);
00189 printf(" landmark type : %s\n",kb_gazer_landmark_types[c]);
00190 *land_type=c;
00191
00192 kb_gazer_get_landmark_mode(&c);
00193 printf(" landmark mode : %s\n",kb_gazer_landmark_modes[c]);
00194 *land_mode=c;
00195
00196 kb_gazer_get_height_fix_mode(&c);
00197 printf(" height fix mode: %s\n",kb_gazer_height_fix_modes[c]);
00198
00199
00200
00201 printf("\nModify Stargazer parameters above (n/y)? ");
00202 c=getchar();
00203 getchar();
00204 if ( c != 'y')
00205 {
00206 printf("\nSkipping Stargazer parameters modification step.\n");
00207 }
00208 else
00209 {
00210 printf("\nEnter the number of landmarks you use [1-4] (1= alone mode): ");
00211 c=getchar();
00212 getchar();
00213 *nb_land=c-'0';
00214 if (*nb_land<1 || *nb_land >4)
00215 {
00216 printf("ERROR: the numbers of landmark should be in [1..4]; 1 was used as default value!\n");
00217 *nb_land=1;
00218 }
00219 kb_gazer_set_landmark_number(*nb_land);
00220
00221 if (*nb_land > 1)
00222 {
00223 printf("\nEnter the initial landmark id number: ");
00224 fgets (text,256,stdin);
00225 i=strlen(text);
00226 if (i>0)
00227 {
00228 text[strlen(text)-1]='\0';
00229 }
00230 *ref_land=atoi(text);
00231 if (*ref_land == 0)
00232 {
00233 printf("ERROR for the initial landmark id number; 546 was used as default value!\n");
00234 *ref_land=546;
00235 }
00236 kb_gazer_set_ref_id(*ref_land);
00237 }
00238
00239
00240 text[0]='\0';
00241 for (i=0;i<NB_MARK_TYPES;i++)
00242 {
00243 sprintf(text+strlen(text),"%s: %d |",kb_gazer_landmark_types[i],i+1);
00244 }
00245
00246 printf("\nEnter the landmark type number | %s : ",text);
00247 c=getchar();
00248 getchar();
00249 *land_type=c-'0';
00250 if (*land_type<1 || *nb_land >6)
00251 {
00252 printf("ERROR: the landmark type should be in [1..6]; HLD1S was used as default value!\n");
00253 *land_type=HLD1S;
00254 } else
00255 {
00256 (*land_type)--;
00257 }
00258 kb_gazer_set_landmark_type(*land_type);
00259
00260 printf("\nWriting parameters values, please wait:\n");
00261 fflush(stdout);
00262
00263
00264 if((ret=kb_gazer_set_end_command())==0)
00265 printf(" first data written succesfully!\n");
00266 else
00267 printf("ERROR writting first data (error = %d)!\n",ret);
00268
00269
00270 printf("\nWriting other parameters values, please wait:\n");
00271 fflush(stdout);
00272
00273
00274 if (*nb_land>1)
00275 {
00276 kb_gazer_set_landmark_mode(MARK_MAP);
00277 }
00278 else
00279 {
00280 kb_gazer_set_landmark_mode(MARK_ALONE);
00281 }
00282
00283 kb_gazer_set_height_fix_mode(HEIGHT_FIX_NO);
00284
00285
00286
00287 if((ret=kb_gazer_set_end_command())==0)
00288 printf(" last data written succesfully!\n");
00289 else
00290 printf("ERROR writting last data (error = %d)!\n",ret);
00291
00292
00293
00294 printf("\nReread parameters:\n");
00295
00296 kb_gazer_get_landmark_number(&c);
00297 printf(" landmark number: %d\n",c);
00298 *nb_land=c;
00299
00300 kb_gazer_get_ref_id(&c);
00301 printf(" reference id : %d\n",c);
00302 *ref_land=c;
00303
00304 kb_gazer_get_landmark_type(&c);
00305 printf(" landmark type : %s\n",kb_gazer_landmark_types[c]);
00306 *land_type=c;
00307
00308 kb_gazer_get_landmark_mode(&c);
00309 printf(" landmark mode : %s\n",kb_gazer_landmark_modes[c]);
00310 *land_mode=c;
00311
00312 kb_gazer_get_height_fix_mode(&c);
00313 printf(" height fix mode: %s\n",kb_gazer_height_fix_modes[c]);
00314 }
00315 return 0;
00316 }
00317
00318
00319
00320
00327 double diff_angles(double a, double b) {
00328 double ret;
00329
00330 ret = a-b;
00331
00332 if (ret>180)
00333 ret-=360;
00334 else
00335 if (ret<-180)
00336 ret+=360;
00337
00338 return ret;
00339
00340 }
00341
00342
00349 double sum_angles(double a, double b) {
00350 double ret;
00351
00352 ret = a+b;
00353
00354 if (ret>180)
00355 ret-=360;
00356 else
00357 if (ret<-180)
00358 ret+=360;
00359 return ret;
00360
00361 }
00362
00363
00381 int goto_xya(double curr_x,double curr_y, double curr_a, double goal_x,double goal_y,double goal_a, double *dist2goal, double *angle2goal)
00382 {
00383 double dx,dy,da,d;
00384 int speed;
00385
00386 d=sqrt((curr_y-goal_y)*(curr_y-goal_y)+(curr_x-goal_x)*(curr_x-goal_x));
00387 *dist2goal=d;
00388
00389
00390 if ( d <END_POS_GOAL_THRESH)
00391 {
00392
00393
00394 da=diff_angles(goal_a,curr_a);
00395 *angle2goal=da;
00396
00397 if (fabs(da) <END_ANGLE_GOAL_THRESH)
00398 {
00399
00400 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
00401 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
00402 return 0;
00403 }
00404 else
00405 {
00406 if (fabs(da)>ROTATE_ANGLE_TRESH)
00407 {
00408 speed=ROTATE_HIGH_SPEED;
00409 }
00410 else
00411 {
00412 speed=ROTATE_LOW_SPEED;
00413 }
00414
00415
00416 if (da>0)
00417 {
00418
00419 kmot_SetPoint( mot1 , kMotRegSpeed , -speed );
00420 kmot_SetPoint( mot2 , kMotRegSpeed , speed );
00421 } else
00422 {
00423
00424 kmot_SetPoint( mot1 , kMotRegSpeed , speed );
00425 kmot_SetPoint( mot2 , kMotRegSpeed , -speed );
00426 }
00427 return 3;
00428 }
00429 }
00430
00431
00432
00433 if ((curr_x-goal_x) == 0)
00434 {
00435
00436 if (curr_y>goal_y)
00437 da = 180;
00438 else
00439 da = 0;
00440 }
00441 else
00442 if ((curr_y-goal_y) == 0)
00443 {
00444
00445 if (curr_x>goal_x)
00446 da = 90;
00447 else
00448 da = -90;
00449 }
00450 {
00451
00452 da=atan((goal_y-curr_y)/(goal_x-curr_x))*180.0/M_PI;
00453
00454
00455 if (curr_x>goal_x)
00456 {
00457 da= sum_angles(90,da);
00458 }
00459 else
00460 {
00461 da= sum_angles(270,da);
00462 }
00463
00464 }
00465
00466
00467 da = diff_angles(da,curr_a);
00468 *angle2goal=da;
00469
00470 if (fabs(da) > MOVE_ANGLE_DIR_THRESH)
00471 {
00472 if (fabs(da)>ROTATE_ANGLE_TRESH)
00473 {
00474 speed=ROTATE_HIGH_SPEED;
00475 }
00476 else
00477 {
00478 speed=ROTATE_LOW_SPEED;
00479 }
00480
00481
00482 if (da>0)
00483 {
00484
00485 kmot_SetPoint( mot1 , kMotRegSpeed , -speed );
00486 kmot_SetPoint( mot2 , kMotRegSpeed , speed );
00487 } else
00488 {
00489
00490 kmot_SetPoint( mot1 , kMotRegSpeed , speed );
00491 kmot_SetPoint( mot2 , kMotRegSpeed , -speed );
00492 }
00493 return 1;
00494 }
00495 else
00496 {
00497
00498
00499 if (d > FAST_SPEED_POS_THRESH)
00500 {
00501 kmot_SetPoint( mot1 , kMotRegSpeed , MOVE_FAST_SPEED );
00502 kmot_SetPoint( mot2 , kMotRegSpeed , MOVE_FAST_SPEED );
00503 }
00504 else
00505 {
00506 kmot_SetPoint( mot1 , kMotRegSpeed , MOVE_SLOW_SPEED );
00507 kmot_SetPoint( mot2 , kMotRegSpeed , MOVE_SLOW_SPEED );
00508 }
00509 return 2;
00510 }
00511
00512 return -1;
00513
00514 }
00515
00516
00524 long long
00525 timeval_diff(struct timeval *difference,
00526 struct timeval *end_time,
00527 struct timeval *start_time
00528 )
00529 {
00530 struct timeval temp_diff;
00531
00532 if(difference==NULL)
00533 {
00534 difference=&temp_diff;
00535 }
00536
00537 difference->tv_sec =end_time->tv_sec -start_time->tv_sec ;
00538 difference->tv_usec=end_time->tv_usec-start_time->tv_usec;
00539
00540
00541
00542 while(difference->tv_usec<0)
00543 {
00544 difference->tv_usec+=1000000;
00545 difference->tv_sec -=1;
00546 }
00547
00548 return 1000000LL*difference->tv_sec+
00549 difference->tv_usec;
00550
00551 }
00552
00553
00554 int main(int argc, char *argv[])
00555 {
00556
00557 int ret=0,c=0,retxya;
00558 int kh3ver,kh3rev;
00559 char version[128];
00560 char text[256];
00561
00562
00563 int gstop=0,gcxo,gcyo;
00564 int gcx=NB_COLUMNS_MAX/2+1;
00565 int gcy=NB_LINES_MAX/2+2;
00566 double dist2goal,angle2goal;
00567 double goal_x=0,goal_y=0,goal_a=0;
00568
00569 double angle,x,y,z,xc,yc;
00570
00571
00572 int nb_land, ref_land, land_type,i,land_mode;
00573
00574
00575 int idnum,line,column,ll=1,lc=1,lp=1;
00576
00577
00578 int stop=0, anyarrow=0, trace =0 , mapm=0,gxya=0;
00579 char cmode;
00580 int save_data=0;
00581 int correction=0;
00582
00583
00584 int speed=DEFAULT_SPEED;
00585
00586 double center_x,center_y,a_axis,b_axis,angle_rot,stddevx,stddevy;
00587
00588 FILE *data_file=NULL;
00589
00590 #ifdef DEBUG
00591 struct timeval earlier;
00592 struct timeval later;
00593 struct timeval interval;
00594 double ti;
00595 #endif
00596
00597
00598 kb_clrscr();
00599
00600
00601 printf("\nKhepera III Stargazer demo program\n");
00602
00603
00604
00605 if ((ret=initKH3())!=0)
00606 {
00607 printf("\nERROR: cannot initialize the Khepera 3 (error = %d)!\n",ret);
00608 return -2;
00609 }
00610
00611
00612 if(kh3_revision((char *)text, dsPic)>0){
00613
00614 kh3ver=(text[1] | text[2]<<8);
00615 kh3rev=(text[3] | text[4]<<8);
00616
00617 printf("\r\nKhepera 3 : version = %u and revision = %u\r\n",kh3ver ,kh3rev);
00618 }
00619
00620
00621 if(kh3_battery_voltage((char *)text, 5, dsPic)>0){
00622 printf("\r\nKhepera 3 : battery remaining = %3u %%\r\n",(text[1] | text[2]<<8));
00623 }
00624
00625 printf("\nInitialising Stargazer module; please wait!\n");
00626 fflush( stdout );
00627
00628
00629
00630 if ((ret=kb_stargazer_Init())!=0)
00631 {
00632 printf("\nError initialising the Stargazer (error = %d)!\n",ret);
00633
00634 kb_stargazer_Close();
00635 return -3;
00636 }
00637
00638
00639 kb_gazer_get_version(version);
00640 printf("\nStargazer version is: %s\n",version);
00641
00642
00643 set_Stargazer_parameters(&nb_land, &ref_land, &land_type,&land_mode);
00644
00645
00646 printf("\nWould you like to do the position calibration (n/y)? ");
00647 fflush( stdout );
00648 c = getchar();
00649
00650 tcflush(0,TCIFLUSH);
00651
00652 kb_gazer_start_computation();
00653
00654 if (c == 'y')
00655 {
00656
00657
00658 if ((ret=kb_gazer_calibration(mot1,mot2,¢er_x,¢er_y,&angle_rot,&a_axis,&b_axis,&stddevx,&stddevy))<0)
00659 {
00660
00661 if (ret==-9)
00662 {
00663 printf("\nERROR during calibration: no landmark found!\n");
00664 }
00665 else
00666 if (ret==-4)
00667 {
00668 printf("ERROR during calibration: max standard deviation (%.1f cm) reached:\n ellipse parameters: centre: (%4.1f,%4.1f) angle: %4.1f major axe: %4.1f minor axe: %4.1f stddevx = %4.2f stddevy = %4.2f\n",CALIB_STDEV_MAX ,center_x,center_y,angle_rot,a_axis,b_axis,stddevx,stddevy);
00669 }
00670 else
00671 {
00672 printf("\nERROR during calibration (error : %d)!\n",ret);
00673 }
00674 kb_stargazer_Close();
00675
00676 return -4;
00677 }
00678 else
00679 {
00680 correction=1;
00681
00682
00683 printf("\n calibration OK: standard deviation x= %4.2f y= %4.2f\n",stddevx,stddevy);
00684 }
00685 } else
00686 {
00687 correction=0;
00688 }
00689
00690 printf("\nExtend your terminal console size to have at least %d columns and %d lines\nthen push any key to start displaying position (use arrows to control the robot)!\n",NB_COLUMNS_MAX+KEYS_TEXT_EXT_LENGTH,NB_LINES_MAX);
00691
00692 kb_clrscr();
00693
00694 kb_change_term_mode(1);
00695
00696
00697 while(!kb_kbhit())
00698 {
00699 usleep(100);
00700 }
00701
00702
00703 while(kb_kbhit())
00704 {
00705 c = getchar();
00706 }
00707
00708 #ifdef DEBUG
00709 gettimeofday(&earlier,NULL);
00710 #endif
00711
00712
00713 while(!stop)
00714 {
00715
00716 anyarrow=0;
00717
00718
00719 if(kb_kbhit())
00720 {
00721
00722 c = getchar();
00723
00724
00725 if (c== 27 && (getchar()==91))
00726 {
00727 c = getchar();
00728 switch(c)
00729 {
00730 case 65:
00731 anyarrow=1;
00732 kmot_SetPoint( mot1 , kMotRegSpeed , speed );
00733 kmot_SetPoint( mot2 , kMotRegSpeed , speed );
00734 break;
00735
00736 case 66:
00737 anyarrow=1;
00738 kmot_SetPoint( mot1 , kMotRegSpeed , -speed );
00739 kmot_SetPoint( mot2 , kMotRegSpeed , -speed );
00740 break;
00741
00742 case 68:
00743 anyarrow=1;
00744 kmot_SetPoint( mot1 , kMotRegSpeed , -speed );
00745 kmot_SetPoint( mot2 , kMotRegSpeed , speed );
00746 break;
00747
00748 case 67:
00749 anyarrow=1;
00750 kmot_SetPoint( mot1 , kMotRegSpeed , speed );
00751 kmot_SetPoint( mot2 , kMotRegSpeed , -speed );
00752 break;
00753
00754 case 53:
00755 speed*=SPEED_FACTOR;
00756 if (speed>MAX_SPEED)
00757 {
00758 speed=MAX_SPEED;
00759 };
00760 c = getchar();
00761 break;
00762
00763 case 54:
00764 speed/=SPEED_FACTOR;
00765 if (speed<MIN_SPEED)
00766 {
00767 speed=MIN_SPEED;
00768 };
00769 c = getchar();
00770 break;
00771
00772
00773 default:
00774 printf("\r\n Another key pushed, quitting...\r\n");
00775 stop=1;
00776 break;
00777 }
00778
00779
00780 }
00781 else
00782 {
00783
00784 switch (c)
00785 {
00786
00787 case '+':
00788
00789 x_factor/=ZOOM_FACTOR;
00790 y_factor/=ZOOM_FACTOR;
00791 if (x_factor<MIN_X_FACTOR)
00792 {
00793 x_factor=MIN_X_FACTOR;
00794 y_factor=MIN_Y_FACTOR;
00795 }
00796 break;
00797
00798 case '-':
00799 x_factor*=ZOOM_FACTOR;
00800 y_factor*=ZOOM_FACTOR;
00801 if (x_factor>MAX_X_FACTOR)
00802 {
00803 x_factor=MAX_X_FACTOR;
00804 y_factor=MAX_Y_FACTOR;
00805 };
00806 break;
00807
00808 case 'c':
00809 if (gxya == 0)
00810 {
00811 kb_clrscr();
00812 printf("\nPush any key to start calibration: the robot will rotate very slowly one revolution!\n");
00813 fflush( stdout );
00814 tcflush(0,TCIFLUSH);
00815 while(!kb_kbhit())
00816 {
00817 usleep(100);
00818 }
00819
00820 if ((ret=kb_gazer_calibration(mot1,mot2,¢er_x,¢er_y,&angle_rot,&a_axis,&b_axis,&stddevx,&stddevy))<0)
00821 {
00822 if (ret==-4)
00823 {
00824 printf("ERROR during calibration: max standard deviation reached (%.1f cm):\n ellipse parameters: centre: (%4.1f,%4.1f) angle: %4.1f major axe: %4.1f minor axe: %4.1f stddevx = %4.2f stddevy = %4.2f\n",CALIB_STDEV_MAX,center_x,center_y,angle_rot,a_axis,b_axis,stddevx,stddevy);
00825 } else
00826 {
00827 printf("\nERROR during calibration (error : %d)\n",ret);
00828 }
00829 } else
00830 {
00831
00832 printf("\n calibration OK: standard deviation x= %4.2f y= %4.2f\n",stddevx,stddevy);
00833
00834 correction=1;
00835
00836 }
00837 printf("\nPush any key to continue.\n");
00838 tcflush(0,TCIFLUSH);
00839 while(!kb_kbhit())
00840 {
00841 usleep(100);
00842 }
00843 kb_clrscr();
00844 }
00845 break;
00846
00847 case 'g':
00848 if (gxya==0)
00849 {
00850
00851 gxya=1;
00852
00853 kb_erase_line(2);
00854 printf("Move with arrows and push RETURN to set goal (another key: cancel)");
00855
00856 gstop=0;
00857
00858
00859
00860 goal_x=(gcx-(NB_COLUMNS_MAX/2+1))*x_factor/NB_COLUMNS_MAX;
00861 goal_y=-(gcy-(NB_LINES_MAX/2+2))*y_factor/NB_LINES_MAX;
00862 kb_erase_line(3);
00863 printf("goal: x=%6.1f y=%6.1f angle=%6.1f",goal_x,goal_y,goal_a);
00864
00865
00866
00867 if ((line >2 && line<NB_LINES_MAX ) && (column >=1 && column <=NB_COLUMNS_MAX))
00868 {
00869 kb_move_cursor(column,line);
00870
00871 if ((angle > 45) && (angle <= 135))
00872 printf("<");
00873 else
00874 if ((angle > 135) && (angle <= 225))
00875 printf("V");
00876 else
00877 if ((angle > 225) && (angle <= 315))
00878 printf(">");
00879 else
00880 printf("A");
00881 }
00882
00883 gcxo=gcx;
00884 gcyo=gcy;
00885 kb_move_cursor(gcx,gcy);
00886 printf("G");
00887 fflush( stdout );
00888 anyarrow=0;
00889 while(!gstop)
00890 {
00891
00892 anyarrow=0;
00893
00894
00895 if(kb_kbhit())
00896 {
00897
00898 c = getchar();
00899
00900
00901 if (c== 27 && (getchar()==91))
00902 {
00903 c = getchar();
00904 switch(c)
00905 {
00906 case 65:
00907 gcy--;
00908 if (gcy <4)
00909 gcy = NB_LINES_MAX-1;
00910 anyarrow=1;
00911 break;
00912
00913 case 66:
00914 gcy++;
00915 if (gcy >NB_LINES_MAX-1)
00916 gcy = 4;
00917 anyarrow=1;
00918 break;
00919
00920 case 68:
00921 gcx--;
00922 if (gcx <1)
00923 gcx = NB_COLUMNS_MAX-1;
00924 anyarrow=1;
00925 break;
00926
00927 case 67:
00928 gcx++;
00929 if (gcx >NB_COLUMNS_MAX-1)
00930 gcx = 1;
00931 anyarrow=1;
00932 break;
00933 }
00934 } else
00935 {
00936
00937 if (c== '\n' || c== '\r')
00938 {
00939 gstop=1;
00940 }
00941 else
00942 {
00943
00944 gstop=1;
00945 gxya=0;
00946
00947 kb_clrscr();
00948 kb_erase_line(3);
00949 printf("Cancelled set goal!");
00950 fflush( stdout );
00951
00952 }
00953 }
00954
00955 if (anyarrow)
00956 {
00957
00958 kb_move_cursor(gcxo,gcyo);
00959 printf(" ");
00960 gcxo=gcx;
00961 gcyo=gcy;
00962
00963
00964 kb_move_cursor(NB_COLUMNS_MAX/2,NB_LINES_MAX/2+2);
00965 printf("0,0");
00966
00967
00968 if ((line >2 && line<NB_LINES_MAX ) && (column >=1 && column <=NB_COLUMNS_MAX))
00969 {
00970 kb_move_cursor(column,line);
00971
00972 if ((angle > 45) && (angle <= 135))
00973 printf("<");
00974 else
00975 if ((angle > 135) && (angle <= 225))
00976 printf("V");
00977 else
00978 if ((angle > 225) && (angle <= 315))
00979 printf(">");
00980 else
00981 printf("A");
00982 }
00983
00984
00985 kb_move_cursor(gcx,gcy);
00986 printf("G");
00987
00988 goal_x=(gcx-(NB_COLUMNS_MAX/2+1))*x_factor/NB_COLUMNS_MAX;
00989 goal_y=-(gcy-(NB_LINES_MAX/2+2))*y_factor/NB_LINES_MAX;
00990 kb_erase_line(3);
00991 printf("goal: x=%6.1f y=%6.1f angle=%6.1f",goal_x,goal_y,goal_a);
00992
00993 fflush( stdout );
00994 }
00995
00996 }
00997 }
00998
00999
01000 if (gxya)
01001 {
01002 goal_x=(gcx-(NB_COLUMNS_MAX/2+1))*x_factor/NB_COLUMNS_MAX;
01003 goal_y=-(gcy-(NB_LINES_MAX/2+2))*y_factor/NB_LINES_MAX;
01004 goal_a=0;
01005 kb_clrscr();
01006
01007 kb_erase_line(3);
01008 printf("Moving to goal: x=%6.1f y=%6.1f angle=%6.1f",goal_x,goal_y,goal_a);
01009 }
01010
01011
01012 } else
01013 {
01014
01015
01016 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
01017 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
01018 kb_erase_line(3);
01019 printf("Stopped goto manually");
01020 gxya=0;
01021 }
01022
01023 break;
01024
01025 case 'k':
01026 correction=!correction;
01027 break;
01028 case 'm':
01029 if ((mapm==0) && (gxya==0))
01030 {
01031
01032 if (land_mode == MARK_ALONE)
01033 {
01034 kb_erase_line(3);
01035 printf("ERROR: could not enter in map mode because configured in ALONE mode!");
01036 break;
01037 }
01038
01039 ret=kb_gazer_start_map_mode();
01040
01041 if (ret<0)
01042 {
01043 kb_erase_line(3);
01044 printf("ERROR: could not enter in map mode (error: %d )!",ret);
01045 if (ref_land != idnum)
01046 printf(" Verify that the robot is under reference landmark: %d ; current landmark is %d!",ref_land,idnum);
01047 break;
01048 } else
01049 {
01050 kb_erase_line(3);
01051 printf("MAP MODE: Move towards other nearest landmark and stop for about 2s near halfway between the two landmarks,\nthen move to the next landmark proceed the same way until all the landmarks are detected");
01052 }
01053
01054 mapm=1;
01055
01056 }
01057 break;
01058
01059 case 'p':
01060 if (gxya==0)
01061 {
01062 kb_clrscr();
01063 kb_gazer_wait_stop_computation();
01064 kb_change_term_mode(0);
01065 set_Stargazer_parameters(&nb_land, &ref_land, &land_type,&land_mode);
01066 kb_change_term_mode(1);
01067 kb_gazer_start_computation();
01068 printf("\nPush any key to continue.\n");
01069 fflush( stdout );
01070 tcflush(0,TCIFLUSH);
01071 while(!kb_kbhit())
01072 {
01073 usleep(100);
01074 }
01075 kb_clrscr();
01076 }
01077 break;
01078
01079
01080 case 'q':
01081 printf("\r\n q key pushed, quitting...\r\n");
01082 stop=1;
01083 break;
01084 case 's':
01085 if (save_data)
01086 {
01087 if (data_file)
01088 {
01089 fclose(data_file);
01090 data_file=NULL;
01091 }
01092 save_data=0;
01093 } else
01094 {
01095 save_data=1;
01096 data_file=fopen(DATA_FILE_NAME,"w");
01097 if (data_file)
01098 {
01099 fprintf(data_file,"center x\t%.5f\tcenter y\t%.5f\tangle\t%.5f\thalf major axis\t%.5f\thalf minor axis\t%.5f\t\n",center_x,center_y,angle_rot,a_axis,b_axis);
01100 fprintf(data_file,"x\ty\txc\tyc\tangle\n");
01101 }
01102 }
01103
01104 break;
01105 case 't':
01106 if (trace)
01107 {
01108 kb_clrscr();
01109 trace=0;
01110 }
01111 else
01112 {
01113 trace=1;
01114 }
01115
01116 break;
01117
01118
01119
01120 default:
01121
01122 break;
01123 }
01124 }
01125
01126 tcflush(0,TCIFLUSH);
01127 }
01128
01129 if (!anyarrow && !gxya)
01130 {
01131
01132 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
01133 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
01134 }
01135
01136
01137 ret=kb_stargazer_read_data(&x,&y,&z,&angle,&idnum,&cmode,0);
01138
01139
01140 if (ret==-3)
01141 {
01142 fprintf(stderr,"\nERROR: read error: buffer too short, leaving!\n");
01143 kb_change_term_mode(0);
01144 kb_stargazer_Close();
01145
01146 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
01147 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
01148 return -5;
01149 }
01150
01151
01152
01153
01154 if (ret==0)
01155 {
01156
01157 if (correction)
01158 {
01159
01160
01161 xc=a_axis*cos((angle+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)
01162 *cos(angle_rot*M_PI/180.0)-b_axis*sin((angle+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*sin(angle_rot*M_PI/180.0);
01163
01164 yc=a_axis*cos((angle+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)
01165 *sin(angle_rot*M_PI/180.0)+b_axis*sin((angle+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*cos(angle_rot*M_PI/180.0);
01166
01167 if (save_data)
01168 {
01169 if (data_file)
01170 {
01171 fprintf(data_file,"%.5f\t%.5f\t%.5f\t%.5f\t%.5f\n",x,y,x-xc,y-yc,angle);
01172 }
01173 }
01174 x-=xc;
01175 y-=yc;
01176 }
01177
01178 line = (int)ROUND(-y/y_factor*NB_LINES_MAX+NB_LINES_MAX/2+2);
01179
01180 column=(int)ROUND(x/x_factor*NB_COLUMNS_MAX+NB_COLUMNS_MAX/2+1);
01181
01182
01183
01184 kb_move_cursor(NB_COLUMNS_MAX/2,NB_LINES_MAX/2+2);
01185 printf("0,0");
01186
01187
01188 if ((line >2 && line<NB_LINES_MAX ) && (column >=1 && column <=NB_COLUMNS_MAX))
01189 {
01190 kb_move_cursor(column,line);
01191
01192 if ((angle > 45) && (angle <= 135))
01193 printf("<");
01194 else
01195 if ((angle > 135) && (angle <= 225))
01196 printf("V");
01197 else
01198 if ((angle > 225) && (angle <= 315))
01199 printf(">");
01200 else
01201 printf("A");
01202
01203 }
01204
01205
01206
01207 kb_erase_line(NB_LINES_MAX);
01208
01209 if (x_factor<MEDIUM_SCALE)
01210 printf("scale: x 5cm = >%*c",(int)floor(NB_COLUMNS_MAX*5.0/x_factor),'<');
01211 else
01212 if (x_factor<BIGGEST_SCALE)
01213 printf("scale: x 20cm = >%*c",(int)floor(NB_COLUMNS_MAX*20.0/x_factor),'<');
01214 else
01215 printf("scale: x 1m = >%*c",(int)floor(NB_COLUMNS_MAX*100.0/x_factor),'<');
01216
01217
01218
01219 kb_move_cursor(NB_COLUMNS_MAX,lp);
01220 printf(" ");
01221
01222
01223 if (y_factor<MEDIUM_SCALE)
01224 {
01225 lp=NB_LINES_MAX-(int)floor(NB_LINES_MAX*5.0/y_factor);
01226 }
01227 else
01228 if (y_factor<BIGGEST_SCALE)
01229 {
01230 lp=NB_LINES_MAX-(int)floor(NB_LINES_MAX*20.0/y_factor);
01231 }
01232 else
01233 {
01234 lp=NB_LINES_MAX-(int)floor(NB_LINES_MAX*100.0/y_factor);
01235 }
01236 kb_move_cursor(NB_COLUMNS_MAX,lp);
01237 printf("-");
01238
01239 kb_move_cursor(NB_COLUMNS_MAX-9,NB_LINES_MAX);
01240
01241 if (y_factor<MEDIUM_SCALE)
01242 printf("y 5cm = -");
01243 else
01244 if (y_factor<BIGGEST_SCALE)
01245 printf("y 20cm = -");
01246 else
01247 printf(" y 1m = -");
01248
01249
01250 if (gxya)
01251 {
01252 retxya=goto_xya(x,y,angle,goal_x,goal_y,goal_a,&dist2goal,&angle2goal);
01253 if (retxya==0)
01254 {
01255 gxya=0;
01256 kb_erase_line(3);
01257 printf("Goal reached: x=%6.1f y=%6.1f angle=%6.1f (error remaining: distance= %6.1f angle= %6.1f) !",goal_x,goal_y,goal_a,dist2goal,angle2goal);
01258
01259 } else
01260 {
01261
01262 gcx=(int)ROUND(goal_x/x_factor*NB_COLUMNS_MAX+NB_COLUMNS_MAX/2+1);
01263 gcy = (int)ROUND(-goal_y/y_factor*NB_LINES_MAX+NB_LINES_MAX/2+2);
01264 kb_move_cursor(gcx,gcy);
01265 printf("G");
01266 kb_erase_line(3);
01267 printf("Moving to goal: x=%6.1f y=%6.1f angle=%6.1f (remaining: distance= %6.1f angle= %6.1f)",goal_x,goal_y,goal_a,dist2goal,angle2goal);
01268 }
01269 }
01270
01271
01272 kb_erase_line(1);
01273 sprintf(text,"[cm,deg] x= %+6.1f y= %+6.1f angle= %5.1f height= %5.1f idnum= %d | zoom: %3.1fx speed[mm/s]: %4.1f (%5d) mode: %c\n",x,y,angle,z,idnum,DEFAULT_X_FACTOR/x_factor,speed/kh3_mms_to_speed,speed,cmode);
01274 printf("%s",text);
01275
01276
01277 ll=line;
01278 lc=column;
01279
01280 }
01281 else {
01282
01283 kb_erase_line(1);
01284
01285
01286 if (ret==-6)
01287 {
01288 printf("The sensor does not see any landmark!\n");
01289 }
01290 else
01291 if ((ret==1) && (mapm==1))
01292 {
01293 kb_erase_line(4);
01294 kb_erase_line(3);
01295 printf("End of map mode; update parameters detected!");
01296 mapm=0;
01297 }
01298 else
01299 if (ret>1)
01300 {
01301 kb_erase_line(3);
01302 kb_erase_line(4);
01303 printf("MAPID number %d detected!",ret);
01304 }
01305 else
01306 if (ret==-8)
01307 {
01308 printf("No data received: the sensor may not see any landmark!");
01309 printf(" Robot speed for arrows control [mm/s]: %4.1f (%5d)",speed/kh3_mms_to_speed,speed);
01310 }
01311 else
01312 {
01313 printf("Another error happened (error: %d)!",ret);
01314 printf(" Robot speed for arrows control [mm/s]: %4.1f (%5d)",speed/kh3_mms_to_speed,speed);
01315
01316 }
01317 }
01318
01319 kb_erase_line(2);
01320 printf("KEYS: (q)=quit (arrows)=move (+/-)=zoom (PG UP/DOWN)=speed (s)=save(%s) (k)=apply corr(%s) (t)=trace(%s) (m)=build map(%s) (p)=param (c)=calib g=goto(%s)",save_data?"ON ":"OFF",correction?"ON ":"OFF",trace?"ON ":"OFF",mapm?"ON ":"OFF",gxya?(retxya==1?"ON1":(retxya==2?"ON2":"ON3")):"OFF");
01321
01322 fflush( stdout );
01323 usleep(89000);
01324
01325
01326
01327 if ( (ll >2 && ll<NB_LINES_MAX ) && (lc >=1 && lc <=NB_COLUMNS_MAX))
01328 {
01329
01330 kb_move_cursor(lc,ll);
01331 if (trace)
01332 printf(".");
01333 else
01334 printf(" ");
01335 }
01336
01337
01338 #ifdef DEBUG
01339 gettimeofday(&later,NULL);
01340 ti=timeval_diff(NULL,&later,&earlier)/1000000.0;
01341 kb_erase_line(4);
01342
01343 printf("frame time [s]: %4.3f (fps= %4.1f)",ti,1.0/ti);
01344 earlier.tv_sec=later.tv_sec;
01345 earlier.tv_usec=later.tv_usec;
01346
01347 #endif
01348
01349 }
01350
01351 if (data_file)
01352 fclose(data_file);
01353
01354
01355 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
01356 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
01357
01358 kb_stargazer_Close();
01359
01360 kb_change_term_mode(0);
01361
01362 kb_clrscr();
01363
01364 return 0;
01365
01366 }
01367