#include <stdio.h>#include <stdlib.h>#include <string.h>#include <ctype.h>#include <signal.h>#include <math.h>#include "posemath.h"#include "tc.h"#include "tp.h"Include dependency graph for testtp.c:
Go to the source code of this file.
Defines | |
| #define | MAX_NUMBERS 8 |
Functions | |
| void | extMotDout (unsigned char byte) |
| char * | dashOption (int argc, char *argv[], char *dashString) |
| void | quit (int sig) |
| int | scanNumbers (char *string, double *numbers, int max) |
| int | anyprintable (const char *string) |
| int | main (int argc, char *argv[]) |
Variables | |
| int | done = 0 |
| int | stopInstead = 0 |
|
|
|
|
|
Definition at line 93 of file testtp.c. 00094 {
00095 int cnt = 0;
00096 char c;
00097
00098 while (0 != (c = string[cnt++]))
00099 if (!isspace(c))
00100 return 1;
00101 return 0;
00102 }
|
|
||||||||||||||||
|
Definition at line 37 of file testtp.c. Referenced by main().
00038 {
00039 int t;
00040
00041 /* start comparison after argv[0] (program name) and end at
00042 second from last, since if it's last nothing can come after */
00043 for (t = 1; t < argc - 1; t++)
00044 {
00045 if (! strcmp(argv[t], dashString))
00046 {
00047 return argv[t+1];
00048 }
00049 }
00050 return NULL;
00051 }
|
|
|
Definition at line 27 of file testtp.c. Referenced by emcmotCommandHandler(), and tcRunCycle().
00028 {
00029 return;
00030 }
|
|
||||||||||||
|
Definition at line 104 of file testtp.c. 00105 {
00106 char *option;
00107 char input[80];
00108 char cmd[80];
00109 int printPrompt = 0;
00110 int running = 0; /* mode when numbers only are input */
00111 int stepping = 0; /* only run once */
00112 double numbers[MAX_NUMBERS]; /* space for number input data */
00113 int num; /* how many there are */
00114 int numNumbers = 0; /* established number of input numbers */
00115 FILE *fp;
00116
00117 /* user variables */
00118 TP_STRUCT tp;
00119 /* TC_STRUCT tcNew; unused */
00120 TC_STRUCT tcSpace[100];
00121 double cycleTime = 0.001;
00122 double vMax = 1.0;
00123 double aMax = 0.5;
00124 double wMax = 0.1;
00125 double wDotMax = 0.05;
00126 EmcPose goalPos;
00127 EmcPose pos,pos1,pos2;
00128 double time = 0.0;
00129 double vScale = 1.0;
00130 PmCartesian vTran;
00131 PmCartesian aTran;
00132 double aTranMag =0.0,vTranMag=0.0;
00133 double aTranMagMax =0.0, vTranMagMax=0.0;
00134 double aRotMag =0.0,vRotMag=0.0;
00135 double aRotMagMax =0.0, vRotMagMax=0.0;
00136 int circ_turn=0;
00137 PmCartesian circ_center;
00138 PmCartesian circ_normal;
00139
00140 circ_normal.x = 0.0;
00141 circ_normal.y = 0.0;
00142 circ_normal.z = 1.0;
00143
00144 tpCreate(&tp, 100, tcSpace);
00145
00146 /* check command line args-- valid ones are
00147 -t <cycle time>
00148 -v <v max>
00149 -a <a max>
00150 -f <file>
00151 */
00152 option = dashOption(argc, argv, "-t");
00153 if (NULL != option)
00154 {
00155 if (1 != sscanf(option, "%lf", &cycleTime))
00156 {
00157 printf("invalid cycle time: %s\n", option);
00158 exit(1);
00159 }
00160 }
00161
00162 option = dashOption(argc, argv, "-v");
00163 if (NULL != option)
00164 {
00165 if (1 != sscanf(option, "%lf", &vMax))
00166 {
00167 printf("invalid max velocity: %s\n", option);
00168 exit(1);
00169 }
00170 }
00171
00172 option = dashOption(argc, argv, "-a");
00173 if (NULL != option)
00174 {
00175 if (1 != sscanf(option, "%lf", &aMax))
00176 {
00177 printf("invalid max velocity: %s\n", option);
00178 exit(1);
00179 }
00180 }
00181
00182 option = dashOption(argc, argv, "-f");
00183 if (NULL != option)
00184 {
00185 /* filename is passed-- open it */
00186 if (NULL == (fp = fopen(option, "r")))
00187 {
00188 printf("can't open %s\n", option);
00189 exit(1);
00190 }
00191 }
00192 else
00193 {
00194 fp = stdin; /* read from stdin instead */
00195 }
00196
00197 /* trap ^C */
00198 signal(SIGINT, quit);
00199
00200 tpSetCycleTime(&tp, cycleTime);
00201 tpSetVmax(&tp, vMax);
00202 tpSetVlimit(&tp, vMax*1.2);
00203 tpSetAmax(&tp, aMax);
00204 tpSetWmax(&tp, wMax);
00205 tpSetWDotmax(&tp, wDotMax);
00206 tpSetVscale(&tp,1.0);
00207
00208 pos.tran.x = pos.tran.y = pos.tran.z = 0.0;
00209 pos.a = pos.b = pos.c = 0.0;
00210 pos1.tran.x = pos1.tran.y = pos1.tran.z = 0.0;
00211 pos1.a = pos1.b = pos1.c = 0.0;
00212 pos2.tran.x = pos2.tran.y = pos2.tran.z = 0.0;
00213 pos2.a = pos2.b = pos2.c = 0.0;
00214 goalPos.tran.x = goalPos.tran.y = goalPos.tran.z = 0.0;
00215 goalPos.a = goalPos.b = goalPos.c = 0.0;
00216 tpSetPos(&tp,goalPos);
00217
00218
00219 /* loop on input */
00220 while (! feof(fp) &&
00221 ! done)
00222 {
00223 if (printPrompt)
00224 {
00225 printf("tp> ");
00226 fflush(stdout);
00227 }
00228 if (NULL == fgets(input, 80, fp))
00229 {
00230 break;
00231 }
00232
00233 /* check for a number first */
00234 num = scanNumbers(input, numbers, MAX_NUMBERS);
00235 if (num > 0)
00236 {
00237 if (numNumbers == 0)
00238 {
00239 numNumbers = num;
00240 }
00241 else if (numNumbers != num)
00242 {
00243 printf("need %d numbers\n", numNumbers);
00244 /* ignore 'em */
00245 continue;
00246 }
00247
00248 tpSetVmax(&tp, vMax);
00249 tpSetVlimit(&tp, vMax*1.2);
00250 tpSetAmax(&tp, aMax);
00251 tpSetVscale(&tp,1.0);
00252
00253 /* set goal pos */
00254 goalPos.tran.x = numbers[0];
00255 if( num > 1 ) goalPos.tran.y = numbers[1];
00256 if( num > 2 ) goalPos.tran.z = numbers[2];
00257 if( num > 3 ) goalPos.a = numbers[3];
00258 if( num > 4 ) goalPos.b = numbers[4];
00259 if( num > 5 ) goalPos.c = numbers[5];
00260
00261
00262 /* add new traj to ring */
00263 if (-1 == tpAddLine(&tp, goalPos))
00264 {
00265 fprintf(stderr, "motion queue full\n");
00266 }
00267 }
00268 else
00269 {
00270 /* not a number-- if we're running, stop us so we can process input */
00271 running = 0;
00272
00273 /* read first arg in */
00274 cmd[0] = 0;
00275 sscanf(input, "%s", cmd);
00276
00277 /* match it in the big if loop */
00278 if (! strncmp(cmd, "help", 1) ||
00279 ! strcmp(cmd, "?"))
00280 {
00281 printf("? or help\tprint help\n");
00282 printf(">\ttoggle prompt on or off\n");
00283 printf(";\tcomment follows\n");
00284 printf("q(uit)\tquit\n");
00285 printf("r(un)\trun\n");
00286 printf(".\ttoggle single step\n");
00287 printf("sh(ow)\tshow status\n");
00288 printf("abort\tabort moves\n");
00289 printf("set t <time step> | v <vel> | vs <scale> | a <accel>\tset params\n");
00290 }
00291 else if (! strcmp(cmd, ">"))
00292 {
00293 printPrompt = !printPrompt;
00294 }
00295 else if (! strcmp(cmd, ";"))
00296 {
00297 /* comment */
00298 }
00299 else if (! strncmp(cmd, "quit", 1))
00300 {
00301 done = 1;
00302 }
00303 else if (! strncmp(cmd, "run", 1))
00304 {
00305 running = 1;
00306 }
00307 else if (! strcmp(cmd, "."))
00308 {
00309 stepping = ! stepping;
00310 running = 0;
00311 }
00312 else if (! strncmp(cmd, "show", 2))
00313 {
00314 tpPrint(&tp);
00315 }
00316 else if (! strcmp(cmd, "abort"))
00317 {
00318 tpAbort(&tp);
00319 }
00320 else if (! strcmp(cmd, "set"))
00321 {
00322 sscanf(input, "%*s %s", cmd);
00323 if (! strcmp(cmd, "t"))
00324 {
00325 if (1 != sscanf(input, "%*s %*s %lf", &cycleTime))
00326 {
00327 /* invalid parameter */
00328 }
00329 else
00330 {
00331 /* read it okay */
00332 tpSetCycleTime(&tp, cycleTime);
00333 }
00334 }
00335 else if (! strcmp(cmd, "v"))
00336 {
00337 if (1 != sscanf(input, "%*s %*s %lf", &vMax))
00338 {
00339 /* invalid parameter */
00340 }
00341 else
00342 {
00343 /* read it okay */
00344 tpSetVmax(&tp, vMax);
00345 tpSetVlimit(&tp, vMax*1.2);
00346 }
00347 }
00348 else if (! strcmp(cmd, "vs"))
00349 {
00350 /* set velocity scale factor */
00351 if (1 != sscanf(input, "%*s %*s %lf", &vScale))
00352 {
00353 /* bad value */
00354 printf("bad value for v scale\n");
00355 }
00356 else
00357 {
00358 /* read it okay */
00359 tpSetVscale(&tp, vScale);
00360 }
00361 }
00362 else if (! strcmp(cmd, "a"))
00363 {
00364 if (1 != sscanf(input, "%*s %*s %lf", &aMax))
00365 {
00366 /* invalid parameter */
00367 }
00368 else
00369 {
00370 /* read it okay */
00371 tpSetAmax(&tp, aMax);
00372 }
00373 }
00374 else
00375 {
00376 /* invalid parameter */
00377 printf("syntax: set t <time> | v <vel> | a <acc>\n");
00378 }
00379 }
00380 else if (! strcmp(cmd, "circle"))
00381 {
00382 sscanf(input, "%*s %d %lf %lf %lf %lf %lf %lf",
00383 &circ_turn, &circ_center.x, &circ_center.y, &circ_center.z,
00384 &goalPos.tran.x,&goalPos.tran.y, &goalPos.tran.z);
00385 goalPos.a = goalPos.b = goalPos.c = 0.0;
00386 tpAddCircle(&tp,goalPos,circ_center, circ_normal, circ_turn);
00387
00388 }
00389 else
00390 {
00391 if (anyprintable(input))
00392 {
00393 printf("?\n");
00394 }
00395 else
00396 {
00397 /* blank line was typed */
00398 }
00399 } /* end of big-if input matching */
00400
00401 } /* end of non-number input processing */
00402
00403 /* out of the input cycle-- interpolate if running */
00404 if (running)
00405 {
00406 /* sets flag for SIGINT signal handler */
00407 stopInstead = 1;
00408
00409 while (! tpIsDone(&tp))
00410 {
00411 tpRunCycle(&tp);
00412 pos = tpGetPos(&tp);
00413 time += cycleTime;
00414
00415
00416 if (numNumbers > 0) printf("time:%6.6f\tposition:\t%6.6f",time, pos.tran.x);
00417 if (numNumbers > 1) printf("\t%6.6f",pos.tran.y);
00418 if (numNumbers > 2) printf("\t%6.6f",pos.tran.z);
00419 if (numNumbers > 3) printf("\t%6.6f",pos.a);
00420 if (numNumbers > 4) printf("\t%6.6f",pos.b);
00421 if (numNumbers > 5) printf("\t%6.6f",pos.c);
00422 if (numNumbers > 0) printf("\n");
00423
00424
00425 vTran.x = (pos.tran.x - pos1.tran.x)/cycleTime;
00426 vTran.y = (pos.tran.y - pos1.tran.y)/cycleTime;
00427 vTran.z = (pos.tran.z - pos1.tran.z)/cycleTime;
00428
00429 if (numNumbers > 0)
00430 printf("time:%6.6f\tvelocity:\t%6.6f", time, vTran.x);
00431 if (numNumbers > 1)
00432 printf("\t%6.6f",vTran.y);
00433 if (numNumbers > 2)
00434 printf("\t%6.6f",vTran.z);
00435 if (numNumbers > 3)
00436 printf("\t%6.6f",(pos.a - pos1.a)/cycleTime);
00437 if (numNumbers > 4)
00438 printf("\t%6.6f",(pos.b - pos1.b)/cycleTime);
00439 if (numNumbers > 5)
00440 printf("\t%6.6f",(pos.c - pos1.c)/cycleTime);
00441 if (numNumbers > 0) printf("\n");
00442
00443 pmCartMag(vTran,&vTranMag);
00444 printf("vTranMag=%f\n",vTranMag);
00445 if(vTranMag > vTranMagMax)
00446 {
00447 vTranMagMax = vTranMag;
00448 }
00449 vRotMag = pmSqrt(
00450 pmSq(pos.a - pos1.a) +
00451 pmSq(pos.b - pos1.b) +
00452 pmSq(pos.c - pos1.c))/(cycleTime);
00453 printf("vRotMag=%f\n",vRotMag);
00454 if(vRotMag > vRotMagMax)
00455 {
00456 vRotMagMax = vRotMag;
00457 }
00458
00459 aTran.x = (pos.tran.x - 2* pos1.tran.x + pos2.tran.x)/(cycleTime*cycleTime);
00460 aTran.y = (pos.tran.y - 2* pos1.tran.y + pos2.tran.y)/(cycleTime*cycleTime);
00461 aTran.z = (pos.tran.z - 2* pos1.tran.z + pos2.tran.z)/(cycleTime*cycleTime);
00462
00463 if (numNumbers > 0)
00464 printf("time:%6.6f\taccelleration:\t%6.6f",
00465 time, aTran.x);
00466 if (numNumbers > 1)
00467 printf("\t%6.6f",aTran.y);
00468 if (numNumbers > 2)
00469 printf("\t%6.6f",aTran.z);
00470 if (numNumbers > 3)
00471 printf("\t%6.6f",(pos.a - 2*pos1.a + pos2.a)/(cycleTime*cycleTime));
00472 if (numNumbers > 4)
00473 printf("\t%6.6f",(pos.b - 2*pos1.b + pos2.b)/(cycleTime*cycleTime));
00474 if (numNumbers > 5)
00475 printf("\t%6.6f",(pos.c - 2*pos1.c + pos2.c)/(cycleTime*cycleTime));
00476 if (numNumbers > 0) printf("\n");
00477
00478
00479 pmCartMag(aTran,&aTranMag);
00480 printf("aTranMag=%f\n",aTranMag);
00481 if(aTranMag > aTranMagMax)
00482 {
00483 aTranMagMax = aTranMag;
00484 }
00485 aRotMag = pmSqrt(
00486 pmSq(pos.a - 2*pos1.a + pos2.a) +
00487 pmSq(pos.b - 2*pos1.b + pos2.b) +
00488 pmSq(pos.c - 2*pos1.c + pos2.c))/(cycleTime*cycleTime);
00489 printf("aRotMag=%f\n",aRotMag);
00490 if(aRotMag > aRotMagMax)
00491 {
00492 aRotMagMax = aRotMag;
00493 }
00494 pos2 = pos1;
00495 pos1 = pos;
00496
00497
00498
00499 /* interactive stopping test-- add yours also if needed */
00500 if (stepping ||
00501 ! stopInstead)
00502 {
00503 running = 0;
00504 break; /* out of while ! isDecel() */
00505 }
00506 }
00507 } /* end of if running */
00508
00509 } /* end of while fp */
00510 printf("vTranMagMax=%f\n",vTranMagMax);
00511 printf("aTranMagMax=%f\n",aTranMagMax);
00512 printf("vRotMagMax=%f\n",vRotMagMax);
00513 printf("aRotMagMax=%f\n",aRotMagMax);
00514
00515 exit(0);
00516 }
|
|
|
Definition at line 56 of file testtp.c. 00057 {
00058 if (stopInstead)
00059 {
00060 stopInstead = 0;
00061 }
00062 else
00063 {
00064 done = 1;
00065 }
00066
00067 /* restore this signal handler */
00068 signal(SIGINT, quit);
00069
00070 return;
00071 }
|
|
||||||||||||||||
|
Definition at line 73 of file testtp.c. 00074 {
00075 char *ptrend, *ptr = string;
00076 int count = 0;
00077
00078 while (count < max)
00079 {
00080 numbers[count] = strtod(ptr, &ptrend);
00081 if (ptrend == ptr)
00082 {
00083 return count;
00084 }
00085 ptr = ptrend;
00086 count++;
00087 }
00088
00089 return count;
00090 }
|
|
|
|
|
|
|
1.2.11.1 written by Dimitri van Heesch,
© 1997-2001