00001
00017 #include <signal.h>
00018 #include <korebot/korebot.h>
00019
00020 static int quitReq = 0;
00021 static int lrfHandle = 0x1234;
00022 static int gpioOKE = 0;
00023 static int lrfOKE = 0;
00024 static int snOKE = 0;
00025 static int measCnt = 0;
00026 static char *LRF_SN;
00027 static char *KBA_SN;
00028 static FILE *outfile;
00029
00030 unsigned short DataDist[121];
00031
00032 fd_set fd_set1;
00033
00034
00035 #define MAX_TIMEOUT_IN_MS 500
00036 #define MAX_DATA_BYTES 246
00037
00038
00039 #define PXA_INT0 8
00040 #define OUT 0
00041
00042 #define device "/dev/tts/2"
00043
00044
00048 void clrscr(void)
00049 {
00050 char a[80];
00051 printf("\033[2J");
00052 printf("\033[0;0f");
00053 }
00054
00055
00056 void initTime( struct timeval *timestruct )
00057 {
00058 gettimeofday(timestruct, NULL);
00059 }
00060
00061 unsigned long deltaTime( struct timeval *timeS1)
00062 {
00063 struct timeval loc;
00064
00065 gettimeofday(&loc, NULL);
00066
00067 timersub(&loc, timeS1, &loc);
00068 timeradd(timeS1, &loc, timeS1);
00069
00070 return loc.tv_usec;
00071 }
00072
00073
00074 int gpioinit( int argc, char * argv[], void * data)
00075 {
00076 int rc;
00077
00078 if((rc = kb_init( argc , argv )) > -1 )
00079 {
00080
00081 kb_gpio_init();
00082
00083
00084
00085 kb_gpio_function(PXA_INT0,0);
00086
00087
00088 kb_gpio_dir(PXA_INT0, OUT);
00089 }
00090
00091 if(rc < 0)
00092 printf("unable to initialise the gpio library\r\n");
00093 else
00094 {
00095 printf("the gpio library has been initialized\r\n");
00096 gpioOKE = 1;
00097 }
00098
00099 }
00100
00101 int lrfon( int argc, char * argv[], void * data)
00102 {
00103 if(gpioOKE)
00104 {
00105 printf("lrf is now powered...\r\n");
00106 kb_lrf_pwrOn();
00107
00108 usleep(1500*1000);
00109 }
00110 else
00111 {
00112 printf("lrfon : error the gpio library need to be initialized, please use gpioinit\r\n");
00113 }
00114
00115
00116 }
00117
00118 int lrfoff( int argc, char * argv[], void * data)
00119 {
00120 if(gpioOKE)
00121 {
00122 kb_lrf_pwrOff();
00123 printf("lrf is now unpowered...\r\n");
00124 }
00125 else
00126 {
00127 printf("lrfoff : error the gpio library need to be initialized, please use gpioinit\r\n");
00128 }
00129
00130 }
00131
00132 int setsn( int argc, char *argv[], void * data)
00133 {
00134 strcat(LRF_SN, argv[1]);
00135 strcat(KBA_SN, argv[2]);
00136
00137 if(*LRF_SN)
00138 {
00139 printf("Led Range Finder pcb serial number is : %s\n", LRF_SN);
00140 }
00141 else
00142 printf("no LRF serial number has been set, please retry\n");
00143
00144 if(*KBA_SN)
00145 {
00146 printf("Korebot adaptor pcb serial number is : %s\n", KBA_SN);
00147 }
00148 else
00149 printf("no KBA serial number has been set, please retry\n");
00150
00151 snOKE = 1;
00152 }
00153
00154 int zeroCnt( int argc, char *argv[], void * data)
00155 {
00156 printf("zeroing the measure counter\r\n");
00157 measCnt = 0;
00158
00159 }
00160
00161 char *lrfheader( )
00162 {
00163 char *locbuf, *header;
00164
00165 printf("lrf_header : preparing the measure header..\n");
00166
00167 locbuf = (char *)malloc(200);
00168 memset(locbuf, 0, 200);
00169
00170 header = (char *)malloc(2000);
00171 memset(header, 0, 2000);
00172
00173 strcat(header, "LED RANGE FINDER MEASUREMENT\n----------------------------\n\n");
00174
00175 if(*LRF_SN)
00176 {
00177 printf("lrf_header : appending lrf sernum to the header..\n");
00178 strcat(header, "the led range finder pcb serial number is ");
00179 strcat(header, LRF_SN);
00180 strcat(header, "\n");
00181 }
00182 else
00183 printf("lrf_header : no lrf sernum to append to the header..\n");
00184
00185 if(*KBA_SN)
00186 {
00187 printf("lrf_header : appending kba sernum to the header..\n");
00188 strcat(header, "the kore bot adaptor pcb serial number is ");
00189 strcat(header, KBA_SN);
00190 strcat(header, "\n\n");
00191 }
00192 else
00193 printf("lrf_header : no kba sernum to append to the header..\n");
00194
00195
00196 if(outfile)
00197 {
00198 printf("lrf_header : putting the measurement header as file header..\n");
00199 fprintf(outfile, header);
00200 }
00201 else
00202 printf("lrf_header : error file not been set, please use the setfile command..\n");
00203
00204 free(locbuf);
00205 free(header);
00206
00207 }
00208
00209 int setfile( int argc, char * argv[], void * data)
00210 {
00211 outfile = fopen(argv[1], "w");
00212
00213 if(outfile)
00214 {
00215 printf("file %s has been openned with success\r\n", argv[1]);
00216 }
00217 else
00218 printf("unable to open the %s file\r\n", *argv[1]);
00219
00220
00221 }
00222
00223
00224 int lrfinit( int argc, char * argv[], void * data)
00225 {
00226 lrfHandle = kb_lrf_Init(device);
00227
00228 }
00229
00230 int lrfmeasure( int argc, char * argv[], void * data)
00231 {
00232 int i, j, k;
00233 int max;
00234 char *measTbl, *measBuf;
00235
00236
00237 measTbl = (char*)malloc(20000); memset(measTbl, 0, 20000);
00238 measBuf = (char*)malloc(20000); memset(measBuf, 0, 20000);
00239
00240 max = atoi(argv[1]);
00241
00242 lrfHandle = kb_lrf_Init(device);
00243
00244 lrfheader();
00245
00246 printf("lrfmeasure : goin to proceeed with %d measure...\r\n", max);
00247
00248
00249
00250 for (k=0; k < max; k++)
00251 {
00252 printf("lrfmeasure : measure %d \n", k);
00253
00254 clrscr();
00255
00256 if (kb_lrf_GetDistances(lrfHandle) >= 0)
00257 {
00258 measCnt++;
00259
00260 strcat(measTbl, measBuf);
00261
00262 for (i=0, j = 0; i<121; i++, j++)
00263 {
00264 if( j == 8)
00265 {
00266 j = 0;
00267 strcat(measTbl, "\n");
00268 printf("\n");
00269 }
00270
00271
00272 sprintf (measBuf, "Dir %3.3d: ", i);
00273 printf ("Dir %3.3d: ", i);
00274
00275 strcat(measTbl, measBuf);
00276
00277 if(kb_lrf_DistanceData[i])
00278 {
00279 printf ("\e[31m");
00280
00281 }
00282 else
00283 {
00284 printf("\e[30m");
00285 }
00286
00287 sprintf (measBuf, "%6.6d ", kb_lrf_DistanceData[i]);
00288 printf ("%6.6d ", kb_lrf_DistanceData[i]);
00289
00290
00291 printf("\e[30mmm ");
00292
00293 }
00294
00295 strcat(measTbl, "\n");
00296
00297 if(outfile)
00298 {
00299 fprintf(outfile, measTbl);
00300 }
00301
00302 memset(measTbl, 0, 20000);
00303 }
00304 j++;
00305
00306 }
00307
00308 gpioinit(0, NULL, NULL);
00309 lrfoff(0, NULL, NULL);
00310
00311 free(measTbl);
00312 free(measBuf);
00313
00314 }
00315
00316
00317
00318
00319
00320
00323 int quit( int argc , char * argv[] , void * data)
00324 {
00325 quitReq = 1;
00326 }
00327
00328 int emergencystop( int argc , char * argv[] , void * data)
00329 {
00330 gpioinit(0, NULL, NULL);
00331
00332 lrfoff(0, NULL, NULL);
00333 }
00334
00335
00336 int help( int argc , char * argv[] , void * data);
00337
00341 static kb_command_t cmds[] = {
00342 { "quit" , 0 , 0 , quit } ,
00343 { "exit" , 0 , 0 , quit } ,
00344 { "bye" , 0 , 0 , quit } ,
00345 { "emstop" , 0, 0 , emergencystop },
00346 { "zeromeasure" , 0 , 0 , zeroCnt },
00347 { "setserialnum" , 2 , 2 , setsn },
00348 { "setfile" , 1 , 1 , setfile },
00349 { "gpioinit" , 0 , 0 , gpioinit },
00350 { "lrfinit" , 0 , 0 , lrfinit },
00351 { "lrfon" , 0 , 0 , lrfon },
00352 { "lrfoff" , 0 , 0 , lrfoff },
00353 { "lrfmeasure" , 0 , 1 , lrfmeasure },
00354 { "help" , 0 , 0 , help } ,
00355 { NULL , 0 , 0 , NULL }
00356 };
00357
00358
00361 int help( int argc , char * argv[] , void * data)
00362 {
00363 kb_command_t * scan = cmds;
00364 while(scan->name != NULL)
00365 {
00366 printf("%s\r\n",scan->name);
00367 scan++;
00368 }
00369 return 0;
00370 }
00371
00372
00376 static char buf[1024];
00377
00378 int main( int argc , char * argv[] )
00379 {
00380 int rc,ver;
00381 struct timeval starttime;
00382 struct timeval fintime;
00383
00384
00385 gettimeofday(&starttime, NULL);
00386
00387
00388 LRF_SN = (char *)malloc(200); KBA_SN = (char *)malloc(200);
00389 memset(LRF_SN, 0, 200); memset(KBA_SN, 0, 200);
00390
00391
00392 clrscr();
00393
00394
00395 kb_set_debug_level(2);
00396
00397 if((rc = kb_init( argc , argv )) < 0 )
00398 return 1;
00399
00400 printf("Led Range Finder Test Program - A.Maye (K-Team) \r\n");
00401
00402
00403 while (!quitReq) {
00404 printf("\n> ");
00405
00406 if ( fgets( buf , sizeof(buf) , stdin ) != NULL ) {
00407 buf[strlen(buf)-1] = '\0';
00408 kb_parse_command( buf , cmds , NULL);
00409 }
00410 }
00411 gettimeofday(&fintime, NULL);
00412 timersub(&fintime, &starttime, &fintime);
00413 printf("Total test time is %ds %dus\n", fintime.tv_sec , fintime.tv_usec);
00414
00415
00416 free(LRF_SN); free(KBA_SN);
00417
00418
00419 }