usrmotintf.cc

来自「CNC 的开放码,EMC2 V2.2.8版」· CC 代码 · 共 814 行 · 第 1/2 页

/********************************************************************
* Description: usrmotintf.cc
*   Defs for interface functions (init, exit, read, write) for user
*   processes which communicate with the real-time motion controller
*   in emcmot.c
*
*   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.29 $
* $Author: cradek $
* $Date: 2007/07/14 21:43:24 $
********************************************************************/

#include "config.h"     	/* LINELEN definition */
#include <stdlib.h>		/* exit() */
#include <sys/stat.h>
#include <string.h>		/* memcpy() */
#include <float.h>		/* DBL_MIN */
#include "motion.h"		/* emcmot_status_t,CMD */
#include "motion_debug.h"       /* emcmot_debug_t */
#include "motion_struct.h"      /* emcmot_struct_t */
#include "emcmotcfg.h"		/* EMCMOT_ERROR_NUM,LEN */
#include "emcmotglb.h"		/* SHMEM_KEY */
#include "usrmotintf.h"		/* these decls */
#include "_timer.h"

#include "inifile.hh"

#define READ_TIMEOUT_SEC 0	/* seconds for timeout */
#define READ_TIMEOUT_USEC 100000	/* microseconds for timeout */

#include "rtapi.h"

static int inited = 0;		/* flag if inited */

static emcmot_command_t *emcmotCommand = 0;
static emcmot_status_t *emcmotStatus = 0;
static emcmot_config_t *emcmotConfig = 0;
static emcmot_debug_t *emcmotDebug = 0;
static emcmot_error_t *emcmotError = 0;
static emcmot_struct_t *emcmotStruct = 0;
emcmot_struct_t *emcmotshmem = NULL;	// Shared memory base address.

/* usrmotIniLoad() loads params (SHMEM_KEY, COMM_TIMEOUT, COMM_WAIT)
   from named ini file */
int usrmotIniLoad(const char *filename)
{
    IniFile inifile(IniFile::ERR_CONVERSION);   // Enable exception.
    
    /* open it */
    if (!inifile.Open(filename)) {
	rtapi_print("can't find emcmot ini file %s\n", filename);
	return -1;
    }

    try {
        inifile.Find((int *)&SHMEM_KEY, "SHMEM_KEY", "EMCMOT");
        inifile.Find(&EMCMOT_COMM_TIMEOUT, "COMM_TIMEOUT", "EMCMOT");
        inifile.Find(&EMCMOT_COMM_WAIT, "COMM_WAIT", "EMCMOT");
    }

    catch(IniFile::Exception &e){
        e.Print();
	return -1;
    }

    return 0;
}

/* writes command from c */
int usrmotWriteEmcmotCommand(emcmot_command_t * c)
{
    emcmot_status_t s;
    static int commandNum = 0;
    static unsigned char headCount = 0;
    double end;

    c->head = ++headCount;
    c->tail = c->head;
    c->commandNum = ++commandNum;

    /* check for mapped mem still around */
    if (0 == emcmotCommand) {
	return EMCMOT_COMM_ERROR_CONNECT;
    }
    /* copy entire command structure to shared memory */
    *emcmotCommand = *c;
    /* poll for receipt of command */
    /* set timeout for comm failure, now + timeout */
    end = etime() + EMCMOT_COMM_TIMEOUT;
    /* now check to see if it got it */
    while (etime() < end) {
	/* update status */
	if (( usrmotReadEmcmotStatus(&s) == 0 ) && ( s.commandNumEcho == commandNum )) {
	    /* now check emcmot status flag */
	    if (s.commandStatus == EMCMOT_COMMAND_OK) {
		return EMCMOT_COMM_OK;
	    } else {
		return EMCMOT_COMM_ERROR_COMMAND;
	    }
	}
    }
    return EMCMOT_COMM_ERROR_TIMEOUT;
}

/* copies status to s */
int usrmotReadEmcmotStatus(emcmot_status_t * s)
{
    int split_read_count;
    
    /* check for shmem still around */
    if (0 == emcmotStatus) {
	return EMCMOT_COMM_ERROR_CONNECT;
    }
    split_read_count = 0;
    do {
	/* copy status struct from shmem to local memory */
	memcpy(s, emcmotStatus, sizeof(emcmot_status_t));
	/* got it, now check head-tail matche */
	if (s->head == s->tail) {
	    /* head and tail match, done */
	    return EMCMOT_COMM_OK;
	}
	/* inc counter and try again, max three times */
    } while ( ++split_read_count < 3 );
    return EMCMOT_COMM_SPLIT_READ_TIMEOUT;
}

/* copies config to s */
int usrmotReadEmcmotConfig(emcmot_config_t * s)
{
    int split_read_count;
    
    /* check for shmem still around */
    if (0 == emcmotConfig) {
	return EMCMOT_COMM_ERROR_CONNECT;
    }
    split_read_count = 0;
    do {
	/* copy config struct from shmem to local memory */
	memcpy(s, emcmotConfig, sizeof(emcmot_config_t));
	/* got it, now check head-tail matches */
	if (s->head == s->tail) {
	    /* head and tail match, done */
	    return EMCMOT_COMM_OK;
	}
	/* inc counter and try again, max three times */
    } while ( ++split_read_count < 3 );
printf("ReadEmcmotConfig COMM_SPLIT_READ_TIMEOUT\n" );
    return EMCMOT_COMM_SPLIT_READ_TIMEOUT;
}

/* copies debug to s */
int usrmotReadEmcmotDebug(emcmot_debug_t * s)
{
    int split_read_count;
    
    /* check for shmem still around */
    if (0 == emcmotDebug) {
	return EMCMOT_COMM_ERROR_CONNECT;
    }
    split_read_count = 0;
    do {
	/* copy debug struct from shmem to local memory */
	memcpy(s, emcmotDebug, sizeof(emcmot_debug_t));
	/* got it, now check head-tail matches */
	if (s->head == s->tail) {
	    /* head and tail match, done */
	    return EMCMOT_COMM_OK;
	}
	/* inc counter and try again, max three times */
    } while ( ++split_read_count < 3 );
printf("ReadEmcmotDebug COMM_SPLIT_READ_TIMEOUT\n" );
    return EMCMOT_COMM_SPLIT_READ_TIMEOUT;
}

/* copies error to s */
int usrmotReadEmcmotError(char *e)
{
    /* check to see if ptr still around */
    if (emcmotError == 0) {
	return -1;
    }

    /* returns 0 if something, -1 if not */
    return emcmotErrorGet(emcmotError, e);
}

/*
 htostr()

 converts short int to 0-1 style string, in s. Assumes a short is 2 bytes.
*/
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - don't know if this is still needed 
				 */

static int htostr(char *s, unsigned short h)
{
    int t;

    for (t = 15; t >= 0; t--) {
	s[t] = h % 2 ? '1' : '0';
	h >>= 1;
    }
    s[16] = 0;			/* null terminate */

    return 0;
}
#endif

void printEmcPose(EmcPose * pose)
{
    printf("x=%f\ty=%f\tz=%f\tu=%f\tv=%f\tw=%f\ta=%f\tb=%f\tc=%f",
           pose->tran.x, pose->tran.y, pose->tran.z, 
           pose->u, pose->v, pose->w,
           pose->a, pose->b, pose->c);
}

void printTPstruct(TP_STRUCT * tp)
{
    printf("queueSize=%d\n", tp->queueSize);
    printf("cycleTime=%f\n", tp->cycleTime);
    printf("vMax=%f\n", tp->vMax);
    printf("vScale=%f\n", tp->vScale);
    printf("aMax=%f\n", tp->aMax);
    printf("vLimit=%f\n", tp->vLimit);
    printf("wMax=%f\n", tp->wMax);
    printf("wDotMax=%f\n", tp->wDotMax);
    printf("nextId=%d\n", tp->nextId);
    printf("execId=%d\n", tp->execId);
    printf("termCond=%d\n", tp->termCond);
    printf("currentPos :");
    printEmcPose(&tp->currentPos);
    printf("\n");
    printf("goalPos :");
    printEmcPose(&tp->goalPos);
    printf("\n");
    printf("done=%d\n", tp->done);
    printf("depth=%d\n", tp->depth);
    printf("activeDepth=%d\n", tp->activeDepth);
    printf("aborting=%d\n", tp->aborting);
    printf("pausing=%d\n", tp->pausing);
}

void usrmotPrintEmcmotDebug(emcmot_debug_t d, int which)
{
//    int t;

    printf("running time: \t%f\n", d.running_time);
    switch (which) {
    case 0:
	printf("split:        \t%d\n", d.split);
	printf("teleop desiredVel: \t%f\t%f\t%f\t%f\t%f\t%f\n",
	    d.teleop_data.desiredVel.tran.x,
	    d.teleop_data.desiredVel.tran.y,
	    d.teleop_data.desiredVel.tran.z,
	    d.teleop_data.desiredVel.a,
	    d.teleop_data.desiredVel.b, d.teleop_data.desiredVel.c);
	printf("teleop currentVel: \t%f\t%f\t%f\t%f\t%f\t%f\n",
	    d.teleop_data.currentVel.tran.x,
	    d.teleop_data.currentVel.tran.y,
	    d.teleop_data.currentVel.tran.z,
	    d.teleop_data.currentVel.a,
	    d.teleop_data.currentVel.b, d.teleop_data.currentVel.c);
	printf("teleop desiredAccell: \t%f\t%f\t%f\t%f\t%f\t%f\n",
	    d.teleop_data.desiredAccell.tran.x,
	    d.teleop_data.desiredAccell.tran.y,
	    d.teleop_data.desiredAccell.tran.z,
	    d.teleop_data.desiredAccell.a,
	    d.teleop_data.desiredAccell.b, d.teleop_data.desiredAccell.c);
	printf("teleop currentAccell: \t%f\t%f\t%f\t%f\t%f\t%f\n",
	    d.teleop_data.currentAccell.tran.x,
	    d.teleop_data.currentAccell.tran.y,
	    d.teleop_data.currentAccell.tran.z,
	    d.teleop_data.currentAccell.a,
	    d.teleop_data.currentAccell.b, d.teleop_data.currentAccell.c);
	break;
/*! \todo Another #if 0 */
#if 0
	printf("\nferror:        ");
	for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d.ferrorCurrent[t]);
	}
	printf("\n");

	printf("\nferror High:        ");
	for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d.ferrorHighMark[t]);
	}
	printf("\n");
	break;
#endif
    case 5:
	printf("traj  m/m/a:\t%f\t%f\t%f\n", d.tMin, d.tMax, d.tAvg);
	printf("\n");
	printf("servo m/m/a:\t%f\t%f\t%f\n", d.sMin, d.sMax, d.sAvg);
	printf("\n");
	printf("(off) m/m/a:\t%f\t%f\t%f\n", d.nMin, d.nMax, d.nAvg);
	printf("\n");
	printf("(cycle to cycle  time) m/m/a:\t%f\t%f\t%f\n", d.yMin, d.yMax,
	    d.yAvg);
	printf("\n");
	printf("(frequency compute  time) m/m/a:\t%f\t%f\t%f\n", d.fMin,
	    d.fMax, d.fAvg);
	printf("\n");
	printf("(frequecy cycle to cycle  time) m/m/a:\t%f\t%f\t%f\n",
	    d.fyMin, d.fyMax, d.fyAvg);
	printf("\n");
	break;

    case 6:
    case 7:
    case 8:
    case 9:
    case 10:
    case 11:
//      printf("jointPos[%d]: %f\n", which - 6, d.jointPos[(which - 6)]);
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - change to work with joint
				   structures */
	printf("coarseJointPos[%d]: %f\n",
	    which - 6, d.coarseJointPos[(which - 6)]);
	printf("jointVel[%d]: %f\n", which - 6, d.jointVel[(which - 6)]);
	printf("rawInput[%d]: %f\n", which - 6, d.rawInput[(which - 6)]);
	printf("rawOutput[%d]: %f\n", which - 6, d.rawOutput[(which - 6)]);
#endif
//      printf("bcompincr[%d]: %f\n", which - 6, d.bcompincr[(which - 6)]);
	break;

    case 12:
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - change to work with joint
				   structures */
	printf("\noldInput:  ");
	for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d.oldInput[t]);
	}
	printf("\nrawInput:  ");
	for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d.rawInput[t]);
	}
	printf("\ninverseInputScale:  ");
	for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
	    printf("\t%f", d.inverseInputScale[t]);
	}
#endif
	printf("\n");

    default:
	break;
    }

}

void usrmotPrintEmcmotConfig(emcmot_config_t c, int which)
{
//    int t;
//    char m[32];

    switch (which) {
    case 0:
	printf("debug level   \t%d\n", c.debug);
	printf("traj time:    \t%f\n", c.trajCycleTime);
	printf("servo time:   \t%f\n", c.servoCycleTime);
	printf("interp rate:  \t%d\n", c.interpolationRate);
	printf("v limit:      \t%f\n", c.limitVel);
	printf("axis vlimit:  \t");
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - waiting for new structs */
	for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
	    printf("%f ", c.axisLimitVel[t]);
	}
	printf("\n");
	printf("axis acc: \t");
	for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
	    printf("%f ", c.axisLimitAcc[t]);
	}
	printf("\n");
#endif
	printf("\n");
	break;

    case 1:
	printf("pid stuff is obsolete\n");
/*! \todo Another #if 0 */
#if 0
	printf
	    ("pid:\tP\tI\tD\tFF0\tFF1\tFF2\tBCKLSH\tBIAS\tMAXI\tDEADBAND\tCYCLE TIME\n");
	for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
	    printf
		("\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%f\t%f\n",
		c.pid[t].p, c.pid[t].i, c.pid[t].d, c.pid[t].ff0,
		c.pid[t].ff1, c.pid[t].ff2, c.pid[t].backlash, c.pid[t].bias,
		c.pid[t].maxError, c.pid[t].deadband, c.pid[t].cycleTime);
	}
	printf("\n");
#endif
	break;