usrmot.c

Source code for an Numeric Cmputer

			statconfigdebug = 1;	/* debug */
		    } else if (!strcmp(cmd, "stat")) {
			statconfigdebug = 0;
			lastPrint =
			    strtol(strstr(input, "stat") + 4, 0, 0);
		    } else if (!strcmp(cmd, "debug")) {
			statconfigdebug = 1;
			lastPrint =
			    strtol(strstr(input, "debug") + 5, 0, 0);
		    } else if (!strcmp(cmd, "config")) {
			statconfigdebug = 2;
			lastPrint =
			    strtol(strstr(input, "config") + 6, 0, 0);
		    } else {
			/* invalid parameter */
			printf
			    ("syntax: show {pids} {flags} {limits} {scales} {times}\n");
			continue;	/* to while loop on stdin */
		    }
		} else {
		    lastPrint = 0;
		    statconfigdebug = 0;
		}

		/* print status */
		switch (statconfigdebug) {
		case 0:
		    if (0 == (errCode =
			      usrmotReadEmcmotStatus(&emcmotStatus))) {
			usrmotPrintEmcmotStatus(emcmotStatus, lastPrint);
		    } else {
			fprintf(stderr, "can't read status: %s\n",
				errCode ==
				EMCMOT_COMM_ERROR_CONNECT ?
				"EMCMOT_COMM_ERROR_CONNECT" : errCode ==
				EMCMOT_COMM_ERROR_TIMEOUT ?
				"EMCMOT_COMM_ERROR_TIMEOUT" : errCode ==
				EMCMOT_COMM_ERROR_COMMAND ?
				"EMCMOT_COMM_ERROR_COMMAND" : errCode ==
				EMCMOT_COMM_SPLIT_READ_TIMEOUT ?
				"EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
		    }
		    break;

		case 1:
		    if (0 ==
			(errCode = usrmotReadEmcmotDebug(&emcmotDebug))) {
			usrmotPrintEmcmotDebug(emcmotDebug, lastPrint);
		    } else {
			fprintf(stderr, "can't read debug: %s\n",
				errCode ==
				EMCMOT_COMM_ERROR_CONNECT ?
				"EMCMOT_COMM_ERROR_CONNECT" : errCode ==
				EMCMOT_COMM_ERROR_TIMEOUT ?
				"EMCMOT_COMM_ERROR_TIMEOUT" : errCode ==
				EMCMOT_COMM_ERROR_COMMAND ?
				"EMCMOT_COMM_ERROR_COMMAND" : errCode ==
				EMCMOT_COMM_SPLIT_READ_TIMEOUT ?
				"EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
		    }
		    break;

		case 2:
		    if (0 == (errCode =
			      usrmotReadEmcmotConfig(&emcmotConfig))) {
			usrmotPrintEmcmotConfig(emcmotConfig, lastPrint);
		    } else {
			fprintf(stderr, "can't read config: %s\n",
				errCode ==
				EMCMOT_COMM_ERROR_CONNECT ?
				"EMCMOT_COMM_ERROR_CONNECT" : errCode ==
				EMCMOT_COMM_ERROR_TIMEOUT ?
				"EMCMOT_COMM_ERROR_TIMEOUT" : errCode ==
				EMCMOT_COMM_ERROR_COMMAND ?
				"EMCMOT_COMM_ERROR_COMMAND" : errCode ==
				EMCMOT_COMM_SPLIT_READ_TIMEOUT ?
				"EMCMOT_COMM_SPLIT_READ_TIMEOUT" : "?");
		    }
		    break;
		}
	    } else if (!strcmp(cmd, "pause")) {
		emcmotCommand.command = EMCMOT_PAUSE;
		if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
		    fprintf(stderr, "Can't send a command to RT-task\n");
		}
	    } else if (!strcmp(cmd, "resume")) {
		emcmotCommand.command = EMCMOT_RESUME;
		if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
		    fprintf(stderr, "Can't send a command to RT-task\n");
		}
	    } else if (!strcmp(cmd, "a")) {
		/* set the axis field, if provided. If not provided, it will
		   default to the last one used. In coord mode it's not used
		   at all. */
		valid = 0;
		if (1 == sscanf(input, "%*s %d", &axis)) {
		    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
			fprintf(stderr, "bad axis %d to abort\n", axis);
		    } else {
			emcmotCommand.axis = axis;
			valid = 1;
		    }
		} else {
		    /* axis not provided, so leave last one in
		       emcmotCommand.axs alone */
		    valid = 1;
		}

		if (valid) {
		    emcmotCommand.command = EMCMOT_ABORT;

		    if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			fprintf(stderr,
				"Can't send a command to RT-task\n");
		    }
		}
	    } else if (!strcmp(cmd, "scale")) {
		if (1 == sscanf(input, "%*s %lf", &emcmotCommand.scale)) {
		    emcmotCommand.command = EMCMOT_SCALE;
		    if (emcmotCommand.scale < 0.0) {
			emcmotCommand.scale = 0.0;	/* clamp it */
		    }
		    if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			fprintf(stderr,
				"Can't send a command to RT-task\n");
		    }
		} else {
		    /* invalid parameter */
		    printf("syntax: scale <0..1>\n");
		}
	    } else if (!strcmp(cmd, "enable")) {
		emcmotCommand.command = EMCMOT_ENABLE;
		if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
		    fprintf(stderr, "Can't send a command to RT-task\n");
		}
	    } else if (!strcmp(cmd, "disable")) {
		emcmotCommand.command = EMCMOT_DISABLE;
		if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
		    fprintf(stderr, "Can't send a command to RT-task\n");
		}
	    } else if (!strcmp(cmd, "free")) {
		emcmotCommand.command = EMCMOT_FREE;
		if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
		    fprintf(stderr, "Can't send a command to RT-task\n");
		}
	    } else if (!strcmp(cmd, "teleop")) {
		emcmotCommand.command = EMCMOT_TELEOP;
		if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
		    fprintf(stderr, "Can't send a command to RT-task\n");
		}
	    } else if (!strcmp(cmd, "coord")) {
		emcmotCommand.command = EMCMOT_COORD;
		if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
		    fprintf(stderr, "Can't send a command to RT-task\n");
		}
	    } else if (!strcmp(cmd, "jog")) {
		if (2 ==
		    sscanf(input, "%*s %d %s", &emcmotCommand.axis, cmd)) {
		    emcmotCommand.command = EMCMOT_JOG_CONT;
		    if (cmd[0] == '+') {
			emcmotCommand.vel = emcmotStatus.vel;
		    } else if (cmd[0] == '-') {
			emcmotCommand.vel = -emcmotStatus.vel;
		    } else {
			fprintf(stderr, "syntax: jog <axis> + | -\n");
			break;
		    }

		    if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			fprintf(stderr,
				"Can't send a command to RT-task\n");
		    }
		} else {
		    fprintf(stderr, "syntax: jog <axis> + | -\n");
		}
	    } else if (!strcmp(cmd, "id")) {
		int newId;
		if (1 == sscanf(input, "%*s %d", &newId)) {
		    motionId = newId;
		    printf("setting id to %d\n", motionId);
		} else {
		    fprintf(stderr, "syntax: id <num>\n");
		}
	    } else if (!strcmp(cmd, "load")) {
		if (1 == sscanf(input, "%*s %s", filename)) {
		    if (NULL != (fp = fopen(filename, "r"))) {
			linenum = 0;
			while (!feof(fp)) {
			    if (NULL != fgets(input, LINELEN, fp)) {
				linenum++;
				if (3 == sscanf(input, "%lf %lf %lf",
						&emcmotCommand.pos.tran.x,
						&emcmotCommand.pos.tran.y,
						&emcmotCommand.pos.tran.
						z)) {
				    printf("sending %f %f %f\n",
					   emcmotCommand.pos.tran.x,
					   emcmotCommand.pos.tran.y,
					   emcmotCommand.pos.tran.z);
				    emcmotCommand.command =
					EMCMOT_SET_LINE;
				    emcmotCommand.id = motionId++;
				    if (usrmotWriteEmcmotCommand
					(&emcmotCommand) == -1) {
					fprintf(stderr,
						"Can't send a command to RT-task\n");
				    }
				} else {
				    /* input error */
				    if (anyprintable(input)) {
					fprintf(stderr,
						"bad input on line %d of file %s\n",
						linenum, filename);
					fclose(fp);
					break;	/* out of while (! feof(fp)) */
				    }
				}
			    } else {
				/* end of input */
				fclose(fp);
				break;	/* out of while (! feof(fp)) */
			    }
			}	/* end while (! feof(fp)) */
		    } /* end if file open success */
		    else {
			fprintf(stderr, "can't open %s\n", filename);
		    }
		} /* end if correct arg to "load" */
		else {
		    fprintf(stderr, "syntax: load <filename>\n");
		}
	    } /* end match on "load" */
	    else if (!strcmp(cmd, "cw") || !strcmp(cmd, "ccw")) {
		if (7 == sscanf(input, "%*s %lf %lf %lf %lf %lf %lf %d",
				&emcmotCommand.pos.tran.x,
				&emcmotCommand.pos.tran.y,
				&emcmotCommand.pos.tran.z,
				&emcmotCommand.center.x,
				&emcmotCommand.center.y,
				&emcmotCommand.center.z,
				&emcmotCommand.turn)) {
		    emcmotCommand.command = EMCMOT_SET_CIRCLE;
		    emcmotCommand.normal.x = 0.0;
		    emcmotCommand.normal.y = 0.0;
		    if (!strcmp(cmd, "cw")) {
			emcmotCommand.normal.z = -1.0;
		    } else {
			emcmotCommand.normal.z = 1.0;
		    }
		    if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			fprintf(stderr,
				"Can't send a command to RT-task\n");
		    }
		} else {
		    fprintf(stderr,
			    "syntax: cw <x> <y> <z> <cx> <cy> <cz> <turn>\n");
		}
	    } else if (!strcmp(cmd, "set")) {
		sscanf(input, "%*s %s", cmd);
/*! \todo FIXME - obsolete commands */
		if (!strcmp(cmd, "t")) {
		    fprintf(stderr, "'set t' command is obsolete\n");
/*! \todo Another #if 0 */
#if 0
		    if (1 != sscanf(input, "%*s %*s %lf",
				    &emcmotCommand.cycleTime)) {
			/* invalid parameter */
			fprintf(stderr, "bad value for cycle time\n");
		    } else {
			emcmotCommand.command = EMCMOT_SET_TRAJ_CYCLE_TIME;
			if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			    fprintf(stderr,
				    "Can't send a command to RT-task\n");
			}
		    }
#endif
		} else if (!strcmp(cmd, "s")) {
		    fprintf(stderr, "'set t' command is obsolete\n");
/*! \todo Another #if 0 */
#if 0
		    if (1 != sscanf(input, "%*s %*s %lf",
				    &emcmotCommand.cycleTime)) {
			/* invalid parameter */
			fprintf(stderr,
				"bad value for interpolation rate\n");
		    } else {
			emcmotCommand.command =
			    EMCMOT_SET_SERVO_CYCLE_TIME;
			if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			    fprintf(stderr,
				    "Can't send a command to RT-task\n");
			}
		    }
#endif
		} else if (!strcmp(cmd, "v")) {
		    if (1 !=
			sscanf(input, "%*s %*s %lf", &emcmotCommand.vel)) {
			/* invalid parameter */
			fprintf(stderr, "bad value for velocity\n");
		    } else {
			emcmotCommand.command = EMCMOT_SET_VEL;
			if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			    fprintf(stderr,
				    "Can't send a command to RT-task\n");
			}
		    }
		} else if (!strcmp(cmd, "a")) {
		    if (1 !=
			sscanf(input, "%*s %*s %lf", &emcmotCommand.acc)) {
			/* invalid parameter */
			fprintf(stderr, "bad value for acceleration\n");
		    } else {
			emcmotCommand.command = EMCMOT_SET_ACC;
			if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			    fprintf(stderr,
				    "Can't send a command to RT-task\n");
			}
		    }
		} else {
		    /* invalid parameter */
/*! \todo FIXME	    printf
			("syntax: set t <traj t> | s <servo t> | v <vel> | a <acc>\n");
*/
		    printf("syntax: set v <vel> | a <acc>\n");
		}
/*! \todo Another #if 0 */
#if 0
	    } else if (!strcmp(cmd, "oscale")) {
		if (3 != sscanf(input, "%*s %d %lf %lf",
				&emcmotCommand.axis,
				&emcmotCommand.scale,
				&emcmotCommand.offset)) {
		    printf("syntax: oscale <axis 0..n-1> <a> <b>\n");
		} else {
		    emcmotCommand.command = EMCMOT_SET_OUTPUT_SCALE;
		    if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			fprintf(stderr,
				"Can't send a command to RT-task\n");
		    }
		}
	    } else if (!strcmp(cmd, "iscale")) {
		if (3 != sscanf(input, "%*s %d %lf %lf",
				&emcmotCommand.axis,
				&emcmotCommand.scale,
				&emcmotCommand.offset)) {
		    printf("syntax: iscale <axis 0..n-1> <a> <b>\n");
		} else {
		    emcmotCommand.command = EMCMOT_SET_INPUT_SCALE;
		    if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
			fprintf(stderr,
				"Can't send a command to RT-task\n");
		    }
		}
	    } else if (!strcmp(cmd, "pol")) {
		if (3 != sscanf(input, "%*s %d %s %d",
				&emcmotCommand.axis, cmd,
				&emcmotCommand.level)) {
		    printf
			("syntax: pol <axis 0..n-1> <enable nhl phl homedir homesw fault> <0 1>\n");
		} else {
		    valid = 1;

		    if (!strcmp(cmd, "enable")) {
			emcmotCommand.axisFlag = EMCMOT_AXIS_ENABLE_BIT;
		    } else if (!strcmp(cmd, "nhl")) {
			emcmotCommand.axisFlag =
			    EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;