00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <string.h>
00041 #include <ctype.h>
00042 #include "_timer.h"
00043 #include "emcmot.h"
00044 #include "usrmotintf.h"
00045 #include "posemath.h"
00046 #include "pid.h"
00047 #include "getinput.h"
00048 #include "emcmotcfg.h"
00049 #include "emcmotglb.h"
00050
00051
00052 #ifndef __GNUC__
00053 #ifndef __attribute__
00054 #define __attribute__(x)
00055 #endif
00056 #endif
00057
00058 static char __attribute__((unused)) ident[] = "$Id: usrmot.c,v 1.11 2001/07/31 15:47:45 wshackle Exp $";
00059
00060
00061 #define INPUTLEN 256
00062
00063
00064 #define MAX_NUMBERS 8
00065
00066
00067 #define SLEEP_SECS 0.100
00068
00069
00070 static int anyprintable(const char *string)
00071 {
00072 int cnt = 0;
00073 char c;
00074
00075 while (0 != (c = string[cnt++]))
00076 if (!isspace(c))
00077 return 1;
00078 return 0;
00079 }
00080
00081 static char * skipWhite(char *s)
00082 {
00083 while (isspace(*s))
00084 {
00085 s++;
00086 }
00087 return s;
00088 }
00089
00090 static char * skipNonwhite(char *s)
00091 {
00092 while (! isspace(*s))
00093 {
00094 s++;
00095 }
00096 return s;
00097 }
00098
00099 static int scanNumbers(char *string, double *numbers, int max)
00100 {
00101 char *ptr = string;
00102 int count = 0;
00103
00104 while (count < max)
00105 {
00106 if (1 != sscanf(ptr, "%lf", &numbers[count]))
00107 {
00108 return count;
00109 }
00110 count++;
00111 ptr = skipNonwhite(ptr);
00112 ptr = skipWhite(ptr);
00113 }
00114
00115 return count;
00116 }
00117
00118
00119
00120
00121
00122
00123 int emcmotGetArgs(int argc, char *argv[])
00124 {
00125 int t;
00126
00127
00128 for (t = 1; t < argc; t++) {
00129 if (!strcmp(argv[t], "-ini")) {
00130 if (t == argc - 1) {
00131 return -1;
00132 }
00133 else {
00134 strcpy(EMCMOT_INIFILE, argv[t+1]);
00135 t++;
00136 }
00137 }
00138
00139 }
00140
00141 return 0;
00142 }
00143
00144
00145
00146
00147 int main(int argc, char *argv[])
00148 {
00149 EMCMOT_COMMAND emcmotCommand;
00150 EMCMOT_STATUS emcmotStatus;
00151 EMCMOT_CONFIG emcmotConfig;
00152 EMCMOT_DEBUG emcmotDebug;
00153 char input[INPUTLEN];
00154 char cmd[INPUTLEN];
00155 char errorString[EMCMOT_ERROR_LEN];
00156 int valid;
00157 int done;
00158 int nchars;
00159 int printPrompt;
00160 int disablePrompt = 0;
00161 double numbers[MAX_NUMBERS];
00162 int num;
00163 int numNumbers = 0;
00164 char filename[INPUTLEN];
00165 int linenum;
00166 FILE *fp;
00167 int lastPrint = 0;
00168 int statconfigdebug = 0;
00169 int motionId = 0;
00170 int axis;
00171 int errCode;
00172 char compfile[INPUTLEN];
00173 double alter;
00174
00175
00176
00177 printf("sizeof(EMCMOT_COMMAND) = %d\n", sizeof(EMCMOT_COMMAND));
00178 printf("sizeof(EMCMOT_STATUS) = %d\n", sizeof(EMCMOT_STATUS));
00179 printf("sizeof(EMCMOT_CONFIG) = %d\n", sizeof(EMCMOT_CONFIG));
00180 printf("sizeof(EMCMOT_DEBUG) = %d\n", sizeof(EMCMOT_DEBUG));
00181 printf("sizeof(EMCMOT_ERROR) = %d\n", sizeof(EMCMOT_ERROR));
00182 printf("sizeof(EMCMOT_LOG) = %d\n", sizeof(EMCMOT_LOG));
00183 printf("sizeof(EMCMOT_STRUCT) = %d\n", sizeof(EMCMOT_STRUCT));
00184
00185
00186 emcmotGetArgs(argc, argv);
00187
00188
00189 if (-1 == usrmotIniLoad(EMCMOT_INIFILE)) {
00190 fprintf(stderr, "can't load ini file %s\n", EMCMOT_INIFILE);
00191 exit(1);
00192 }
00193
00194
00195 if (-1 == usrmotInit()) {
00196 fprintf(stderr, "can't initialize comm interface\n");
00197 exit(1);
00198 }
00199
00200 emcmotCommand.pos.a = 0.0;
00201 emcmotCommand.pos.b = 0.0;
00202 emcmotCommand.pos.c = 0.0;
00203
00204
00205 done = 0;
00206 printPrompt = 1;
00207 while (! feof(stdin) &&
00208 ! done) {
00209
00210 while (0 == usrmotReadEmcmotError(errorString)) {
00211 printf("error: %s\n", errorString);
00212 }
00213
00214
00215 if (printPrompt) {
00216 if (! disablePrompt) {
00217 printf("motion> ");
00218 fflush(stdout);
00219 }
00220 printPrompt = 0;
00221 }
00222
00223
00224 nchars = getinput(input, INPUTLEN);
00225 if (nchars > 0) {
00226 printPrompt = 1;
00227 }
00228 else if (nchars == -1) {
00229
00230 esleep(SLEEP_SECS);
00231 continue;
00232 }
00233 else {
00234
00235 break;
00236 }
00237
00238
00239 num = scanNumbers(input, numbers, MAX_NUMBERS);
00240 if (num > 0) {
00241 if (numNumbers == 0) {
00242 numNumbers = num;
00243 }
00244 else if (numNumbers != num) {
00245 fprintf(stderr, "need %d numbers\n", numNumbers);
00246
00247 continue;
00248 }
00249
00250
00251
00252 emcmotCommand.command = EMCMOT_SET_LINE;
00253 emcmotCommand.pos.tran.x = numbers[0];
00254 emcmotCommand.pos.tran.y = numbers[1];
00255 emcmotCommand.pos.tran.z = numbers[2];
00256 emcmotCommand.id = motionId++;
00257
00258 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00259 fprintf(stderr, "Can't send a command to RT-task\n");
00260 }
00261 }
00262 else {
00263
00264 cmd[0] = 0;
00265 sscanf(input, "%s", cmd);
00266
00267
00268 if (! strcmp(cmd, "help") ||
00269 ! strcmp(cmd, "?")) {
00270 printf("? or help\tprint help\n");
00271 printf(">\ttoggle prompt on or off\n");
00272 printf(";\tcomment follows\n");
00273 printf("quit\tquit\n");
00274 printf("show {pids} {flags} {limits} {scales} {times}\tshow status\n");
00275 printf("show debug <screen>\tshow debug\n");
00276 printf("show config <screen>\tshow config\n");
00277 printf("free\tset mode to free\n");
00278 printf("teleop\tset mode to teleop\n");
00279 printf("coord\tset mode to coordinated\n");
00280 printf("pause\tpause motion controller\n");
00281 printf("resume\tresume motion controller\n");
00282 printf("a\tabort motion controller\n");
00283 printf("scale <0..1>\tscale velocity\n");
00284 printf("enable | disable\tenable or disable motion controller\n");
00285 printf("jog <axis> + | -\tjog axis pos or neg\n");
00286 printf("jog <axis> + | -\tjog axis pos or neg\n");
00287 printf("id <num> \tset base id for subsequent moves\n");
00288 printf("<x> <y> <z>\tput motion on queue\n");
00289 printf("load <file>\tput motions from file on queue\n");
00290 printf("cw <x> <y> <z> <cx> <cy> <cz> <turn>\tput CW circle on queue\n");
00291 printf("ccw <x> <y> <z> <cx> <cy> <cz> <turn>\tput CCW circle on queue\n");
00292 printf("set t <traj t> | s <servo t> | v <vel> | a <acc>\tset params\n");
00293 printf("oscale <axis 0..n-1> <a> <b>\traw[n] = a(out[n]-b)\n");
00294 printf("iscale <axis 0..n-1> <a> <b>\tin[n] = a(raw[n]-b)\n");
00295 printf("pol <axis 0..n-1> <enable nhl phl homedir homesw fault> <0 1>\n");
00296 printf("pid <axis 0..n-1> <ini file>\tset PID gains for motor n\n");
00297 printf("limit <axis> <min> <max>\tset position limits\n");
00298 printf("clamp <axis> <min> <max>\tset output limits\n");
00299 printf("ferror <axis> <value>\tset max following error\n");
00300 printf("live <axis 0..n-1>\tenable amp n\n");
00301 printf("kill <axis 0..n-1>\tkill amp n\n");
00302 printf("activate <axis 0..n-1>\tactivate axis n\n");
00303 printf("deactivate <axis 0..n-1>\tdeactivate axis n\n");
00304 printf("log open <type ...> | start | stop | close | dump <file>\tlog data\n");
00305 printf("dac <axis 0..n-1> <-10.0 .. 10.0>\toutput value to DAC\n");
00306 printf("home <axis 0..n-1>\thome axis\n");
00307 printf("nolim \toverride hardware limits\n");
00308 printf("wd on | off\tenable or disable watchdog toggle\n");
00309 #ifdef ENABLE_PROBING
00310 printf("probe <x> <y> <z>\tMove toward x,y,z, if probe is tripped on the way the probe position will be updated and motion stopped.\n");
00311 printf("probeclear\tClear the probeTripped status flag.\n");
00312 printf("probeindex <index>\tSet which input is checked for probe status.\n");
00313 printf("probepolarity <polarity>\tSet whether a probe is tripped on a 0 or 1.\n");
00314 #endif
00315 }
00316 else if (! strcmp(cmd, ">")) {
00317 disablePrompt = !disablePrompt;
00318 }
00319 else if (! strcmp(cmd, ";")) {
00320
00321 }
00322 else if (! strcmp(cmd, "quit")) {
00323 done = 1;
00324 }
00325 else if (! strcmp(cmd, "show")) {
00326 if (1 == sscanf(input, "%*s %s", cmd)) {
00327 statconfigdebug = 0;
00328 if (! strcmp(cmd, "pids")) {
00329 lastPrint = 1;
00330 statconfigdebug = 2;
00331 }
00332 else if (! strcmp(cmd, "flags")) {
00333 lastPrint = 2;
00334 }
00335 else if (! strcmp(cmd, "limits")) {
00336 lastPrint = 3;
00337 statconfigdebug = 2;
00338 }
00339 else if (! strcmp(cmd, "scales")) {
00340 lastPrint = 4;
00341 }
00342 else if (! strcmp(cmd, "times")) {
00343 lastPrint = 5;
00344 statconfigdebug = 1;
00345 }
00346 else if (! strcmp(cmd, "stat")) {
00347 statconfigdebug = 0;
00348 lastPrint = strtol(strstr(input,"stat")+4,0,0);
00349 }
00350 else if (! strcmp(cmd, "debug")) {
00351 statconfigdebug = 1;
00352 lastPrint = strtol(strstr(input,"debug")+5,0,0);
00353 }
00354 else if (! strcmp(cmd, "config")) {
00355 statconfigdebug = 2;
00356 lastPrint = strtol(strstr(input,"config")+6,0,0);
00357 }
00358 else {
00359
00360 printf("syntax: show {pids} {flags} {limits} {scales} {times}\n");
00361 continue;
00362 }
00363 }
00364 else {
00365 lastPrint = 0;
00366 statconfigdebug = 0;
00367 }
00368
00369
00370 switch (statconfigdebug) {
00371 case 0:
00372 if (0 == (errCode = usrmotReadEmcmotStatus(&emcmotStatus))) {
00373 usrmotPrintEmcmotStatus(emcmotStatus, lastPrint);
00374 }
00375 else {
00376 fprintf(stderr, "can't read status: %s\n",
00377 errCode == EMCMOT_COMM_ERROR_CONNECT ? "EMCMOT_COMM_ERROR_CONNECT" :
00378 errCode == EMCMOT_COMM_ERROR_TIMEOUT ? "EMCMOT_COMM_ERROR_TIMEOUT" :
00379 errCode == EMCMOT_COMM_ERROR_COMMAND ? "EMCMOT_COMM_ERROR_COMMAND" :
00380 errCode == EMCMOT_COMM_SPLIT_READ_TIMEOUT ? "EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
00381 }
00382 break;
00383
00384 case 1:
00385 if (0 == (errCode = usrmotReadEmcmotDebug(&emcmotDebug))) {
00386 usrmotPrintEmcmotDebug(emcmotDebug, lastPrint);
00387 }
00388 else {
00389 fprintf(stderr, "can't read debug: %s\n",
00390 errCode == EMCMOT_COMM_ERROR_CONNECT ? "EMCMOT_COMM_ERROR_CONNECT" :
00391 errCode == EMCMOT_COMM_ERROR_TIMEOUT ? "EMCMOT_COMM_ERROR_TIMEOUT" :
00392 errCode == EMCMOT_COMM_ERROR_COMMAND ? "EMCMOT_COMM_ERROR_COMMAND" :
00393 errCode == EMCMOT_COMM_SPLIT_READ_TIMEOUT ? "EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
00394 }
00395 break;
00396
00397 case 2:
00398 if (0 == (errCode = usrmotReadEmcmotConfig(&emcmotConfig))) {
00399 usrmotPrintEmcmotConfig(emcmotConfig, lastPrint);
00400 }
00401 else {
00402 fprintf(stderr, "can't read config: %s\n",
00403 errCode == EMCMOT_COMM_ERROR_CONNECT ? "EMCMOT_COMM_ERROR_CONNECT" :
00404 errCode == EMCMOT_COMM_ERROR_TIMEOUT ? "EMCMOT_COMM_ERROR_TIMEOUT" :
00405 errCode == EMCMOT_COMM_ERROR_COMMAND ? "EMCMOT_COMM_ERROR_COMMAND" :
00406 errCode == EMCMOT_COMM_SPLIT_READ_TIMEOUT ? "EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
00407 }
00408 break;
00409 }
00410 }
00411 else if (! strcmp(cmd, "pause")) {
00412 emcmotCommand.command = EMCMOT_PAUSE;
00413 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00414 fprintf(stderr, "Can't send a command to RT-task\n");
00415 }
00416 }
00417 else if (! strcmp(cmd, "resume")) {
00418 emcmotCommand.command = EMCMOT_RESUME;
00419 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00420 fprintf(stderr, "Can't send a command to RT-task\n");
00421 }
00422 }
00423 else if (! strcmp(cmd, "a")) {
00424
00425
00426
00427 valid = 0;
00428 if (1 == sscanf(input, "%*s %d", &axis)) {
00429 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00430 fprintf(stderr, "bad axis %d to abort\n", axis);
00431 }
00432 else {
00433 emcmotCommand.axis = axis;
00434 valid = 1;
00435 }
00436 }
00437 else {
00438
00439
00440 valid = 1;
00441 }
00442
00443 if (valid) {
00444 emcmotCommand.command = EMCMOT_ABORT;
00445
00446 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00447 fprintf(stderr, "Can't send a command to RT-task\n");
00448 }
00449 }
00450 }
00451 else if (! strcmp(cmd, "scale")) {
00452 if (1 == sscanf(input, "%*s %lf",
00453 &emcmotCommand.scale)) {
00454 emcmotCommand.command = EMCMOT_SCALE;
00455 if (emcmotCommand.scale < 0.0) {
00456 emcmotCommand.scale = 0.0;
00457 }
00458 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00459 fprintf(stderr, "Can't send a command to RT-task\n");
00460 }
00461 }
00462 else {
00463
00464 printf("syntax: scale <0..1>\n");
00465 }
00466 }
00467 else if (! strcmp(cmd, "enable")) {
00468 emcmotCommand.command = EMCMOT_ENABLE;
00469 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00470 fprintf(stderr, "Can't send a command to RT-task\n");
00471 }
00472 }
00473 else if (! strcmp(cmd, "disable")) {
00474 emcmotCommand.command = EMCMOT_DISABLE;
00475 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00476 fprintf(stderr, "Can't send a command to RT-task\n");
00477 }
00478 }
00479 else if (! strcmp(cmd, "free")) {
00480 emcmotCommand.command = EMCMOT_FREE;
00481 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00482 fprintf(stderr, "Can't send a command to RT-task\n");
00483 }
00484 }
00485 else if (! strcmp(cmd, "teleop")) {
00486 emcmotCommand.command = EMCMOT_TELEOP;
00487 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00488 fprintf(stderr, "Can't send a command to RT-task\n");
00489 }
00490 }
00491 else if (! strcmp(cmd, "coord")) {
00492 emcmotCommand.command = EMCMOT_COORD;
00493 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00494 fprintf(stderr, "Can't send a command to RT-task\n");
00495 }
00496 }
00497 else if (! strcmp(cmd, "jog")) {
00498 if (2 == sscanf(input, "%*s %d %s",
00499 &emcmotCommand.axis, cmd)) {
00500 emcmotCommand.command = EMCMOT_JOG_CONT;
00501 if (cmd[0] == '+') {
00502 emcmotCommand.vel = emcmotStatus.vel;
00503 }
00504 else if (cmd[0] == '-') {
00505 emcmotCommand.vel = - emcmotStatus.vel;
00506 }
00507 else {
00508 fprintf(stderr, "syntax: jog <axis> + | -\n");
00509 break;
00510 }
00511
00512 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00513 fprintf(stderr, "Can't send a command to RT-task\n");
00514 }
00515 }
00516 else {
00517 fprintf(stderr, "syntax: jog <axis> + | -\n");
00518 }
00519 }
00520 else if (! strcmp(cmd, "id")) {
00521 int newId;
00522 if (1 == sscanf(input, "%*s %d", &newId)) {
00523 motionId = newId;
00524 printf("setting id to %d\n", motionId);
00525 }
00526 else {
00527 fprintf(stderr, "syntax: id <num>\n");
00528 }
00529 }
00530 else if (! strcmp(cmd, "load")) {
00531 if (1 == sscanf(input, "%*s %s", filename)) {
00532 if (NULL != (fp = fopen(filename, "r"))) {
00533 linenum = 0;
00534 while (! feof(fp)) {
00535 if (NULL != fgets(input, INPUTLEN, fp)) {
00536 linenum++;
00537 if (3 == sscanf(input, "%lf %lf %lf",
00538 &emcmotCommand.pos.tran.x,
00539 &emcmotCommand.pos.tran.y,
00540 &emcmotCommand.pos.tran.z)) {
00541 printf("sending %f %f %f\n",
00542 emcmotCommand.pos.tran.x,
00543 emcmotCommand.pos.tran.y,
00544 emcmotCommand.pos.tran.z);
00545 emcmotCommand.command = EMCMOT_SET_LINE;
00546 emcmotCommand.id = motionId++;
00547 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00548 fprintf(stderr, "Can't send a command to RT-task\n");
00549 }
00550 }
00551 else {
00552
00553 if (anyprintable(input)) {
00554 fprintf(stderr, "bad input on line %d of file %s\n",
00555 linenum, filename);
00556 fclose(fp);
00557 break;
00558 }
00559 }
00560 }
00561 else {
00562
00563 fclose(fp);
00564 break;
00565 }
00566 }
00567 }
00568 else {
00569 fprintf(stderr, "can't open %s\n", filename);
00570 }
00571 }
00572 else {
00573 fprintf(stderr, "syntax: load <filename>\n");
00574 }
00575 }
00576 else if (! strcmp(cmd, "cw") ||
00577 ! strcmp(cmd, "ccw")) {
00578 if (7 == sscanf(input, "%*s %lf %lf %lf %lf %lf %lf %d",
00579 &emcmotCommand.pos.tran.x,
00580 &emcmotCommand.pos.tran.y,
00581 &emcmotCommand.pos.tran.z,
00582 &emcmotCommand.center.x,
00583 &emcmotCommand.center.y,
00584 &emcmotCommand.center.z,
00585 &emcmotCommand.turn)) {
00586 emcmotCommand.command = EMCMOT_SET_CIRCLE;
00587 emcmotCommand.normal.x = 0.0;
00588 emcmotCommand.normal.y = 0.0;
00589 if (!strcmp(cmd, "cw")) {
00590 emcmotCommand.normal.z = -1.0;
00591 }
00592 else {
00593 emcmotCommand.normal.z = 1.0;
00594 }
00595 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00596 fprintf(stderr, "Can't send a command to RT-task\n");
00597 }
00598 }
00599 else {
00600 fprintf(stderr, "syntax: cw <x> <y> <z> <cx> <cy> <cz> <turn>\n");
00601 }
00602 }
00603 else if (! strcmp(cmd, "set")) {
00604 sscanf(input, "%*s %s", cmd);
00605 if (! strcmp(cmd, "t")) {
00606 if (1 != sscanf(input, "%*s %*s %lf",
00607 &emcmotCommand.cycleTime)) {
00608
00609 fprintf(stderr, "bad value for cycle time\n");
00610 }
00611 else {
00612 emcmotCommand.command = EMCMOT_SET_TRAJ_CYCLE_TIME;
00613 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00614 fprintf(stderr, "Can't send a command to RT-task\n");
00615 }
00616 }
00617 }
00618 else if (! strcmp(cmd, "s")) {
00619 if (1 != sscanf(input, "%*s %*s %lf", &emcmotCommand.cycleTime)) {
00620
00621 fprintf(stderr, "bad value for interpolation rate\n");
00622 }
00623 else {
00624 emcmotCommand.command = EMCMOT_SET_SERVO_CYCLE_TIME;
00625 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00626 fprintf(stderr, "Can't send a command to RT-task\n");
00627 }
00628 }
00629 }
00630 else if (! strcmp(cmd, "v")) {
00631 if (1 != sscanf(input, "%*s %*s %lf", &emcmotCommand.vel)) {
00632
00633 fprintf(stderr, "bad value for velocity\n");
00634 }
00635 else {
00636 emcmotCommand.command = EMCMOT_SET_VEL;
00637 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00638 fprintf(stderr, "Can't send a command to RT-task\n");
00639 }
00640 }
00641 }
00642 else if (! strcmp(cmd, "a")) {
00643 if (1 != sscanf(input, "%*s %*s %lf", &emcmotCommand.acc)) {
00644
00645 fprintf(stderr, "bad value for acceleration\n");
00646 }
00647 else {
00648 emcmotCommand.command = EMCMOT_SET_ACC;
00649 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00650 fprintf(stderr, "Can't send a command to RT-task\n");
00651 }
00652 }
00653 }
00654 else {
00655
00656 printf("syntax: set t <traj t> | s <servo t> | v <vel> | a <acc>\n");
00657 }
00658 }
00659 else if (! strcmp(cmd, "oscale")) {
00660 if (3 != sscanf(input, "%*s %d %lf %lf",
00661 &emcmotCommand.axis,
00662 &emcmotCommand.scale,
00663 &emcmotCommand.offset)) {
00664 printf("syntax: oscale <axis 0..n-1> <a> <b>\n");
00665 }
00666 else {
00667 emcmotCommand.command = EMCMOT_SET_OUTPUT_SCALE;
00668 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00669 fprintf(stderr, "Can't send a command to RT-task\n");
00670 }
00671 }
00672 }
00673 else if (! strcmp(cmd, "iscale")) {
00674 if (3 != sscanf(input, "%*s %d %lf %lf",
00675 &emcmotCommand.axis,
00676 &emcmotCommand.scale,
00677 &emcmotCommand.offset)) {
00678 printf("syntax: iscale <axis 0..n-1> <a> <b>\n");
00679 }
00680 else {
00681 emcmotCommand.command = EMCMOT_SET_INPUT_SCALE;
00682 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00683 fprintf(stderr, "Can't send a command to RT-task\n");
00684 }
00685 }
00686 }
00687 else if (! strcmp(cmd, "pol")) {
00688 if (3 != sscanf(input, "%*s %d %s %d",
00689 &emcmotCommand.axis,
00690 cmd,
00691 &emcmotCommand.level)) {
00692 printf("syntax: pol <axis 0..n-1> <enable nhl phl homedir homesw fault> <0 1>\n");
00693 }
00694 else {
00695 valid = 1;
00696
00697 if (! strcmp(cmd, "enable")) {
00698 emcmotCommand.axisFlag = EMCMOT_AXIS_ENABLE_BIT;
00699 }
00700 else if (! strcmp(cmd, "nhl")) {
00701 emcmotCommand.axisFlag = EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;
00702 }
00703 else if (! strcmp(cmd, "phl")) {
00704 emcmotCommand.axisFlag = EMCMOT_AXIS_MAX_HARD_LIMIT_BIT;
00705 }
00706 else if (! strcmp(cmd, "homedir")) {
00707 emcmotCommand.axisFlag = EMCMOT_AXIS_HOMING_BIT;
00708 }
00709 else if (! strcmp(cmd, "homesw")) {
00710 emcmotCommand.axisFlag = EMCMOT_AXIS_HOME_SWITCH_BIT;
00711 }
00712 else if (! strcmp(cmd, "fault")) {
00713 emcmotCommand.axisFlag = EMCMOT_AXIS_FAULT_BIT;
00714 }
00715 else {
00716 valid = 0;
00717 }
00718
00719 if (valid) {
00720 emcmotCommand.command = EMCMOT_SET_POLARITY;
00721 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00722 fprintf(stderr, "Can't send a command to RT-task\n");
00723 }
00724 }
00725 else {
00726 printf("syntax: pol <axis 0..n-1> <enable nhl phl homedir homesw fault> <0 1>\n");
00727 }
00728 }
00729 }
00730 else if (! strcmp(cmd, "pid")) {
00731 if (1 != sscanf(input, "%*s %d", &emcmotCommand.axis) ||
00732 emcmotCommand.axis < 0 ||
00733 emcmotCommand.axis >= EMCMOT_MAX_AXIS ||
00734 1 != sscanf(input, "%*s %*s %s", filename)) {
00735 printf("syntax: pid <n> <ini file>\n");
00736 }
00737 else {
00738
00739 if (0 != pidIniLoad(&emcmotCommand.pid, filename)) {
00740 fprintf(stderr, "error loading pid params from %s\n", filename);
00741 }
00742 else {
00743 emcmotCommand.command = EMCMOT_SET_PID;
00744 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00745 fprintf(stderr, "Can't send a command to RT-task\n");
00746 }
00747 }
00748 }
00749 }
00750 else if (! strcmp(cmd, "limit")) {
00751 if (3 != sscanf(input, "%*s %d %lf %lf",
00752 &emcmotCommand.axis,
00753 &emcmotCommand.minLimit,
00754 &emcmotCommand.maxLimit)) {
00755 printf("syntax: limit <axis> <min> <max>\n");
00756 }
00757 else {
00758 emcmotCommand.command = EMCMOT_SET_POSITION_LIMITS;
00759 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00760 fprintf(stderr, "Can't send a command to RT-task\n");
00761 }
00762 }
00763 }
00764 else if (! strcmp(cmd, "clamp")) {
00765 if (3 != sscanf(input, "%*s %d %lf %lf",
00766 &emcmotCommand.axis,
00767 &emcmotCommand.minLimit,
00768 &emcmotCommand.maxLimit)) {
00769 printf("syntax: clamp <axis> <min> <max>\n");
00770 }
00771 else {
00772 emcmotCommand.command = EMCMOT_SET_OUTPUT_LIMITS;
00773 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00774 fprintf(stderr, "Can't send a command to RT-task\n");
00775 }
00776 }
00777 }
00778 else if (! strcmp(cmd, "ferror")) {
00779 if (2 != sscanf(input, "%*s %d %lf",
00780 &emcmotCommand.axis,
00781 &emcmotCommand.maxFerror)) {
00782 printf("syntax: ferror <axis> <ferror>\n");
00783 }
00784 else {
00785 emcmotCommand.command = EMCMOT_SET_MAX_FERROR;
00786 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00787 fprintf(stderr, "Can't send a command to RT-task\n");
00788 }
00789 }
00790 }
00791 else if (! strcmp(cmd, "live")) {
00792 if (1 != sscanf(input, "%*s %d", &emcmotCommand.axis) ||
00793 emcmotCommand.axis < 0 ||
00794 emcmotCommand.axis >= EMCMOT_MAX_AXIS) {
00795 printf("syntax: live <n>\n");
00796 }
00797 else {
00798 emcmotCommand.command = EMCMOT_ENABLE_AMPLIFIER;
00799 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00800 fprintf(stderr, "Can't send a command to RT-task\n");
00801 }
00802 }
00803 }
00804 else if (! strcmp(cmd, "kill")) {
00805 if (1 != sscanf(input, "%*s %d", &emcmotCommand.axis) ||
00806 emcmotCommand.axis < 0 ||
00807 emcmotCommand.axis >= EMCMOT_MAX_AXIS) {
00808 printf("syntax: kill <n>\n");
00809 }
00810 else {
00811 emcmotCommand.command = EMCMOT_DISABLE_AMPLIFIER;
00812 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1)
00813 {
00814 fprintf(stderr, "Can't send a command to RT-task\n");
00815 }
00816 }
00817 }
00818 else if (! strcmp(cmd, "activate")) {
00819 if (1 != sscanf(input, "%*s %d", &emcmotCommand.axis) ||
00820 emcmotCommand.axis < 0 ||
00821 emcmotCommand.axis >= EMCMOT_MAX_AXIS) {
00822 printf("syntax: activate <n>\n");
00823 }
00824 else {
00825 emcmotCommand.command = EMCMOT_ACTIVATE_AXIS;
00826 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00827 fprintf(stderr, "Can't send a command to RT-task\n");
00828 }
00829 }
00830 }
00831 else if (! strcmp(cmd, "deactivate")) {
00832 if (1 != sscanf(input, "%*s %d", &emcmotCommand.axis) ||
00833 emcmotCommand.axis < 0 ||
00834 emcmotCommand.axis >= EMCMOT_MAX_AXIS) {
00835 printf("syntax: deactivate <n>\n");
00836 }
00837 else {
00838 emcmotCommand.command = EMCMOT_DEACTIVATE_AXIS;
00839 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00840 fprintf(stderr, "Can't send a command to RT-task\n");
00841 }
00842 }
00843 }
00844 else if (! strcmp(cmd, "log")) {
00845 sscanf(input, "%*s %s", cmd);
00846 if (! strcmp(cmd, "open")) {
00847
00848 valid = 0;
00849 if (3 != sscanf(input, "%*s %*s %d %d %d",
00850 &emcmotCommand.logSize,
00851 &emcmotCommand.logSkip,
00852 &emcmotCommand.logType) ||
00853 emcmotCommand.logSize <= 0) {
00854 printf("syntax: log open <size> <skip> <type> <...>\n");
00855 }
00856 else {
00857
00858 switch (emcmotCommand.logType) {
00859 case EMCMOT_LOG_TYPE_AXIS_POS:
00860 case EMCMOT_LOG_TYPE_AXIS_VEL:
00861 case EMCMOT_LOG_TYPE_POS_VOLTAGE:
00862 if (1 != sscanf(input, "%*s %*s %*s %*s %*s %d",
00863 &emcmotCommand.axis)) {
00864 printf("syntax: log open <size> <skip> <type> <axis>\n");
00865 }
00866 else {
00867 valid = 1;
00868 }
00869 break;
00870
00871 case EMCMOT_LOG_TYPE_CMD:
00872
00873 emcmotCommand.logSkip = -1;
00874 valid = 1;
00875 break;
00876
00877 default:
00878 valid = 1;
00879 break;
00880 }
00881 }
00882
00883
00884 if (valid) {
00885 emcmotCommand.command = EMCMOT_OPEN_LOG;
00886 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00887 fprintf(stderr, "Can't send a command to RT-task\n");
00888 }
00889 }
00890 }
00891 else if (! strcmp(cmd, "start")) {
00892 emcmotCommand.command = EMCMOT_START_LOG;
00893 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00894 fprintf(stderr, "Can't send a command to RT-task\n");
00895 }
00896 }
00897 else if (! strcmp(cmd, "stop")) {
00898 emcmotCommand.command = EMCMOT_STOP_LOG;
00899 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00900 fprintf(stderr, "Can't send a command to RT-task\n");
00901 }
00902 }
00903 else if (! strcmp(cmd, "close")) {
00904 emcmotCommand.command = EMCMOT_CLOSE_LOG;
00905 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00906 fprintf(stderr, "Can't send a command to RT-task\n");
00907 }
00908 }
00909 else if (! strcmp(cmd, "dump")) {
00910 int include_header=1;
00911 sscanf(input, "%*s %*s %s %d", filename, include_header);
00912 if (-1 == usrmotDumpLog(filename,include_header)) {
00913 fprintf(stderr, "Can't dump log to %s\n", filename);
00914 }
00915 }
00916 else {
00917
00918 printf("syntax: log open | start | stop | close | dump <file>\n");
00919 }
00920 }
00921 else if (! strcmp(cmd, "dac")) {
00922 if (2 == sscanf(input, "%*s %d %lf",
00923 &emcmotCommand.axis,
00924 &emcmotCommand.dacOut)) {
00925 emcmotCommand.command = EMCMOT_DAC_OUT;
00926 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00927 fprintf(stderr, "Can't send a command to RT-task\n");
00928 }
00929 }
00930 else {
00931 printf("syntax: dac <num> <-10.0 .. 10.0>\n");
00932 }
00933 }
00934 else if (! strcmp(cmd, "home")) {
00935 if (1 == sscanf(input, "%*s %d", &emcmotCommand.axis)) {
00936 emcmotCommand.command = EMCMOT_HOME;
00937 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00938 fprintf(stderr, "Can't send a command to RT-task\n");
00939 }
00940 }
00941 else {
00942 printf("syntax: home <axis>\n");
00943 }
00944 }
00945 else if (! strcmp(cmd, "nolim")) {
00946 emcmotCommand.command = EMCMOT_OVERRIDE_LIMITS;
00947 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00948 fprintf(stderr, "Can't send a command to RT-task\n");
00949 }
00950 }
00951 #ifdef ENABLE_PROBING
00952 else if (! strcmp(cmd, "probeclear")) {
00953 emcmotCommand.command = EMCMOT_CLEAR_PROBE_FLAGS;
00954 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00955 fprintf(stderr, "Can't send a command to RT-task\n");
00956 }
00957 }
00958 else if (! strcmp(cmd, "probeindex")) {
00959 if (1 == sscanf(input, "%*s %d", &emcmotCommand.probeIndex)) {
00960 emcmotCommand.command = EMCMOT_SET_PROBE_INDEX;
00961 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00962 fprintf(stderr, "Can't send a command to RT-task\n");
00963 }
00964 }
00965 else {
00966 printf("syntax: probeindex <index>\n");
00967 }
00968 }
00969 else if (! strcmp(cmd, "probepolarity")) {
00970 if (1 == sscanf(input, "%*s %d", &emcmotCommand.level)) {
00971 emcmotCommand.command = EMCMOT_SET_PROBE_POLARITY;
00972 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00973 fprintf(stderr, "Can't send a command to RT-task\n");
00974 }
00975 }
00976 else {
00977 printf("syntax: probepolarity <polarity>\n");
00978 }
00979 }
00980 else if (! strcmp(cmd, "probe")) {
00981 if (3 == sscanf(input, "%*s %lf %lf %lf",
00982 &emcmotCommand.pos.tran.x,
00983 &emcmotCommand.pos.tran.y,
00984 &emcmotCommand.pos.tran.z)) {
00985 emcmotCommand.command = EMCMOT_PROBE;
00986 emcmotCommand.id = motionId++;
00987 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
00988 fprintf(stderr, "Can't send a command to RT-task\n");
00989 }
00990 }
00991 else {
00992 printf("syntax: probe <x> <y> <z>\n");
00993 }
00994 }
00995 #endif
00996 else if (!strcmp(cmd, "wd")) {
00997 valid = 0;
00998
00999 if (1 == sscanf(input, "%*s %s", cmd)) {
01000 if (!strcmp(cmd, "on")) {
01001 if (1 != sscanf(input, "%*s %*s %d",
01002 &emcmotCommand.wdWait) ||
01003 emcmotCommand.wdWait < 0) {
01004
01005 emcmotCommand.wdWait = 0;
01006 }
01007 emcmotCommand.command = EMCMOT_ENABLE_WATCHDOG;
01008 valid = 1;
01009 }
01010 else if (!strcmp(cmd, "off")) {
01011 emcmotCommand.command = EMCMOT_DISABLE_WATCHDOG;
01012 valid = 1;
01013 }
01014 }
01015
01016 if (valid) {
01017 if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
01018 fprintf(stderr, "Can't send a command to RT-task\n");
01019 }
01020 }
01021 else {
01022 printf("syntax: wd on {<waits>} | off\n");
01023 }
01024 }
01025 else if (! strcmp(cmd, "comp")) {
01026 if (1 != sscanf(input, "%*s %d", &axis)) {
01027 fprintf(stderr, "syntax: comp <axis> {<file>}\n");
01028 }
01029 else {
01030
01031
01032 if (1 == sscanf(input, "%*s %*d %s", compfile)) {
01033 if (0 != usrmotLoadComp(axis, compfile)) {
01034 fprintf(stderr, "Can't load comp file %s\n", compfile);
01035 }
01036 }
01037 else {
01038 if (0 != usrmotPrintComp(axis)) {
01039 fprintf(stderr, "Can't print comp table\n");
01040 }
01041 }
01042 }
01043 }
01044 else if (! strcmp(cmd, "alter")) {
01045 if (1 != sscanf(input, "%*s %d", &axis)) {
01046 fprintf(stderr, "syntax: alter <axis> {<value>}\n");
01047 }
01048 else {
01049
01050
01051 if (1 == sscanf(input, "%*s %*d %lf", &alter)) {
01052 if (0 != usrmotAlter(axis, alter)) {
01053 fprintf(stderr, "Can't set alter value %f\n", alter);
01054 }
01055 }
01056 else {
01057 if (0 != usrmotQueryAlter(axis, &alter)) {
01058 fprintf(stderr, "Can't print alter value\n");
01059 }
01060 else {
01061 printf("axis %d alter: %f\n", axis, alter);
01062 }
01063 }
01064 }
01065 }
01066 else {
01067 if (anyprintable(input)) {
01068 printf("huh? : %s", input);
01069 }
01070 else {
01071
01072
01073
01074 switch(statconfigdebug)
01075 {
01076 case 0:
01077 if (0 == (errCode = usrmotReadEmcmotStatus(&emcmotStatus))) {
01078 usrmotPrintEmcmotStatus(emcmotStatus, lastPrint);
01079 }
01080 else {
01081 fprintf(stderr, "can't read status: %s\n",
01082 errCode == EMCMOT_COMM_ERROR_CONNECT ? "EMCMOT_COMM_ERROR_CONNECT" :
01083 errCode == EMCMOT_COMM_ERROR_TIMEOUT ? "EMCMOT_COMM_ERROR_TIMEOUT" :
01084 errCode == EMCMOT_COMM_ERROR_COMMAND ? "EMCMOT_COMM_ERROR_COMMAND" :
01085 errCode == EMCMOT_COMM_SPLIT_READ_TIMEOUT ? "EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
01086 }
01087 break;
01088
01089 case 1:
01090 if (0 == (errCode = usrmotReadEmcmotDebug(&emcmotDebug))) {
01091 usrmotPrintEmcmotDebug(emcmotDebug, lastPrint);
01092 }
01093 else {
01094 fprintf(stderr, "can't read debug: %s\n",
01095 errCode == EMCMOT_COMM_ERROR_CONNECT ? "EMCMOT_COMM_ERROR_CONNECT" :
01096 errCode == EMCMOT_COMM_ERROR_TIMEOUT ? "EMCMOT_COMM_ERROR_TIMEOUT" :
01097 errCode == EMCMOT_COMM_ERROR_COMMAND ? "EMCMOT_COMM_ERROR_COMMAND" :
01098 errCode == EMCMOT_COMM_SPLIT_READ_TIMEOUT ? "EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
01099 }
01100 break;
01101
01102 case 2:
01103 if (0 == (errCode = usrmotReadEmcmotConfig(&emcmotConfig))) {
01104 usrmotPrintEmcmotConfig(emcmotConfig, lastPrint);
01105 }
01106 else {
01107 fprintf(stderr, "can't read config: %s\n",
01108 errCode == EMCMOT_COMM_ERROR_CONNECT ? "EMCMOT_COMM_ERROR_CONNECT" :
01109 errCode == EMCMOT_COMM_ERROR_TIMEOUT ? "EMCMOT_COMM_ERROR_TIMEOUT" :
01110 errCode == EMCMOT_COMM_ERROR_COMMAND ? "EMCMOT_COMM_ERROR_COMMAND" :
01111 errCode == EMCMOT_COMM_SPLIT_READ_TIMEOUT ? "EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
01112 }
01113 break;
01114 }
01115 }
01116 }
01117
01118 }
01119
01120 }
01121
01122 usrmotExit();
01123 exit(0);
01124 }