usrmot.c

Source code for an Numeric Cmputer

/********************************************************************
* Description: usrmot.c
*   Linux user-level process for communicating with RT motion controller
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
*
* Last change:
* $Revision: 1.16 $
* $Author: jmkasunich $
* $Date: 2005/11/24 03:41:40 $
********************************************************************/

#include "config.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>		/* isspace() */
#include <unistd.h>		/* STDIN_FILENO */
#include <fcntl.h>		/* F_GETFL, O_NONBLOCK */
#include <signal.h>		/* signal(), SIGINT */
#include "_timer.h"		/* esleep() */
#include "motion.h"
#include "usrmotintf.h"		/* usrmotInit(), etc */
#include "posemath.h"
#include "emcmotcfg.h"		/* EMCMOT_ERROR_LEN,NUM */
#include "emcmotglb.h"		/* SHMEM_KEY */

/* max numbers allowed */
#define MAX_NUMBERS 8

/* microseconds to sleep between calls to getinput() */
#define SLEEP_SECS 0.100

/* takes a string, and returns 0 if all whitespace, 1 otherwise */
static int anyprintable(const char *string)
{
    int cnt = 0;
    char c;

    while (0 != (c = string[cnt++]))
	if (!isspace(c))
	    return 1;
    return 0;
}

static char *skipWhite(char *s)
{
    while (isspace(*s)) {
	s++;
    }
    return s;
}

static char *skipNonwhite(char *s)
{
    while (!isspace(*s)) {
	s++;
    }
    return s;
}

static int scanNumbers(char *string, double *numbers, int max)
{
    char *ptr = string;
    int count = 0;

    while (count < max) {
	if (1 != sscanf(ptr, "%lf", &numbers[count])) {
	    return count;
	}
	count++;
	ptr = skipNonwhite(ptr);
	ptr = skipWhite(ptr);
    }

    return count;
}

/*
  emcmotGetArgs() looks for -ini <inifile>, and sets the global
  EMCMOT_INIFILE (not to be confused with the EMC-level EMC_INIFILE).
*/

int emcmotGetArgs(int argc, char *argv[])
{
    int t;

    /* process command line args, indexing argv[] from [1] */
    for (t = 1; t < argc; t++) {
	if (!strcmp(argv[t], "-ini")) {
	    if (t == argc - 1) {
		return -1;
	    } else {
		strcpy(EMCMOT_INIFILE, argv[t + 1]);
		t++;		/* step over following arg */
	    }
	}
	/* else not recognized-- ignore */
    }

    return 0;
}

/*
  getinput() returns the number of chars read, -1 if no chars were available,
  or 0 if EOF. It doesn't block, so you can call this repeatedly and when
  it returns non-zero you have that many chars, not including the added NULL.
  */
int getinput(char *buffer, int maxchars)
{
    int flags;
    int nchars;
    int index = 0;

    /* save the flags */
    flags = fcntl(STDIN_FILENO, F_GETFL);

    /* make terminal non-blocking */
    fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);

    /* read the outstanding chars, one by one, until newline or no more */
    while (1 == (nchars = read(STDIN_FILENO, &buffer[index], 1))) {
	if (buffer[index++] == '\n') {
	    buffer[index] = 0;	/* null terminate */
	    break;
	}
    }

    /* restore the terminal */
    fcntl(STDIN_FILENO, F_SETFL, flags);

    if (nchars == -1) {
	return -1;		/* nothing new */
    }

    if (nchars == 0) {
	return 0;		/* end of file */
    }

    return index;
}

static void usrmotQuit(int sig)
{
    printf("Received %i SIGINT - Detaching from motion\n", sig);
    usrmotExit();
    exit(0);
}

/*
   syntax:  usrmot
*/
int main(int argc, char *argv[])
{
    emcmot_command_t emcmotCommand;
    emcmot_status_t emcmotStatus;
    emcmot_config_t emcmotConfig;
    emcmot_debug_t emcmotDebug;
    char input[LINELEN];
    char cmd[LINELEN];
    char errorString[EMCMOT_ERROR_LEN];
    int valid;
    int done;
    int nchars;
    int printPrompt;
    int disablePrompt = 0;	/* flag for disabling prompt printing via > */
    double numbers[MAX_NUMBERS];	/* space for number input data */
    int num;			/* how many there are */
    int numNumbers = 0;		/* established number of input numbers */
    char filename[LINELEN];
    int linenum;
    FILE *fp;			/* ini file ptr */
    int lastPrint = 0;		/* flag for which status subset to print */
    int statconfigdebug = 0;	/* 0 for stat, 1 for debug, 2 for config */
    int motionId = 0;		/* motion id sent down with moves */
    int axis;			/* axis selected for command */
    int errCode;		/* returned from usrmotWrite,Read... */
    char compfile[LINELEN];	/* name of the compensation file */
    double alter;		/* value for external alter */

    /* print the sizes first, so that even if emcmot isn't up and running we
       can use usrmot to simply print the size and fail */
    printf("sizeof(emcmot_command_t) = %d\n", sizeof(emcmot_command_t));
    printf("sizeof(emcmot_status_t) = %d\n", sizeof(emcmot_status_t));
    printf("sizeof(emcmot_config_t) = %d\n", sizeof(emcmot_config_t));
    printf("sizeof(emcmot_internal_t) = %d\n", sizeof(emcmot_internal_t));
    printf("sizeof(emcmot_debug_t) = %d\n", sizeof(emcmot_debug_t));
    printf("sizeof(emcmot_error_t) = %d\n", sizeof(emcmot_error_t));
    printf("sizeof(emcmot_comp_t) = %d\n", sizeof(emcmot_comp_t));
    printf("sizeof(emcmot_joint_t) = %d\n", sizeof(emcmot_joint_t));
     printf("sizeof(emcmot_struct_t) = %d\n", sizeof(emcmot_struct_t));
    printf("sizeof(TC_STRUCT) = %d\n", sizeof(TC_STRUCT));

    /* process command line args */
    emcmotGetArgs(argc, argv);

    /* read comm parameters */
    if (-1 == usrmotIniLoad(EMCMOT_INIFILE)) {
	fprintf(stderr, "can't load ini file %s\n", EMCMOT_INIFILE);
	exit(1);
    }

    /* init comm */
    if (-1 == usrmotInit("usrmot")) {
	fprintf(stderr, "can't initialize comm interface\n");
	exit(1);
    }

    /* Now that we have connected to shared memory via rtapi register a
       SIGINT handler */
    signal(SIGINT, usrmotQuit);

    emcmotCommand.pos.a = 0.0;
    emcmotCommand.pos.b = 0.0;
    emcmotCommand.pos.c = 0.0;

    /* loop on input */
    done = 0;
    printPrompt = 1;
    while (!feof(stdin) && !done) {
	/* read errors */
	while (0 == usrmotReadEmcmotError(errorString)) {
	    printf("error: %s\n", errorString);
	}

	/* check if we need to print a prompt */
	if (printPrompt) {
	    if (!disablePrompt) {
		printf("motion> ");
		fflush(stdout);
	    }
	    printPrompt = 0;
	}

	/* get the next input line, if any */
	nchars = getinput(input, LINELEN);
	if (nchars > 0) {
	    printPrompt = 1;
	} else if (nchars == -1) {
	    /* no new input-- cycle again */
	    esleep(SLEEP_SECS);
	    continue;		/* the while(!feof(stdin)) loop */
	} else {
	    /* nchars == 0, EOF */
	    break;
	}

	/* got input-- check for a number first */
	num = scanNumbers(input, numbers, MAX_NUMBERS);
	if (num > 0) {
	    if (numNumbers == 0) {
		numNumbers = num;
	    } else if (numNumbers != num) {
		fprintf(stderr, "need %d numbers\n", numNumbers);
		/* ignore 'em */
		continue;
	    }

	    /* write out command */

	    emcmotCommand.command = EMCMOT_SET_LINE;
	    emcmotCommand.pos.tran.x = numbers[0];
	    emcmotCommand.pos.tran.y = numbers[1];
	    emcmotCommand.pos.tran.z = numbers[2];
	    emcmotCommand.id = motionId++;

	    if (usrmotWriteEmcmotCommand(&emcmotCommand) == -1) {
		fprintf(stderr, "Can't send a command to RT-task\n");
	    }
	} else {
	    /* read first arg in */
	    cmd[0] = 0;
	    sscanf(input, "%s", cmd);

	    /* match it in the big if loop */
	    if (!strcmp(cmd, "help") || !strcmp(cmd, "?")) {
		printf("? or help\tprint help\n");
		printf(">\ttoggle prompt on or off\n");
		printf(";\tcomment follows\n");
		printf("quit\tquit\n");
		printf
		    ("show {flags} {limits} {scales} {times}\tshow status\n");
		printf("show debug <screen>\tshow debug\n");
		printf("show config <screen>\tshow config\n");
		printf("free\tset mode to free\n");
		printf("teleop\tset mode to teleop\n");
		printf("coord\tset mode to coordinated\n");
		printf("pause\tpause motion controller\n");
		printf("resume\tresume motion controller\n");
		printf("a\tabort motion controller\n");
		printf("scale <0..1>\tscale velocity\n");
		printf
		    ("enable | disable\tenable or disable motion controller\n");
		printf("jog <axis> + | -\tjog axis pos or neg\n");
		printf("jog <axis> + | -\tjog axis pos or neg\n");
		printf("id <num>   \tset base id for subsequent moves\n");
		printf("<x> <y> <z>\tput motion on queue\n");
		printf("load <file>\tput motions from file on queue\n");
		printf
		    ("cw <x> <y> <z> <cx> <cy> <cz> <turn>\tput CW circle on queue\n");
		printf
		    ("ccw <x> <y> <z> <cx> <cy> <cz> <turn>\tput CCW circle on queue\n");
/*! \todo FIXME old set command
		printf
		    ("set t <traj t> | s <servo t> | v <vel> | a <acc>\tset params\n");
*/
		printf("set v <vel> | a <acc>\tset params\n");
/*! \todo FIXME
		printf
		    ("oscale <axis 0..n-1> <a> <b>\traw[n] = a(out[n]-b)\n");
		printf("iscale <axis 0..n-1> <a> <b>\tin[n] = a(raw[n]-b)\n");
		printf
		    ("pol <axis 0..n-1> <enable nhl phl homedir homesw fault> <0 1>\n");
		printf
		    ("pid <axis 0..n-1> <ini file>\tset PID gains for motor n\n");
*/
		printf("limit <axis> <min> <max>\tset position limits\n");
		printf("clamp <axis> <min> <max>\tset output limits\n");
		printf("ferror <axis> <value>\tset max following error\n");
		printf("live <axis 0..n-1>\tenable amp n\n");
		printf("kill <axis 0..n-1>\tkill amp n\n");
		printf("activate <axis 0..n-1>\tactivate axis n\n");
		printf("deactivate <axis 0..n-1>\tdeactivate axis n\n");
/*! \todo FIXME
		printf
		    ("dac <axis 0..n-1> <-10.0 .. 10.0>\toutput value to DAC\n");
*/
		printf("home <axis 0..n-1>\thome axis\n");
		printf("nolim             \toverride hardware limits\n");
		printf("wd on | off\tenable or disable watchdog toggle\n");
		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");
		printf
		    ("probeclear\tClear the probeTripped status flag.\n");
/*! \todo FIXME
		printf
		    ("probeindex <index>\tSet which input is checked for probe status.\n");
		printf
		    ("probepolarity <polarity>\tSet whether a probe is tripped on a 0 or 1.\n");
*/
	    } else if (!strcmp(cmd, ">")) {
		disablePrompt = !disablePrompt;
	    } else if (!strcmp(cmd, ";")) {
		/* comment */
	    } else if (!strcmp(cmd, "quit")) {
		done = 1;
	    } else if (!strcmp(cmd, "show")) {
		if (1 == sscanf(input, "%*s %s", cmd)) {
		    statconfigdebug = 0;
		    if (!strcmp(cmd, "pids")) {
			lastPrint = 1;
			statconfigdebug = 2;	/* config */
		    } else if (!strcmp(cmd, "flags")) {
			lastPrint = 2;
		    } else if (!strcmp(cmd, "limits")) {
			lastPrint = 3;
			statconfigdebug = 2;	/* config */
		    } else if (!strcmp(cmd, "scales")) {
			lastPrint = 4;
		    } else if (!strcmp(cmd, "times")) {
			lastPrint = 5;