00001
00016 #include <math.h>
00017 #include <signal.h>
00018 #include <limits.h>
00019 #include <korebot/korebot.h>
00020
00021 static int quitReq = 0;
00022 static int lrfHandle = -1;
00023
00024
00025 static FILE *outfile;
00026
00027 static char filename[256];
00028
00029
00030 #define NB_COLUMNS_MAX 80.0
00031 #define NB_LINES_MAX 24.0
00032
00033
00034 #define DEFAULT_X_FACTOR (2.0*2000.0)
00035 #define DEFAULT_Y_FACTOR (2.0*2000.0)
00036 #define MAX_X_FACTOR (2.0*5600.0)
00037 #define MAX_Y_FACTOR (2.0*5600.0)
00038 #define MIN_X_FACTOR (2.0*500.0)
00039 #define MIN_Y_FACTOR (2.0*500.0)
00040
00041 #define LRF_DEVICE "/dev/ttyACM0" // device where the LRF is connected: here USB port
00042
00043
00044 static float x_factor = DEFAULT_X_FACTOR;
00045 static float y_factor = DEFAULT_Y_FACTOR;
00046
00047 #define ZOOM_FACTOR 1.25
00048
00049
00055 static void ctrlc_handler( int sig )
00056 {
00057 quitReq = 1;
00058
00059 kb_lrf_Close(lrfHandle);
00060 lrfHandle=-1;
00061
00062 if (outfile)
00063 {
00064 fclose(outfile);
00065 printf("\nMeasure file is %s\n",filename);
00066 }
00067
00068
00069 exit(1);
00070 }
00071
00072
00073
00074
00078 int laseron( int argc, char * argv[], void * data)
00079 {
00080 if (lrfHandle<0)
00081 {
00082 printf("Error with lrfmeasure: lrf not initialised; run lrfinit first!\n");
00083 return -1;
00084 }
00085 kb_lrf_Laser_On(lrfHandle);
00086 printf("lrf laser beam is now on...\r\n");
00087
00088 }
00089
00090
00094 int laseroff( int argc, char * argv[], void * data)
00095 {
00096 if (lrfHandle<0)
00097 {
00098 printf("Error with lrfmeasure: lrf not initialised; run lrfinit first!\n");
00099 return -1;
00100 }
00101
00102 kb_lrf_Laser_Off(lrfHandle);
00103 printf("lrf laser beam is now off...\r\n");
00104
00105
00106 }
00107
00108
00109
00113 int poweron( int argc, char * argv[], void * data)
00114 {
00115 kb_lrf_Power_On();
00116
00117 }
00118
00119
00123 int poweroff( int argc, char * argv[], void * data)
00124 {
00125
00126 if (lrfHandle>=0)
00127 {
00128 kb_lrf_Laser_Off(lrfHandle);
00129 kb_lrf_Close(lrfHandle);
00130 lrfHandle=-1;
00131 }
00132
00133 kb_lrf_Power_Off();
00134
00135 }
00136
00137
00138
00142 void lrfheader( )
00143 {
00144 printf("lrf_header : preparing the measure header..\n");
00145 if(outfile)
00146 {
00147 printf("lrf_header : putting the measurement header as file header..\n");
00148 fprintf(outfile, "LED RANGE FINDER MEASUREMENT\ncount\tindex\tangle\tdist\tx\ty\n\n");
00149 }
00150 else
00151 printf("lrf_header : error file not been set, please use the setfile command..\n");
00152 }
00153
00154
00158 int setfile( int argc, char * argv[], void * data)
00159 {
00160
00161 if (outfile)
00162 {
00163 fclose(outfile);
00164 }
00165
00166 outfile = fopen(argv[1], "w");
00167
00168 strcpy(filename,argv[1]);
00169
00170 if(outfile)
00171 {
00172 printf("File %s has been opened with success!\r\n",filename);
00173
00174 }
00175 else
00176 printf("unable to open the %s file\r\n", filename);
00177
00178 }
00179
00180
00181
00182
00186 int lrfinit( int argc, char * argv[], void * data)
00187 {
00188 lrfHandle = kb_lrf_Init(LRF_DEVICE);
00189
00190 }
00191
00192
00196 int lrfmeasure( int argc, char * argv[], void * data)
00197 {
00198 int i, j,k,ret,stop=0;
00199 int max,average=1;
00200 float x,y,angle;
00201 int line,column,ll=0,lc=0;
00202 char key;
00203
00204 if (lrfHandle<0)
00205 {
00206 printf("Error with lrfmeasure: lrf not initialised; run lrfinit first!\n");
00207 return -1;
00208 }
00209
00210
00211 max = atoi(argv[1]);
00212
00213 if (argc == 3)
00214 {
00215 average=atoi(argv[2]);
00216 }
00217
00218
00219 if (max ==0)
00220 max=INT_MAX-1;
00221
00222
00223
00224 lrfheader();
00225
00226 printf("lrfmeasure : going to proceed with %d measure...\r\n", max);
00227
00228
00229 kb_change_term_mode(1);
00230
00231 for (k=0; (k < max) && (stop==0); k++)
00232 {
00233
00234
00235 kb_clrscr();
00236
00237
00238
00239 kb_move_cursor(1,NB_LINES_MAX);
00240
00241 if (x_factor<3000)
00242 printf(" scale x -> : 20 cm = |%*c",(int)floor(NB_COLUMNS_MAX*200.0/x_factor),'|');
00243 else
00244 printf(" scale x -> : 1 meter = |%*c",(int)floor(NB_COLUMNS_MAX*1000.0/x_factor),'|');
00245
00246
00247 if (y_factor<3000)
00248 kb_move_cursor(NB_COLUMNS_MAX,NB_LINES_MAX-(int)floor(NB_LINES_MAX*200.0/y_factor));
00249 else
00250 kb_move_cursor(NB_COLUMNS_MAX,NB_LINES_MAX-(int)floor(NB_LINES_MAX*1000.0/y_factor));
00251 printf("-");
00252 kb_move_cursor(NB_COLUMNS_MAX-22,NB_LINES_MAX);
00253
00254 if (y_factor<3000)
00255 printf("scale y ^ : 20 cm = -");
00256 else
00257 printf("scale y ^ : 1 meter = -");
00258
00259
00260
00261 if (average >1)
00262 ret=kb_lrf_GetDistances_Averaged(lrfHandle,average);
00263 else
00264 ret=kb_lrf_GetDistances(lrfHandle);
00265
00266 if (ret>=0)
00267 {
00268
00269 kb_erase_line(1);
00270 kb_erase_line(2);
00271 kb_move_cursor(1,1);
00272
00273 printf("measure index:%4d R =robot * =obstacle timestamp=%12ld[ms] zoom: %3.1fx\n KEYS: (k)=zoom in (l)=zoom out (a)verage=%d (Any other key):stop",k+1,kb_lrf_Get_Timestamp(),x_factor/DEFAULT_X_FACTOR,average);
00274
00275
00276 kb_move_cursor(NB_COLUMNS_MAX/2-1,2.0*NB_LINES_MAX/3.0+2+1);
00277 printf("(R)");
00278
00279 for (i=0; i< LRF_DATA_NB; i++)
00280 {
00281
00282
00283
00284
00285 angle= (i-(LRF_DATA_NB/2.0-1024/4.0))*360.0/1024.0;
00286
00287
00288 x=kb_lrf_DistanceData[i]*cos(angle*M_PI/180.0);
00289 y=kb_lrf_DistanceData[i]*sin(angle*M_PI/180.0);
00290
00291
00292
00293 line = (int)ceil(-y/y_factor*NB_LINES_MAX+2.0*NB_LINES_MAX/3.0+2+1);
00294 column=(int)ceil(x/x_factor*NB_COLUMNS_MAX+NB_COLUMNS_MAX/2+1);
00295
00296
00297
00298
00299
00300 if ((line >=3 && line<NB_LINES_MAX ) && (column >=1 && column <NB_COLUMNS_MAX) && (kb_lrf_DistanceData[i]>=20) && (kb_lrf_DistanceData[i]<=4000))
00301 {
00302 kb_move_cursor(column,line);
00303 printf("*");
00304 }
00305
00306
00307
00308
00309 if(outfile)
00310 {
00311 fprintf(outfile,"%d\t%d\t%6.1f\t%ld\t%7.1f\t%7.1f\n",k,i,angle,kb_lrf_DistanceData[i],x,y);
00312 }
00313 }
00314
00315
00316
00317
00318
00319 }
00320 else
00321 {
00322 printf("Error: could not get measure %d!\n",k+1);
00323 }
00324
00325 usleep(100000);
00326
00327
00328
00329 if (kb_kbhit())
00330 {
00331 key=getchar();
00332
00333 switch(key)
00334 {
00335 case 'a':
00336 if (average<5)
00337 average++;
00338 else
00339 average=1;
00340 break;
00341
00342 case 'k':
00343 if (x_factor>MIN_X_FACTOR)
00344 {
00345 x_factor/=ZOOM_FACTOR;
00346 y_factor/=ZOOM_FACTOR;
00347 }
00348 else
00349 {
00350 x_factor=MIN_X_FACTOR;
00351 y_factor=MIN_Y_FACTOR;
00352 }
00353 break;
00354
00355 case 'l':
00356 if (x_factor<MAX_X_FACTOR)
00357 {
00358 x_factor*=ZOOM_FACTOR;
00359 y_factor*=ZOOM_FACTOR;
00360 }
00361 else
00362 {
00363 x_factor=MAX_X_FACTOR;
00364 y_factor=MAX_Y_FACTOR;
00365 };
00366 break;
00367
00368 default:
00369 printf("\r\n Another key pushed, quitting...\r\n");
00370 stop=1;
00371 break;
00372 }
00373 }
00374 }
00375 kb_clrscr();
00376
00377 kb_change_term_mode(0);
00378
00379 laseroff(0, NULL, NULL);
00380
00381
00382 }
00383
00384
00388 int lrfmeasureaverage( int argc, char * argv[], void * data)
00389 {
00390 lrfmeasure( argc, argv, data);
00391 }
00392
00393
00394
00395
00396
00399 int quit( int argc , char * argv[] , void * data)
00400 {
00401 quitReq = 1;
00402
00403 }
00404
00405
00406 int help( int argc , char * argv[] , void * data);
00407
00411 static kb_command_t cmds[] = {
00412 { "quit" , 0 , 0 , quit } ,
00413 { "exit" , 0 , 0 , quit } ,
00414 { "bye" , 0 , 0 , quit } ,
00415 { "setfile" , 1 , 1 , setfile },
00416 { "lrfinit" , 0 , 0 , lrfinit },
00417 { "laseron" , 0 , 0 , laseron },
00418 { "laseroff" , 0 , 0 , laseroff },
00419 { "poweron" , 0 , 0 , poweron },
00420 { "poweroff" , 0 , 0 , poweroff },
00421 { "lrfmeasure" , 1 , 2 , lrfmeasure },
00422 { "help" , 0 , 0 , help } ,
00423 { NULL , 0 , 0 , NULL }
00424 };
00425
00426
00429 int help( int argc , char * argv[] , void * data)
00430 {
00431 kb_command_t * scan = cmds;
00432 while(scan->name != NULL)
00433 {
00434 printf("%s\r\n",scan->name);
00435 scan++;
00436 }
00437 return 0;
00438 }
00439
00440
00441
00442
00446 static char buf[1024];
00447
00448 int main( int argc , char * argv[] )
00449 {
00450 int rc;
00451
00452
00453
00454 kb_clrscr();
00455
00456
00457 kb_set_debug_level(2);
00458
00459 if((rc = kb_init( argc , argv )) < 0 )
00460 return 1;
00461
00462 printf("Led Range Finder Test Program (C) K-Team S.A\r\n");
00463
00464
00465 signal( SIGINT , ctrlc_handler );
00466
00467
00468
00469 while (!quitReq) {
00470 printf("\n> ");
00471
00472 if ( fgets( buf , sizeof(buf) , stdin ) != NULL ) {
00473 buf[strlen(buf)-1] = '\0';
00474 kb_parse_command( buf , cmds , NULL);
00475 }
00476 }
00477
00478
00479
00480 kb_lrf_Close(lrfHandle);
00481
00482 if (outfile)
00483 {
00484 fclose(outfile);
00485 printf("\nMeasure file is %s\n",filename);
00486 }
00487
00488 return 0;
00489 }