linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
4e407d0a73271c7ef46f950f5a82b3a9db6c3197
sics/site_ansto/motor_dmc2280.c

3466 lines
104 KiB
C
Raw Blame History

/** \file motor_dmc2280.c
* \brief Driver for Galil DMC2280 motor controller.
*
* Implements a SICS motor object with a MotorDriver interface.
*
* Copyright: see file Copyright.txt
*
* Ferdi Franceschini November 2005
*
* TODO
* - check for motors enabled on plc
* - Check error bit, see Dan's email
*/
#include <stdlib.h>
/* ISO C Standard: 7.16 Boolean type and values <stdbool.h> */
#include <stdbool.h>
#include <math.h>
#include <float.h>
#include <assert.h>
#include <string.h>
#include <stdarg.h>
#include <sys/time.h>
#include <fortify.h>
#include <sics.h>
#include <asyncqueue.h>
#include <nwatch.h>
#include <modriv.h>
#include <motor.h>
#include <dynstring.h>
#include <time.h>
#include "anstoutil.h"
#define UNITSLEN 256
#define TEXTPARLEN 1024
#define CMDLEN 1024
/** \brief Used to ensure that the getDMCSetting function is called
* with valid values.
* \see getDMCSetting
*/
enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration};
enum commandtype {CMD_RUN=1, CMD_HALT=2};
typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv;
enum eventtype {eTimerEvent, eMessageEvent, eCommandEvent, eTimeoutEvent};
typedef struct EvtEvent_s EvtEvent, *pEvtEvent;
typedef void (*StateFunc)(pDMC2280Driv self, pEvtEvent event);
typedef struct EvtTimer_s { } EvtTimer;
typedef struct EvtMessage_s {
pAsyncTxn cmd;
} EvtMessage;
typedef struct EvtCommand_s {
enum commandtype cmd_type;
} EvtCommand;
typedef struct EvtTimeout_s { } EvtTimeout;
struct EvtEvent_s {
enum eventtype event_type;
union {
EvtTimer tmr;
EvtMessage msg;
EvtCommand cmd;
EvtTimeout tmo;
} event;
};
static pAsyncProtocol DMC2280_Protocol = NULL;
/*-----------------------------------------------------------------------
The motor driver structure. Please note that the first set of fields has
be identical with the fields of AbstractModriv in ../modriv.h
------------------------------------------------------------------------*/
struct __MoDriv {
/* general motor driver interface
fields. _REQUIRED!
*/
float fUpper; /**< hard upper limit */
float fLower; /**< hard lower limit */
char *name; /**< motor name */
int (*GetPosition)(void *self, float *fPos);
int (*RunTo)(void *self, float fNewVal);
int (*GetStatus)(void *self);
void (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
int (*TryAndFixIt)(void *self,int iError, float fNew);
int (*Halt)(void *self);
int (*GetDriverPar)(void *self, char *name,
float *value);
int (*SetDriverPar)(void *self,SConnection *pCon,
char *name, float newValue);
void (*ListDriverPar)(void *self, char *motorName,
SConnection *pCon);
void (*KillPrivate)(/*@only@*/void *self);
int (*GetDriverTextPar)(void *self, char *name,
char *textPar);
/* DMC-2280 specific fields */
pAsyncUnit asyncUnit;
pMotor pMot; /**< Points to logical motor object */
int errorCode;
char *errorMsg; /**< Points to memory for error messages */
char units[256]; /**< physical units for axis */
char long_name[256]; /**< long name of motor */
char part[256]; /**< assembly which motor belongs to */
float speed; /**< physical units per second */
float maxSpeed; /**< physical units per second */
float accel; /**< physical units per second^2 */
float maxAccel; /**< physical units per second^2 */
float decel; /**< physical units per second^2 */
float maxDecel; /**< physical units per second^2 */
char axisLabel;
char lastCmd[CMDLEN];
char dmc2280Error[CMDLEN];
float home; /**< home position for axis, default=0 */
int motorHome; /**< motor home position in steps */
int noPowerSave; /**< Flag = 1 to leave motors on after a move */
float stepsPerX; /**< steps per physical unit */
int abs_encoder; /**< Flag = 1 if there is an abs enc */
int absEncHome; /**< Home position in counts for abs enc */
float cntsPerX; /**< absolute encoder counts per physical unit */
int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */
int currFlags;
int currSteps;
int currCounts;
float currPosition; /**< Position at last position check */
float lastPosition; /**< Position at last position check */
float lastSteps;
float lastCounts;
struct timeval time_lastPos_set; /**< Time when lastPosition was set */
float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */
float blockage_thresh; /**< motion threshold for blockage checking */
float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */
int blockage_fail; /**< flag =1 if we should fail the motor */
int has_airpads; /**< Flag = 1 if there is are airpads for this motor */
float backlash_offset; /**< signed offset to drive from */
float fTarget; /**< target passed from SICS to timer callback */
int settle; /**< motor settling time in seconds */
struct timeval time_settle_done; /**< time when settling will be over */
int airpad_state; /**< state of the airpads finite state machine */
int airpad_counter; /**< airpad timer retry counter */
pNWTimer airpad_timer; /**< timer waiting for airpad action to complete */
pNWTimer motoff_timer; /**< motor off timer */
int debug;
int stepCount; /**< number of step operations for this move cycle */
float creep_offset; /**< last little bit to creep in units */
float creep_precision;
int creep_val;
int preseek; /**< Flag = 1 if current move is a backlash preseek */
int has_fsm; /**< Flag = 1 if motor has finite state machine driver model */
int run_flag; /**< Flag = 1 if RUN command has been issued and deferred */
int driver_status;
StateFunc myState;
StateFunc myPrevState;
int subState;
pNWTimer state_timer; /**< motor state timer */
SConnection *trace;
};
int DMC2280MotionControl = 1; /* defaults to enabled */
#define AIR_POLL_TIMER 1000
#define MOTOR_POLL_FAST 100
#define MOTOR_POLL_SLOW 1000
#define ON_SETTLE_TIMER 1000
#define MAX_CREEP_STEPS 100
#define AIRPADS_DOWN 0
#define AIRPADS_RAISE 1
#define AIRPADS_UP 2
#define AIRPADS_LOWER 3
/*------------------- error codes ----------------------------------*/
#define BADADR -1 // NOT SET: Unknown host/port?
#define BADBSY -2
#define BADCMD -3
#define BADPAR -4 // NOT SET: Does SICS already check parameter types?
#define BADUNKNOWN -5
#define BADSTP -6 // NOT SET
#define BADEMERG -7 // NOT SET: ESTOP
#define RVRSLIM -8
#define FWDLIM -9
#define POSFAULT -11 // NOT SET
#define BADCUSHION -12 // Air Cushion Fault
#define ERRORLIM -13
#define IMPOSSIBLE_LIM_SW -14
#define BGFAIL -15 // NOT SET
#define BLOCKED -16
#define MOTIONCONTROLOFF -17
#define MOTIONCONTROLUNK -18
#define MOTCMDTMO -19 /* Motor Command Timeout */
#define STATEERROR -20
#define THREADZERO -21 /* Thread zero not running */
/*--------------------------------------------------------------------*/
#define STATUSMOVING 128 /* Motor is moving */
#define STATUSERRORLIMIT 64 /* Number of errors exceed limit */
#define STATUSOFF 32 /* Motor off */
#define STATUSFWDLIMIT 8 /* Forward limit switch active */
#define STATUSRVRSLIMIT 4 /* Reverse limit switch active */
#define INIT_STR_SIZE 256
#define STR_RESIZE_LENGTH 256
#define BUFFLEN 512
#define _SAVEPOWER 0
#define SETPOS "setpos"
#define HOME "home"
#define HARDLOWERLIM "hardlowerlim"
#define HARDUPPERLIM "hardupperlim"
#define UNITS "units"
#define SPEED "speed"
#define MAXSPEED "maxspeed"
#define ACCEL "accel"
#define MAXACCEL "maxaccel"
#define DECEL "decel"
#define MAXDECEL "maxdecel"
#define MOTOFFDELAY "motoffdelay"
#define AIRPADS "airpads"
#define SETTLE "settle"
#define LONG_NAME "long_name"
#define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval"
static int DMC2280Halt(void *pData);
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue);
/** \brief Convert axis speed in physical units to
* motor speed in steps/sec.
* \param self (r) provides access to the motor's data structure
* \param speed in physical units, eg mm/sec degrees/sec
* \return the speed in motor steps/sec
*/
static int motSpeed(pDMC2280Driv self, float axisSpeed) {
int speed;
speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5);
return speed;
}
/** \brief Convert axis acceleration in physical units to
* to motor speed in steps/sec^2
* \param self (r) provides access to the motor's data structure
* \param acceleration in physical units, eg mm/sec^2 degrees/sec^2
* \return the acceleration in motor steps/sec^2
*/
static int motAccel(pDMC2280Driv self, float axisAccel) {
int accel;
accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5);
return accel;
}
/** \brief Convert axis deceleration in physical units to
* motor deceleration in steps/sec^2
* \param self (r) provides access to the motor's data structure
* \param deceleration in physical units, eg mm/sec^2 degrees/sec^2
* \return the deceleration in motor steps/sec^2
*/
static int motDecel(pDMC2280Driv self, float axisDecel) {
int decel;
decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5);
return decel;
}
/** \brief Convert motor position to physical units
* using the current motor steps or encoder counts
* \param self (r) provides access to the motor's data structure
* \return the motor position in physical units
*/
static float motPosit(pDMC2280Driv self) {
float fPos;
if (self->abs_encoder)
fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->home;
else
fPos = (self->currSteps - self->motorHome) / self->stepsPerX + self->home;
return fPos;
}
/** \brief Convert motor target in physical units to
* motor absolute position in steps
* \param self (r) provides access to the motor's data structure
* \param target in physical units, eg mm, degrees
* \return the absolute position in motor steps
*/
static int motAbsol(pDMC2280Driv self, float target) {
double absolute;
int result;
if (self->abs_encoder) {
#if 1
/* distance of target from home in units */
absolute = target - self->home;
/* subtract current encoder position in units */
absolute -= (self->currCounts - self->absEncHome) / self->cntsPerX;
/* convert to motor steps */
absolute *= self->stepsPerX;
/* add current position in steps */
absolute += (double) self->currSteps;
#else
absolute = ((self->absEncHome - self->currCounts) +
(self->cntsPerX * (target - self->home))) *
(self->stepsPerX / self->cntsPerX) +
self->currSteps;
#endif
}
else {
absolute = (target - self->home) * self->stepsPerX + self->motorHome;
}
if (absolute > 0)
result = (int) (absolute + 0.5);
else if (absolute < 0)
result = (int) (absolute - 0.5);
else
result = (int) absolute;
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Rounding %f to %d", absolute, result);
SICSLogWrite(line, eStatus);
}
return result;
}
/** \brief Convert motor target in physical units to
* motor absolute position in steps
* \param self (r) provides access to the motor's data structure
* \param target in physical units, eg mm, degrees
* \return the absolute position in motor steps
*/
static int motCreep(pDMC2280Driv self, float target) {
int target_steps = motAbsol(self, target);
if (!self->abs_encoder || /* no absolute encoder */
self->preseek || /* backlash preseek active */
self->creep_offset == 0) /* not creeping anyway */
return target_steps;
else {
int offset = target_steps - self->currSteps;
if (self->creep_val == 0) { /* initial creep step */
if (offset > 0) /* moving up */
self->creep_val = -1;
else if (offset < 0) /* moving down */
self->creep_val = +1;
}
else {
if (self->creep_val > 0) /* moving down */
++self->creep_val;
else
--self->creep_val;
if (abs(self->creep_val) > MAX_CREEP_STEPS &&
abs(self->creep_val) > 2.0 * fabs(self->stepsPerX / self->cntsPerX)) {
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s creep stopped, stepcount = %d",
self->name, self->stepCount);
SICSLogWrite(line, eStatus);
}
return target_steps;
}
}
if (self->creep_val > 0) /* moving down handle as positive */
offset = -offset;
if (offset > fabs(self->stepsPerX * self->creep_precision)) {
if (offset - (int) fabs(self->stepsPerX * self->creep_offset) > (int) fabs(self->stepsPerX / self->cntsPerX))
offset = offset - fabs(self->stepsPerX * self->creep_offset);
else {
if (offset <= fabs(self->stepsPerX / self->cntsPerX))
offset = 1;
else
offset = offset / 2;
}
self->preseek = 1;
}
if (self->creep_val > 0) /* moving down restore to negative */
offset = - offset;
if (self->debug) {
char text[CMDLEN];
snprintf(text, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d",
self->currSteps, target_steps, offset, self->currSteps + offset);
SICSLogWrite(text, eStatus);
}
target_steps = self->currSteps + offset;
}
return target_steps;
}
static int DMC_Tx(pAsyncProtocol p, pAsyncTxn ctx)
{
int iRet = 1;
pAsyncTxn myCmd = (pAsyncTxn) ctx;
if (myCmd) {
myCmd->txn_status = ATX_ACTIVE;
iRet = AsyncUnitWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
/* TODO handle errors */
if (iRet < 0) { /* TODO: EOF */
/*
iRet = AsyncUnitReconnect(myCmd->unit);
if (iRet == 0)
*/
return 0;
}
}
return 1;
}
static int DMC_Rx(pAsyncProtocol p, pAsyncTxn ctx, int rxchar) {
int iRet = 1;
pAsyncTxn myCmd = (pAsyncTxn) ctx;
switch (myCmd->txn_state) {
case 0: /* first character */
if (rxchar == ':') {
/* normal prompt */
myCmd->txn_state = 99;
myCmd->txn_status = ATX_COMPLETE;
}
else if (rxchar == '?') {
/* error prompt, send TC1 ahead of any queued commands */
iRet = AsyncUnitWrite(myCmd->unit, "TC1\r\n", 5);
myCmd->txn_state = 1;
}
else {
/* normal data */
myCmd->txn_state = 1;
}
/* note fallthrough */
case 1: /* receiving reply */
if (myCmd->inp_idx < myCmd->inp_len)
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
if (rxchar == 0x0D)
myCmd->txn_state = 2;
break;
case 2: /* received CR and looking for LF */
if (myCmd->inp_idx < myCmd->inp_len)
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
if (rxchar == 0x0A) {
/* end of line */
/*
myCmd->inp_idx -= 2;
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
*/
myCmd->txn_state = 0;
}
else
myCmd->txn_state = 1;
break;
}
if (myCmd->txn_state == 99) {
myCmd->inp_buf[myCmd->inp_idx] = '\0';
if (strncmp(myCmd->inp_buf, myCmd->out_buf, myCmd->out_len) == 0) {
int i;
SICSLogWrite("Line echo detected", eStatus);
for (i = myCmd->out_len; i <= myCmd->inp_idx; ++i) {
myCmd->inp_buf[i - myCmd->out_len] = myCmd->inp_buf[i];
}
}
iRet = 0;
}
if (iRet == 0) { /* end of command */
return AQU_POP_CMD;
}
return iRet;
}
static int DMC_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
if (event == AQU_TIMEOUT) {
/* handle command timeout */
pTxn->txn_status = ATX_TIMEOUT;
return AQU_POP_CMD;
}
return AQU_POP_CMD;
}
static int DMC_SendCmd(pDMC2280Driv self,
char* command,
AsyncTxnHandler callback)
{
return AsyncUnitSendTxn(self->asyncUnit,
command, strlen(command),
callback, self, CMDLEN);
}
/**
* \brief SendCallback is the callback for the general command.
*/
static int SendCallback(pAsyncTxn pCmd)
{
char* cmnd = pCmd->out_buf;
char* resp = pCmd->inp_buf;
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
if (pCmd->txn_status == ATX_TIMEOUT) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite("<TIMEOUT>", eStatus);
}
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
}
else {
switch (resp[0]) {
case ':':
case ' ':
if (self->debug) {
SICSLogWrite(cmnd, eStatus);
SICSLogWrite(resp, eStatus);
}
break;
case '?':
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
strncpy(self->dmc2280Error, &resp[1], CMDLEN);
SICSLogWrite(cmnd, eError);
SICSLogWrite(resp, eError);
self->errorCode = BADCMD;
break;
default:
SICSLogWrite(cmnd, eError);
SICSLogWrite(resp, eError);
self->errorCode = BADUNKNOWN;
break;
}
}
return 0;
}
static int DMC2280Queue(pDMC2280Driv self, char *cmd, AsyncTxnHandler cb) {
if (cb == NULL)
cb = SendCallback;
return DMC_SendCmd(self, cmd, cb);
}
/** \brief Sends a DMC2280 command to the motor controller.
*
* If the command fails it displays the DMC2280 error message to the client
* and writes it to the log file, also sets errorCode field in motor's
* data structure.
*
* \param self (rw) provides access to the motor's data structure
* \param *command (r) DMC2280 command
* \return
* - SUCCESS
* - FAILURE
* \see SUCCESS FAILURE
*/
/* First character returned by controller is
'?' for an invalid command or
':' for a valid command with no response
' ' for a valid command with a response (':' follows)
*/
static int DMC2280Send(pDMC2280Driv self, char *command) {
return DMC2280Queue(self, command, SendCallback);
}
/**
* \brief Sends a command and waits for a response
*
* \param self motor data
* \param cmd command to send
* \param reply space to return response
* \return
*/
static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
int status;
status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, CMDLEN);
if (status != 1) {
if (self->debug)
SICSLogWrite(cmd, eStatus);
if (status == -1)
self->errorCode = MOTCMDTMO;
else
self->errorCode = BADUNKNOWN;
return FAILURE;
}
switch (reply[0]) {
case ':':
if (self->debug) {
SICSLogWrite(cmd, eStatus);
SICSLogWrite(reply, eStatus);
}
return SUCCESS;
case ' ':
if (self->debug) {
SICSLogWrite(cmd, eStatus);
SICSLogWrite(reply, eStatus);
}
return SUCCESS;
case '?':
strncpy(self->lastCmd, cmd, CMDLEN);
strncpy(self->dmc2280Error, &reply[1], CMDLEN);
SICSLogWrite(cmd, eError);
SICSLogWrite(reply, eError);
self->errorCode = BADCMD;
return FAILURE;
default:
strncpy(self->lastCmd, cmd, CMDLEN);
strncpy(self->dmc2280Error, &reply[0], CMDLEN);
SICSLogWrite(cmd, eError);
SICSLogWrite(reply, eError);
self->errorCode = BADUNKNOWN;
return FAILURE;
}
return OKOK;
}
/** \brief Record the given posn and timestamp it.
*
* \param *pData provides access to a motor's data
* \param posn, the axis position which you want to remember.
* */
static void set_lastMotion(pDMC2280Driv self, float steps, float counts) {
assert(self != NULL);
self->lastSteps = steps;
self->lastCounts = counts;
gettimeofday(&(self->time_lastPos_set), NULL);
}
static int set_currMotion(pDMC2280Driv self, const char* text) {
int iRet, iFlags;
double steps, counts, flags;
iRet = sscanf(text, "%lf %lf %lf", &steps, &counts, &flags);
if (iRet != 3)
return 0;
iFlags = (int)(flags + 0.1);
self->currFlags = iFlags;
self->currSteps = steps;
self->currCounts = counts;
self->currPosition = motPosit(self);
return 1;
}
/** \brief Reads motion.
*
* \param *steps motor steps
* \param *counts encoder counts
* \return
* SUCCESS
* FAILURE
*/
static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
char reply[CMDLEN];
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel);
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return FAILURE;
if (2 != sscanf(reply, "%f %f", steps, counts))
return FAILURE;
return SUCCESS;
}
/** \brief Reads absolute encoder.
*
* \param *pos is the absolute encoder reading on SUCCESS.
* \return
* SUCCESS
* FAILURE
*/
static int readAbsEnc(pDMC2280Driv self, float *pos) {
char reply[CMDLEN];
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return FAILURE;
*pos = (float) atoi(reply);
return SUCCESS;
}
/** \brief Reads Thread variable
*
* \param *pos is the thread variable value on SUCCESS.
* \return
* SUCCESS
* FAILURE
*/
static int ReadThread(pDMC2280Driv self, int thread, float *pos) {
char reply[CMDLEN];
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG _XQ%d", thread);
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return 0;
*pos = (float) atoi(reply);
return 1;
}
/** \brief Reads HOMERUN variable
*
* \param *pos is the homerun variable value on SUCCESS.
* \return
* SUCCESS
* FAILURE
*/
static int readHomeRun(pDMC2280Driv self, float *pos) {
char reply[CMDLEN];
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG HOMERUN");
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return FAILURE;
*pos = (float) atoi(reply);
return SUCCESS;
}
/** \brief Runs the HOMERUN routine on the MC
*
* \return
* SUCCESS
* FAILURE
*/
static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
float fValue;
char cmd[CMDLEN];
/* Read HOMERUN should get error if it does not have one */
if (FAILURE == readHomeRun(self, &fValue))
return FAILURE;
/* 0 => reset homerun */
if (newValue < 0.5) {
snprintf(cmd, CMDLEN, "HOMERUN=0");
if (FAILURE == DMC2280Send(self, cmd))
return FAILURE;
return SUCCESS;
}
snprintf(cmd, CMDLEN, "XQ #HOME,1");
if (FAILURE == DMC2280Send(self, cmd))
return FAILURE;
return SUCCESS;
}
/**
* \brief calculate and issue the motion commands
*
* \param self provides access to a motor's data
* \param fValue target position in physical units, software zeros
* have already been applied.
* \return
* - OKOK request succeeded
* - HWFault request failed
*/
static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
char axis;
char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN];
int absEncHome, motorHome, newAbsPosn;
float stepsPerX, cntsPerX;
float target;
axis=self->axisLabel;
motorHome = self->motorHome;
stepsPerX=self->stepsPerX;
snprintf(SHx, CMDLEN, "SH%c", axis);
snprintf(BGx, CMDLEN, "BG%c", axis);
target = fValue - self->home;
if (self->preseek)
self->preseek = 0;
else {
if (self->backlash_offset > FLT_EPSILON) {
if (target > self->lastPosition) {
self->preseek = 1;
target += self->backlash_offset;
if (target > self->fUpper)
target = self->fUpper;
}
}
else if (self->backlash_offset < -FLT_EPSILON) {
if (target < self->lastPosition) {
self->preseek = 1;
target += self->backlash_offset;
if (target < self->fLower)
target = self->fLower;
}
}
}
if (1 == self->abs_encoder) {
absEncHome = self->absEncHome;
cntsPerX = self->cntsPerX;
#if 0
/* PAF=-((absEncHome-_TPF)/-cntsPerX + target)*stepsPerX + _TDF */
snprintf(absPosCmd, CMDLEN,
"PA%c=(((%d-_TP%c)/%f)+%f)*%f + _TD%c",
axis,
absEncHome,
axis,
cntsPerX,
target,
stepsPerX,
axis);
#else
/* PAZ=((absEncHome-_TPZ) + (cntsPerX * target)) * stepsPerX / cntsPerX + _TDZ */
char s_cnts[20];
char s_trgt[20];
char s_stps[20];
int i;
snprintf(s_cnts, sizeof(s_cnts), "%.4f", cntsPerX);
for (i = strlen(s_cnts); i > 0; --i)
if (s_cnts[i-1] == '.') {
s_cnts[i-1] = '\0';
break;
}
else if (s_cnts[i-1] == '0')
s_cnts[i-1] = '\0';
else
break;
snprintf(s_trgt, sizeof(s_trgt), "%.4f", target);
for (i = strlen(s_trgt); i > 0; --i)
if (s_trgt[i-1] == '.') {
s_trgt[i-1] = '\0';
break;
}
else if (s_trgt[i-1] == '0')
s_trgt[i-1] = '\0';
else
break;
snprintf(s_stps, sizeof(s_stps), "%.4f", stepsPerX);
for (i = strlen(s_stps); i > 0; --i)
if (s_stps[i-1] == '.') {
s_stps[i-1] = '\0';
break;
}
else if (s_stps[i-1] == '0')
s_stps[i-1] = '\0';
else
break;
snprintf(absPosCmd, CMDLEN,
"PA%c=((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c",
axis,
absEncHome,
axis,
s_cnts,
s_trgt,
s_stps,
s_cnts,
axis);
#endif
#ifdef BACKLASHFIX
do {
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis,
(int) (target * cntsPerX + absEncHome + 0.5));
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
} while (0);
#endif
} else {
newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5);
snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn);
}
if (FAILURE == DMC2280Send(self, SHx))
return HWFault;
if (FAILURE == DMC2280Send(self, absPosCmd))
return HWFault;
if (FAILURE == DMC2280Send(self, BGx))
return HWFault;
return OKOK;
}
/**
* \brief process the airpad status response
*/
static int airpad_callback(pAsyncTxn pCmd) {
char* resp = pCmd->inp_buf;
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
if (pCmd->txn_status == ATX_TIMEOUT) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite("<TIMEOUT>", eStatus);
}
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
}
else {
float fReply;
if (self->debug) {
SICSLogWrite(pCmd->inp_buf, eStatus);
SICSLogWrite(pCmd->out_buf, eStatus);
}
fReply = (float) atof(resp);
if (self->airpad_state == AIRPADS_RAISE && fReply > 0) {
int iRet;
self->airpad_state = AIRPADS_UP;
iRet = DMC2280RunCommon(self, self->fTarget);
if (iRet != OKOK)
self->errorCode = BADCUSHION;
return 0;
}
if (self->airpad_state == AIRPADS_LOWER && fReply == 0) {
self->airpad_state = AIRPADS_DOWN;
return 0;
}
}
return 0;
}
/**
* \brief request the airpad status periodically
*/
static int airpad_timeout(void* ctx, int mode) {
pDMC2280Driv self = (pDMC2280Driv) ctx;
self->airpad_timer = NULL;
if (self->airpad_state == AIRPADS_UP ||
self->airpad_state == AIRPADS_DOWN)
return 0;
if (self->airpad_counter <= 0) {
self->errorCode = BADCUSHION;
self->airpad_state = AIRPADS_DOWN;
return 0;
}
--self->airpad_counter;
NetWatchRegisterTimer(&self->airpad_timer,
AIR_POLL_TIMER,
airpad_timeout, self);
if (FAILURE == DMC2280Queue(self, "MG APDONE", airpad_callback)) {
self->errorCode = BADCUSHION;
self->airpad_state = AIRPADS_DOWN;
return 0;
}
return 0;
}
/**
* \brief initiate the raising or lowering of the airpads
*
* \param self motor data
* \param flag 1 for raise and 0 for lower
* \return 1 for SUCCESS or 0 for FAILURE
*/
static int DMC_AirPads(pDMC2280Driv self, int flag) {
char *cmd = NULL;
if (self->airpad_timer)
NetWatchRemoveTimer(self->airpad_timer);
self->airpad_timer = NULL;
if (flag) {
cmd = "FTUBE=1";
self->airpad_state = AIRPADS_RAISE;
}
else {
cmd = "FTUBE=0";
self->airpad_state = AIRPADS_LOWER;
}
if (FAILURE == DMC2280Send(self, cmd)) {
self->airpad_state = AIRPADS_DOWN;
self->errorCode = BADCUSHION;
return 0;
}
self->airpad_counter = 10;
NetWatchRegisterTimer(&self->airpad_timer,
AIR_POLL_TIMER,
airpad_timeout, self);
return 1;
}
/**
* \brief turn the motor off after a delay
*
* \param context motor data
* \param mode
*/
static int motoff_timeout(void* context, int mode) {
pDMC2280Driv self = (pDMC2280Driv) context;
char cmd[CMDLEN];
self->motoff_timer = NULL;
if (self->has_airpads) {
DMC_AirPads(self, 0);
return 0;
}
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
DMC2280Send(self, cmd);
return 0;
}
/** \brief Check if the axis has moved significantly since
* the last check.
*
* The motion is checked against the expected at intervals of
* pDMC2280Driv->blockage_ckInterval.
*
* \param *pData provides access to a motor's data
* \return
* - 1 MOTOR OK, position has changed significantly during move
* - 0 MOTOR BLOCKED, no significant change in position detected.
*/
static int checkMotion(void *pData) {
float steps, counts, ratio_obs, ratio_exp, ratio_cmp;
long int usec_TimeDiff;
struct timeval now;
pDMC2280Driv self;
self = (pDMC2280Driv)pData;
assert(self != NULL);
/* we can only test if there is an absolute encoder */
if (!self->abs_encoder)
return 1;
if (self->time_lastPos_set.tv_sec == 0) {
/* first time - initialise the data */
if (self->has_fsm) {
steps = self->currSteps;
counts = self->currCounts;
}
else {
if (FAILURE == readMotion(self, &steps, &counts))
return 0;
}
set_lastMotion(pData, steps, counts);
return 1;
}
gettimeofday(&now, NULL);
usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec;
usec_TimeDiff *= 1000000;
usec_TimeDiff += now.tv_usec;
usec_TimeDiff -= self->time_lastPos_set.tv_usec;
if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
return 1;
if (self->has_fsm) {
steps = self->currSteps;
counts = self->currCounts;
}
else {
if (FAILURE == readMotion(self, &steps, &counts))
return 0;
}
/* If not stepping, then not blocked */
if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
/* just update the timestamp */
set_lastMotion(pData, self->lastSteps, self->lastCounts);
return 1;
}
/* calculate observed and expected steps per count ratios */
if (counts == self->lastCounts) /* prevent divide by zero */
ratio_obs = (steps - self->lastSteps);
else
ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts);
ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX;
ratio_cmp = ratio_obs / ratio_exp;
/* wrong signs, less than half, or more than double is trouble */
if (ratio_cmp < 0.0 ||
ratio_cmp > self->blockage_ratio ||
(1.0 / ratio_cmp) > self->blockage_ratio) {
char msg[132];
float cmp = ratio_cmp;
if (fabs(cmp) > 0 && fabs(cmp) < 1)
cmp = 1.0 / cmp;
snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f",
self->name, ratio_obs, ratio_exp, cmp);
SICSLogWrite(msg, eError);
snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%f/%f",
steps, self->lastSteps, counts, self->lastCounts,
self->stepsPerX, self->cntsPerX);
SICSLogWrite(msg, eError);
if (self->blockage_fail)
return 0;
set_lastMotion(pData, steps, counts);
return 1;
} else {
if (self->debug) {
char msg[132];
float cmp = ratio_cmp;
if (fabs(cmp) > 0 && fabs(cmp) < 1)
cmp = 1.0 / cmp;
snprintf(msg, sizeof(msg), "Motor %s pass: obs=%f, exp=%f, cmp=%f",
self->name, ratio_obs, ratio_exp, cmp);
SICSLogWrite(msg, eError);
}
set_lastMotion(pData, steps, counts);
return 1;
}
return 1;
}
/**
* \brief handle the run command for a motor with airpads
*
* \param self motor data
* \param fValue new motor position
*/
static int DMC2280RunAir(pDMC2280Driv self, float fValue) {
if (!DMC_AirPads(self, 1))
return HWFault;
return OKOK;
}
/* State Functions */
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event);
static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event);
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event);
static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event);
static int state_msg_callback(pAsyncTxn pCmd);
static int state_tmr_callback(void* ctx, int mode);
static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd);
static char* state_name(StateFunc func) {
if (func == DMCState_Unknown) return "DMCState_Unknown";
if (func == DMCState_Idle) return "DMCState_Idle";
if (func == DMCState_AirOn) return "DMCState_AirOn";
if (func == DMCState_MotorOn) return "DMCState_MotorOn";
if (func == DMCState_Moving) return "DMCState_Moving";
if (func == DMCState_MotorHalt) return "DMCState_MotorHalt";
if (func == DMCState_OffTimer) return "DMCState_OffTimer";
if (func == DMCState_AirOff) return "DMCState_AirOff";
if (func == DMCState_MotorOff) return "DMCState_MotorOff";
if (func == DMCState_WaitStatus) return "DMCState_WaitStatus";
return "<unknown_state>";
}
void str_n_cat(char* s1, int len, const char* s2) {
int i = strlen(s1);
const char* p = s2;
while (i < len - 3 && *p) {
if (*p == '\r') {
s1[i++] = '\\';
s1[i++] = 'r';
++p;
}
else if (*p == '\n') {
s1[i++] = '\\';
s1[i++] = 'n';
++p;
}
else
s1[i++] = *p++;
}
s1[i] = '\0';
}
static char* event_name(pEvtEvent event, char* text, int length) {
switch (event->event_type) {
case eTimerEvent:
snprintf(text, length, "eTimerEvent");
return text;
case eMessageEvent:
snprintf(text, length, "eMessageEvent:");
str_n_cat(text, length, event->event.msg.cmd->out_buf);
str_n_cat(text, length, "|");
str_n_cat(text, length, event->event.msg.cmd->inp_buf);
return text;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
snprintf(text, length, "eCommandEvent:RUN");
return text;
case CMD_HALT:
snprintf(text, length, "eCommandEvent:HALT");
return text;
}
snprintf(text, length, "eCommandEvent:unknown");
return text;
case eTimeoutEvent:
snprintf(text, length, "eTimeoutEvent");
return text;
default:
snprintf(text, length, "<unknown_event>");
return text;
}
}
static void report_event(pDMC2280Driv self, pEvtEvent event) {
if (self->debug || self->trace) {
char line[CMDLEN];
char text[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s, State=%s, event=%s", self->name,
state_name(self->myState), event_name(event, text, CMDLEN));
if (self->debug)
SICSLogWrite(line, eStatus);
if (self->trace)
SCWrite(self->trace, line, eStatus);
}
}
static void change_state(pDMC2280Driv self, StateFunc func) {
if (self->debug || self->trace) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s, OldState=%s, NewState=%s", self->name,
state_name(self->myState), state_name(func));
if (self->debug)
SICSLogWrite(line, eStatus);
if (self->trace)
SCWrite(self->trace, line, eStatus);
}
self->myPrevState = self->myState;
self->myState = func;
self->subState = 0;
if (func == DMCState_Idle) {
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_SLOW,
state_tmr_callback, self);
}
}
static void unhandled_event(pDMC2280Driv self, pEvtEvent event) {
char line[CMDLEN];
char text[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s, State=%s, unhandled event=%s",
self->name,
state_name(self->myState),
event_name(event, text, CMDLEN));
SICSLogWrite(line, eStatus);
}
static int state_msg_callback(pAsyncTxn pCmd)
{
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
EvtEvent event;
if (pCmd->txn_status == ATX_TIMEOUT) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite("<TIMEOUT>", eStatus);
}
event.event_type = eTimeoutEvent;
event.event.msg.cmd = pCmd;
}
else {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite(pCmd->inp_buf, eStatus);
}
event.event_type = eMessageEvent;
event.event.msg.cmd = pCmd;
}
if (self->debug || self->trace)
report_event(self, &event);
self->myState(self, &event);
return 0;
}
static int state_tmr_callback(void* ctx, int mode)
{
pDMC2280Driv self = (pDMC2280Driv) ctx;
EvtEvent event;
self->state_timer = 0;
event.event_type = eTimerEvent;
if (self->debug || self->trace)
report_event(self, &event);
self->myState(self, &event);
return 0;
}
static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd) {
EvtEvent event;
event.event_type = eCommandEvent;
event.event.cmd.cmd_type = cmd;
if (self->debug || self->trace)
report_event(self, &event);
self->myState(self, &event);
return 0;
}
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
int value;
switch (event->event_type) {
case eTimerEvent:
/* Set speed */
value = motSpeed(self, self->speed);
snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
if (FAILURE == DMC2280Send(self, cmd))
break;
/* Set acceleration */
value = motAccel(self, self->accel);
snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value);
if (FAILURE == DMC2280Send(self, cmd))
break;
/* Set deceleration */
value = motDecel(self, self->decel);
snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value);
if (FAILURE == DMC2280Send(self, cmd))
break;
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet;
iRet = set_currMotion(self, pCmd->inp_buf);
if (iRet == 0)
break;
set_lastMotion(self, self->currSteps, self->currCounts);
if (self->abs_encoder == 0) {
change_state(self, DMCState_Idle);
return;
}
#if 0
value = ((self->currCounts - self->absEncHome) / self->cntsPerX) * self->stepsPerX;
self->currSteps = value;
snprintf(cmd, CMDLEN, "DP%c=%d", self->axisLabel, value);
#else
snprintf(cmd, CMDLEN, "LV");
#endif
DMC_SendCmd(self, cmd, state_msg_callback);
return;
}
if (pCmd->out_buf[0] == 'D') { /* DP */
change_state(self, DMCState_Idle);
return;
}
if (pCmd->out_buf[0] == 'L') { /* LV */
char req[12];
char rdy[12];
snprintf(req, 12, "REQ%c=", self->axisLabel);
snprintf(rdy, 12, "RSP%c=", self->axisLabel);
if (strstr(pCmd->inp_buf, req) &&
strstr(pCmd->inp_buf, rdy)) {
self->has_airpads = 2;
}
change_state(self, DMCState_Idle);
return;
}
}
while (0);
break;
case eTimeoutEvent:
/* TODO: handle timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_WaitStatus);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'M') { /* MG _XQ0,_TSx */
float fReply;
int iRet, iFlags;
int fwd_limit_active, rvrs_limit_active, errorlimit;
iRet = sscanf(pCmd->inp_buf, "%f %d", &fReply, &iFlags);
if (iRet != 2)
break;
if (fReply < 0) {
if (self->subState > 0) {
self->driver_status = HWFault;
self->errorCode = THREADZERO;
return;
}
DMC_SendCmd(self, "XQ #AUTO,0", state_msg_callback);
return;
}
/* Handle limit switches */
fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
errorlimit = (iFlags & STATUSERRORLIMIT);
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
self->driver_status = HWFault;
} else if (errorlimit) {
self->errorCode = ERRORLIM;
self->driver_status = HWFault;
}
if (self->driver_status == HWFault) {
return;
}
if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=1");
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOn);
}
else if (self->has_airpads == 2) {
snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOn);
}
else {
snprintf(cmd, CMDLEN, "SH%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_MotorOn);
}
return;
}
if (pCmd->out_buf[0] == 'X') { /* XQ #AUTO,0 */
snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
self->subState = 1;
}
} while (0);
break;
case eCommandEvent:
if (self->state_timer) {
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
}
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
self->driver_status = HWBusy;
snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
self->subState = 0;
return;
case CMD_HALT:
/* TODO: handle halt command */
return;
}
break;
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
if (self->has_airpads == 2)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else
snprintf(cmd, CMDLEN, "MG APDONE");
DMC_SendCmd(self, cmd, state_msg_callback);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'F' || /* FTUBE */
pCmd->out_buf[0] == 'R') { /* REQx= */
NetWatchRegisterTimer(&self->state_timer,
AIR_POLL_TIMER,
state_tmr_callback, self);
return;
}
else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx */
float fReply;
fReply = (float) atof(pCmd->inp_buf);
if (fReply > 0) {
NetWatchRegisterTimer(&self->state_timer,
ON_SETTLE_TIMER,
state_tmr_callback, self);
change_state(self, DMCState_MotorOn);
return;
}
/* TODO handle airpad timeout */
NetWatchRegisterTimer(&self->state_timer,
AIR_POLL_TIMER,
state_tmr_callback, self);
return;
}
} while (0);
break;
/* TODO: handle halt command */
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'S') { /* SH */
NetWatchRegisterTimer(&self->state_timer,
ON_SETTLE_TIMER,
state_tmr_callback, self);
return;
}
else if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet, absolute;
float target;
iRet = set_currMotion(self, pCmd->inp_buf);
if (iRet == 0)
break;
set_lastMotion(self, self->currSteps, self->currCounts);
/* compute position for PA command */
target = self->fTarget;
self->preseek = 0;
if (self->backlash_offset) {
if (self->backlash_offset > 0) {
/*
* We want to be moving from high to low,
* if the target is higher than current
* we must pre-seek to the higher side
*/
if (target > self->currPosition) {
self->preseek = 1;
target += self->backlash_offset;
if (target > self->fUpper)
target = self->fUpper;
}
}
else if (self->backlash_offset < 0) {
/*
* We want to be moving from to low high ,
* if the target is lower than current
* we must pre-seek to the lower side
*/
if (target < self->currPosition) {
self->preseek = 1;
target += self->backlash_offset;
if (target < self->fLower)
target = self->fLower;
}
}
}
self->creep_val = 0;
absolute = motCreep(self, target);
snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute);
DMC_SendCmd(self, cmd, state_msg_callback);
return;
}
else if (pCmd->out_buf[0] == 'P') { /* PA */
self->stepCount = 1;
snprintf(cmd, CMDLEN, "BG%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_Moving);
return;
}
} while (0);
break;
/* TODO: handle halt command */
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'B') { /* BG */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
else if (pCmd->out_buf[0] == 'P') { /* PA */
snprintf(cmd, CMDLEN, "BG%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
self->stepCount++;
return;
}
else if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet, iFlags;
bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
iRet = set_currMotion(self, pCmd->inp_buf);
if (iRet == 0)
break;
iFlags = self->currFlags;
moving = (iFlags & STATUSMOVING);
if (moving) {
/* If Motion Control is off, report HWFault */
if (DMC2280MotionControl != 1) {
state_cmd_execute(self, CMD_HALT);
if (DMC2280MotionControl == 0)
self->errorCode = MOTIONCONTROLOFF;
else
self->errorCode = MOTIONCONTROLUNK;
self->driver_status = HWFault;
return;
}
if (self->abs_encoder && checkMotion(self) == 0)
{
/* handle blocked */
state_cmd_execute(self, CMD_HALT);
self->errorCode = BLOCKED;
self->driver_status = HWFault;
return;
}
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
/* Motor has stopped */
/* Handle limit switches */
fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
errorlimit = (iFlags & STATUSERRORLIMIT);
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
self->driver_status = HWFault;
} else if (fwd_limit_active) {
self->errorCode = FWDLIM;
self->driver_status = HWFault;
} else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
self->driver_status = HWFault;
} else if (errorlimit) {
self->errorCode = ERRORLIM;
self->driver_status = HWFault;
}
if (self->driver_status == HWFault) {
state_cmd_execute(self, CMD_HALT);
return;
}
/*
* If this was a pre-seek then compute the next iteration
*/
if (self->preseek) {
float target;
int absolute;
self->preseek = 0;
target = self->fTarget;
/* if we are not creeping it is backlash correction */
/* TODO: ensure minimum progress so it doesn't stall */
/* TODO: handle edge conditions like limits */
if (self->creep_val == 0) {
float precision;
MotorGetPar(self->pMot, "precision", &precision);
/* TODO: take precision into account */
if (self->backlash_offset > 0) {
if (target + self->backlash_offset > self->currPosition + precision) {
self->preseek = 1;
target += self->backlash_offset + precision;
if (target > self->fUpper)
target = self->fUpper;
}
}
else if (self->backlash_offset < 0) {
if (target + self->backlash_offset < self->currPosition - precision) {
self->preseek = 1;
target += self->backlash_offset - precision;
if (target < self->fLower)
target = self->fLower;
}
}
/* limit the maximum number of tries */
if (self->preseek && self->stepCount > 10) {
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s preseek stopped stepcount = %d",
self->name, self->stepCount);
SICSLogWrite(line, eStatus);
}
self->preseek = 0;
}
else if (self->preseek) {
absolute = motCreep(self, target);
}
else {
/* change of direction, reset motion check */
set_lastMotion(self, self->currSteps, self->currCounts);
absolute = motCreep(self, target);
}
}
else
absolute = motCreep(self, target);
/*
* If we are still iterating, continue
*/
if (self->preseek) {
if (absolute == self->currSteps) {
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s motion stopped absolute = %d",
self->name, absolute);
SICSLogWrite(line, eStatus);
}
self->preseek = 0;
}
else {
snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute);
DMC_SendCmd(self, cmd, state_msg_callback);
return;
}
}
/*
* We are no longer iterating, so fall through
*/
}
/* handle settle time */
if (self->settle && self->time_settle_done.tv_sec == 0) {
gettimeofday(&self->time_settle_done, NULL);
self->time_settle_done.tv_sec += self->settle;
NetWatchRegisterTimer(&self->state_timer,
self->settle,
state_tmr_callback, self);
return;
}
if (self->motOffDelay)
NetWatchRegisterTimer(&self->state_timer,
self->motOffDelay,
state_tmr_callback, self);
else
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
state_tmr_callback, self);
self->driver_status = HWIdle;
change_state(self, DMCState_OffTimer);
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_HALT:
/* handle halt command, send message */
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_MotorHalt);
return;
}
break;
case eTimeoutEvent:
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
state_cmd_execute(self, CMD_HALT);
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
{
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'S') { /* ST */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
else if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet, iFlags;
iRet = set_currMotion(self, pCmd->inp_buf);
if (iRet == 0)
break;
iFlags = self->currFlags;
if (iFlags & STATUSMOVING) {
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=0");
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff);
return;
}
else if (self->has_airpads == 2) {
snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff);
return;
}
else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_MotorOff);
return;
}
}
} while (0);
break;
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=0");
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff);
}
else if (self->has_airpads == 2) {
snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff);
return;
}
else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_MotorOff);
}
return;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* handle run command, convert to motor on timer expired */
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
self->driver_status = HWBusy;
change_state(self, DMCState_MotorOn);
event->event_type = eTimerEvent;
self->myState(self, event);
return;
case CMD_HALT:
/* handle halt command, convert to motor off timer expired */
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
event->event_type = eTimerEvent;
self->myState(self, event);
return;
}
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
if (self->has_airpads == 2)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else
snprintf(cmd, CMDLEN, "MG APDONE");
DMC_SendCmd(self, cmd, state_msg_callback);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'F' || /* FTUBE */
pCmd->out_buf[0] == 'R') { /* REQx= */
}
else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx */
float fReply;
fReply = (float) atof(pCmd->inp_buf);
if (fReply == 0) {
NetWatchRegisterTimer(&self->state_timer,
ON_SETTLE_TIMER,
state_tmr_callback, self);
change_state(self, DMCState_MotorOff);
return;
}
}
NetWatchRegisterTimer(&self->state_timer,
AIR_POLL_TIMER,
state_tmr_callback, self);
} while (0);
return;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* TODO: handle run command */
return;
case CMD_HALT:
/* TODO: handle halt command */
return;
}
break;
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eTimerEvent:
case eMessageEvent:
/* progress to IDLE */
change_state(self, DMCState_Idle);
if (self->driver_status == HWBusy)
self->driver_status = HWIdle;
return;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* TODO: handle run command */
return;
case CMD_HALT:
/* TODO: handle halt command */
return;
}
break;
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eTimerEvent:
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'M') { /* MG _TD%c,_TP%c,_TS%c */
int iRet;
iRet = set_currMotion(self, pCmd->inp_buf);
if (iRet == 0)
break;
change_state(self, DMCState_Idle);
if (self->run_flag) {
self->run_flag = 0;
state_cmd_execute(self, CMD_RUN);
}
else {
float fDelta;
if (self->abs_encoder)
fDelta = fabs(self->currCounts - self->lastCounts);
else
fDelta = fabs(self->currSteps - self->lastSteps);
if (fDelta > 10) {
SConnection *pDumCon;
MotCallback sCall;
set_lastMotion(self, self->currSteps, self->currCounts);
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
pDumCon = SCCreateDummyConnection(pServ->pSics);
MotorGetSoftPosition(self->pMot,pDumCon,&sCall.fVal);
SCDeleteConnection(pDumCon);
sCall.pName = self->pMot->name;
InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall);
InvokeCallBack(self->pMot->pCall, MOTEND, &sCall);
}
}
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* TODO: handle run command */
self->driver_status = HWBusy;
self->run_flag = 1;
return;
case CMD_HALT:
/* TODO: handle halt command */
return;
}
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
/** \brief Reads motor position, implements the GetPosition
* method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \param *fPos contains the motor position in physical units after a call.
* \return
* - OKOK request succeeded
* - HWFault request failed
* */
static int DMC2280GetPos(void *pData, float *fPos){
pDMC2280Driv self = NULL;
char reply[CMDLEN];
char cmd[CMDLEN];
float absEncPos, motorPos;
reply[0]='\0';
self = (pDMC2280Driv)pData;
assert(self != NULL);
#if 1
if (self->has_fsm) {
if (self->myState == DMCState_Unknown)
SicsWait(1);
if (self->abs_encoder)
*fPos = (self->currCounts - self->absEncHome)/self->cntsPerX + self->home;
else
*fPos = (self->currSteps - self->motorHome)/self->stepsPerX + self->home;
return OKOK;
}
#endif
if (1 == self->abs_encoder) {
if (readAbsEnc(self, &absEncPos) == FAILURE)
return HWFault;
*fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home;
} else {
snprintf(cmd, ERRLEN, "TD%c", self->axisLabel);
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return HWFault;
motorPos =(float)atof(reply);
*fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home;
}
return OKOK;
}
/** \brief DMC2280 implementation of the RunTo
* method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \param fValue target position in physical units, software zeros
* have already been applied.
* \return
* - OKOK request succeeded
* - HWFault request failed
* */
static int DMC2280Run(void *pData,float fValue){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
assert(self != NULL);
/* If Motion Control is off, report HWFault */
if (DMC2280MotionControl != 1) {
if (DMC2280MotionControl == 0)
self->errorCode = MOTIONCONTROLOFF;
else
self->errorCode = MOTIONCONTROLUNK;
return HWFault;
}
if (self->motoff_timer)
NetWatchRemoveTimer(self->motoff_timer);
self->motoff_timer = NULL;
if (self->settle)
self->time_settle_done.tv_sec = 0;
/*
* Note: this will read the current position which will block
*/
do {
float currPos;
DMC2280GetPos(pData, &currPos);
self->lastPosition = currPos;
self->time_lastPos_set.tv_sec = 0;
} while (0);
self->fTarget = fValue;
if (self->has_fsm) {
/* TODO: FIXME and handle RUN command everywhere */
while (self->myState == DMCState_AirOff
|| self->myState == DMCState_MotorOff) {
SicsWait(1);
}
state_cmd_execute(self, CMD_RUN);
return 1;
}
/*
* Note: this passes control to a timer routine
*/
if (self->has_airpads)
if (self->airpad_state != AIRPADS_UP)
return DMC2280RunAir(self, fValue);
return DMC2280RunCommon(self, fValue);
}
/** \brief Returns the motor status while it's moving,
* implements the GetStatus method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \return
* - HWFault hardware fault or status cannot be read.
* - HWPosFault controller was unable to position the motor.
* - HWBusy The motor is still driving.
* - HWWarn There is a warning from the controller.
* - HWIdle The motor has finished driving and is idle.
*/
static int DMC2280Status(void *pData){
pDMC2280Driv self = NULL;
char cmd[CMDLEN];
int switches;
char switchesAscii[CMDLEN];
#ifdef BACKLASHFIX
char reply[CMDLEN];
int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus;
int SHOULD_FIXPOS=1, should_fixpos;
#endif
bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
self = (pDMC2280Driv)pData;
assert(self != NULL);
if (self->has_fsm) {
return self->driver_status;
}
/*
* If we are waiting for the motor or airpads then we
* are busy
*/
if (self->motoff_timer || self->airpad_timer)
return HWBusy;
/* Make sure that speed, accel and decel are set correctly */
/* ckSpeedAccelDecel(self); */
/* Get status of switches
* see TS (Tell Switches) in Galil manc2xx.pdf */
snprintf(cmd, CMDLEN, "TS%c", self->axisLabel);
if (FAILURE == DMC2280SendReceive(self, cmd, switchesAscii))
return HWFault;
sscanf(switchesAscii, "%d", &switches);
moving = (switches & STATUSMOVING)>0;
fwd_limit_active = !((switches & STATUSFWDLIMIT)>0);
rvrs_limit_active = !((switches & STATUSRVRSLIMIT)>0);
errorlimit = (switches & STATUSERRORLIMIT)>0;
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
return HWFault;
}
if (moving) {
int iRet;
/* If Motion Control is off, report HWFault */
if (DMC2280MotionControl != 1) {
if (DMC2280MotionControl == 0)
self->errorCode = MOTIONCONTROLOFF;
else
self->errorCode = MOTIONCONTROLUNK;
return HWFault;
}
/* If pos hasn't changed since last
* check then stop and scream */
#if 0
iRet = checkPosition(pData);
#else
iRet = checkMotion(pData);
#endif
if (iRet == 0) {
DMC2280Halt(pData);
self->errorCode = BLOCKED;
return HWFault;
} else {
self->errorCode = BADBSY;
return HWBusy;
}
} else {
/* If motor stopped check limits and error status */
if (fwd_limit_active) {
self->errorCode = FWDLIM;
return HWFault;
} else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
return HWFault;
} else if (errorlimit) {
self->errorCode = ERRORLIM;
return HWFault;
}
#ifdef BACKLASHFIX
if (self->abs_encoder == 1) {
/* Make sure that the servo loop is closed by checking if
* the CLSLOOP thread is running on the controller.*/
if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply))
return HWFault;
sscanf(reply, "%d", &servoLoopStatus);
if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) {
/* Start subroutine on controller to close the servo loop */
if (FAILURE == DMC2280Send(self, "XQ#CLSLOOP"))
return HWFault;
}
snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel);
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return HWFault;
sscanf(reply, "%d", &should_fixpos);
if (should_fixpos == SHOULD_FIXPOS) {
snprintf(cmd, CMDLEN, "%cFIXPOS=1", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
self->errorCode=BADBSY;
return HWBusy;
}
}
#endif
/*
* When we get here, the motion has completed
*/
if (self->preseek) {
DMC2280RunCommon(self, self->fTarget);
return HWBusy;
}
/* and we
* must determine when and how to shut off the motor
*/
if (self->settle) {
struct timeval *then = &self->time_settle_done;
struct timeval now;
gettimeofday(&now, NULL);
if (then->tv_sec == 0 ||
(then->tv_sec - now.tv_sec) > self->settle) {
gettimeofday(then, NULL);
then->tv_sec += self->settle;
return HWBusy;
} else {
if ((now.tv_sec > then->tv_sec) ||
((now.tv_sec == then->tv_sec) &&
(now.tv_usec >= then->tv_usec))) {
/* it's finished, fall through */
} else {
return HWBusy;
}
}
}
if (self->has_airpads) {
if (self->airpad_state == AIRPADS_DOWN)
return HWIdle;
if (self->airpad_state == AIRPADS_LOWER)
return HWBusy;
if (self->motOffDelay > 0 ) {
NetWatchRegisterTimer(&self->motoff_timer,
self->motOffDelay,
motoff_timeout, self);
return HWIdle;
}
if (!DMC_AirPads(self, 0))
return HWFault;
return HWIdle;
}
if (self->noPowerSave == _SAVEPOWER) {
if (self->motOffDelay > 0 ) {
#if 0
snprintf(cmd, CMDLEN, "AT %d; MO%c", self->motOffDelay, self->axisLabel);
#else
NetWatchRegisterTimer(&self->motoff_timer,
self->motOffDelay,
motoff_timeout, self);
return HWIdle;
#endif
} else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
}
DMC2280Send(self, cmd);
}
return HWIdle;
}
}
/** \brief DMC2280 implementation of the GetError
* method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \param *iCode error code returned to abstract motor
* \param *error error message
* \param errLen maximum error message length allowed by abstract motor
*/
static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
assert(self != NULL);
/* Allocate errLen bytes for error messages */
if (self->errorMsg == NULL) {
self->errorMsg = (char *) malloc(errLen);
if (self->errorMsg == NULL) {
*iCode = 0;
return;
}
}
*iCode = self->errorCode;
switch(*iCode){
#ifdef HAS_RS232
case NOTCONNECTED:
case BADSEND:
case BADMEMORY:
case INCOMPLETE:
strncpy(error, "General Error(TODO)", (size_t)errLen); /* TODO */
break;
#endif
case MOTCMDTMO:
strncpy(error, "Command Timeout", (size_t)errLen);
break;
case BADADR:
strncpy(error, "Bad address", (size_t)errLen);
break;
case BADBSY:
strncpy(error, "Motor still busy", (size_t)errLen);
break;
case BADCMD:
snprintf(error, (size_t)errLen, "Bad command: '%s'\ndmcError: ", self->lastCmd);
strncat(error, self->dmc2280Error, (size_t)errLen);
break;
case BADPAR:
strncpy(error, "Bad parameter", (size_t)errLen);
break;
case BADUNKNOWN:
strncpy(error, "Unknown error condition", (size_t)errLen);
break;
case BADSTP:
strncpy(error, "Motor is stopped", (size_t)errLen);
break;
case BADEMERG:
strncpy(error, "Emergency stop is engaged", (size_t)errLen);
break;
case BGFAIL:
strncpy(error, "Begin not possible due to limit switch", (size_t)errLen);
break;
case RVRSLIM:
strncpy(error, "Crashed into limit switch", (size_t)errLen);
break;
case FWDLIM:
strncpy(error, "Crashed into limit switch", (size_t)errLen);
break;
case POSFAULT:
strncpy(error, "Positioning fault detected", (size_t)errLen);
break;
case BADCUSHION:
strncpy(error, "Air cushion problem", (size_t)errLen);
break;
case ERRORLIM:
strncpy(error, "Axis error exceeds error limit", (size_t)errLen);
break;
case IMPOSSIBLE_LIM_SW:
strncpy(error, "Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen);
break;
case BLOCKED:
strncpy(error, "STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen);
break;
case MOTIONCONTROLOFF:
strncpy(error, "MOTION CONTROL SEEMS TO BE DISABLED", (size_t)errLen);
break;
case MOTIONCONTROLUNK:
strncpy(error, "MOTION CONTROL SEEMS TO BE UNKNOWN", (size_t)errLen);
break;
case STATEERROR:
strncpy(error, "ERROR IN DMC2280 FINITE STATE MACHINE", (size_t)errLen);
break;
case THREADZERO:
strncpy(error, "THREAD ZERO NOT RUNNING ON CONTROLLER", (size_t)errLen);
break;
default:
/* FIXME What's the default */
snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
break;
}
strncpy(self->errorMsg, error, (size_t)errLen);
}
/** \brief Attempts to recover from an error. Implements the TryAndFixIt
* method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \param iCode error code returned by DMC2280Error
* \param fValue unused, target position
* \return A return code which informs the abstract motors next action.
* - MOTREDO try to redo the last move.
* - MOTFAIL move failed, give up.
*/
static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
assert(self != NULL);
switch(iCode){
case BADADR:
return MOTFAIL;
case BADCMD:
case BADPAR:
case BLOCKED:
case MOTIONCONTROLOFF:
case MOTIONCONTROLUNK:
case MOTCMDTMO:
case THREADZERO:
return MOTFAIL;
case POSFAULT:
#if HAS_RS232
case BADSEND:
case TIMEOUT:
case BADMEMORY: /* Won't happen if MonConnect sets the send terminator */
case INCOMPLETE:
return MOTREDO;
case NOTCONNECTED:
#endif
return MOTREDO;
break;
case STATEERROR:
/* TODO: recover state error */
if (self->has_fsm) {
if (self->state_timer)
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
change_state(self, DMCState_Unknown);
/* Schedule a timer event as soon as possible */
NetWatchRegisterTimer(&self->state_timer,
0,
state_tmr_callback, self);
}
return MOTFAIL;
default:
return MOTFAIL;
break;
}
}
/** \brief Emergency halt. Implements the Halt
* method in the MotorDriver interface.
*
* Cannot set maximum deceleration because the DC command
* is not valid for absolute (ie PA) moves. See DC description
* in DMC-2xxx command ref (ie manc2xxx.pdf)
* \param *pData provides access to a motor's data
*
* XXX Does abstract motor use the return values?
*/
static int DMC2280Halt(void *pData){
pDMC2280Driv self = NULL;
char cmd[CMDLEN];
self = (pDMC2280Driv)pData;
assert(self != NULL);
if (self->has_fsm) {
state_cmd_execute(self, CMD_HALT);
return 1;
}
/* Stop motor */
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
/* TODO: FIXME cannot do this until motor stops */
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
else
return 1;
}
/** \brief Fetches the value of the named parameter,
* implements the GetDriverPar method in the MotorDriver interface.
*
* Note: The GetDriverPar method in the MotorDriver interface only
* allows float values to be returned.
*
* If the speed, acceleration or deceleration is requested then
* this compares the setting on the controller to the required setting,
* if they don't match then the controller is set to the required value.
*
* Note: Doesn't warn if the speed, acceleration, or deceleration set on
* the controller differ from the required settings.
*
* \param *pData (r) provides access to a motor's data
* \param *name (r) the name of the parameter to fetch.
* \param *fValue (w) the parameter's value.
* \return
* - 1 request succeeded
* - 0 request failed
* */
static int DMC2280GetPar(void *pData, char *name,
float *fValue){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
/* XXX Maybe move this to a configuration parameter.*/
if(strcasecmp(name,HOME) == 0) {
*fValue = self->home;
return 1;
}
if(strcasecmp(name,HARDLOWERLIM) == 0) {
*fValue = self->fLower;
return 1;
}
if(strcasecmp(name,HARDUPPERLIM) == 0) {
*fValue = self->fUpper;
return 1;
}
if(strcasecmp(name,SPEED) == 0) {
*fValue = self->speed;
return 1;
}
if(strcasecmp(name,MAXSPEED) == 0) {
*fValue = self->maxSpeed;
return 1;
}
if(strcasecmp(name,ACCEL) == 0) {
*fValue = self->accel;
return 1;
}
if(strcasecmp(name,MAXACCEL) == 0) {
*fValue = self->maxAccel;
return 1;
}
if(strcasecmp(name,DECEL) == 0) {
*fValue = self->decel;
return 1;
}
if(strcasecmp(name,MAXDECEL) == 0) {
*fValue = self->maxDecel;
return 1;
}
if(strcasecmp(name,MOTOFFDELAY) == 0) {
*fValue = self->motOffDelay;
return 1;
}
if(strcasecmp(name,"debug") == 0) {
*fValue = self->debug;
return 1;
}
if(strcasecmp(name,SETTLE) == 0) {
*fValue = self->settle;
return 1;
}
if(strcasecmp(name,AIRPADS) == 0) {
*fValue = self->has_airpads;
return 1;
}
if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
*fValue = self->blockage_ckInterval;
return 1;
}
if(strcasecmp(name,"blockage_thresh") == 0) {
*fValue = self->blockage_thresh;
return 1;
}
if(strcasecmp(name,"blockage_ratio") == 0) {
*fValue = self->blockage_ratio;
return 1;
}
if(strcasecmp(name,"blockage_fail") == 0) {
*fValue = self->blockage_fail;
return 1;
}
if(strcasecmp(name,"backlash_offset") == 0) {
*fValue = self->backlash_offset;
return 1;
}
if (self->abs_encoder != 0) {
if (strcasecmp(name,"absenc") == 0) {
if (readAbsEnc(self, fValue) == SUCCESS)
return 1;
else
return 0;
}
if (strcasecmp(name,"absenchome") == 0) {
*fValue = self->absEncHome;
return 1;
}
if (self->has_fsm) {
if(strcasecmp(name,"creep_offset") == 0) {
*fValue = self->creep_offset;
return 1;
}
if(strcasecmp(name,"creep_precision") == 0) {
*fValue = self->creep_precision;
return 1;
}
}
}
else {
if (strcasecmp(name,"homerun") == 0) {
if (readHomeRun(self, fValue) == SUCCESS)
return 1;
else
return 0;
}
}
if(strcasecmp(name,"thread0") == 0)
return ReadThread(self, 0, fValue);
if(strcasecmp(name,"thread1") == 0)
return ReadThread(self, 1, fValue);
if(strcasecmp(name,"thread2") == 0)
return ReadThread(self, 2, fValue);
if(strcasecmp(name,"thread3") == 0)
return ReadThread(self, 3, fValue);
if(strcasecmp(name,"thread4") == 0)
return ReadThread(self, 4, fValue);
if(strcasecmp(name,"thread5") == 0)
return ReadThread(self, 5, fValue);
if(strcasecmp(name,"thread6") == 0)
return ReadThread(self, 6, fValue);
if(strcasecmp(name,"thread7") == 0)
return ReadThread(self, 7, fValue);
return 0;
}
/** \brief Sets the named parameter, implements the SetDriverPar
* method in the MotorDriver interface.
*
* Note: The SetDriverPar method in the MotorDriver interface only
* allows float values to be set.
* \param *pData (rw) provides access to a motor's data
* \param *pCon (r) connection object.
* \param *name (r) of the parameter to set.
* \param *newValue (r) new value.
* \return
* - 1 request succeeded
* - 0 request failed
* */
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue){
pDMC2280Driv self = NULL;
char pError[ERRLEN];
char cmd[CMDLEN];
self = (pDMC2280Driv)pData;
/* XXX Maybe move this to a configuration parameter.*/
/* Set home, managers only. Users should set softposition */
if(strcasecmp(name,HOME) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
if ( (self->fLower - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
SCWrite(pCon, pError, eError);
return 1;
}
if ( (newValue - self->fUpper) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
SCWrite(pCon, pError, eError);
return 1;
}
self->home = newValue;
return 1;
}
}
if(strcasecmp(name,SETPOS) == 0) {
float oldZero, newZero;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
MotorGetPar(self->pMot,"softzero",&oldZero);
newZero = (self->pMot->fPosition - newValue) + oldZero;
MotorSetPar(self->pMot,pCon,"softzero",newZero);
return 1;
}
/* Set motor off delay, managers only */
if(strcasecmp(name,MOTOFFDELAY) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->motOffDelay = newValue;
return 1;
}
}
/* Debug, managers only */
if(strcasecmp(name,"debug") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->debug = newValue;
return 1;
}
}
/* Setttle, managers only */
if(strcasecmp(name,SETTLE) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->settle = newValue;
return 1;
}
}
/* Set airpads, managers only */
if(strcasecmp(name,AIRPADS) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->has_airpads = newValue;
return 1;
}
}
/* Set interval between blocked motor checks,
* managers only */
if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->blockage_ckInterval = newValue;
return 1;
}
}
/* Set movement threshold for blocked motor checks,
* managers only */
if(strcasecmp(name,"blockage_thresh") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->blockage_thresh = newValue;
return 1;
}
}
/* Set ratio for blocked motor checks,
* managers only */
if(strcasecmp(name,"blockage_ratio") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->blockage_ratio = newValue;
return 1;
}
}
/* Set blocked motor checks failure mode,
* managers only */
if(strcasecmp(name,"blockage_fail") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->blockage_fail = newValue;
return 1;
}
}
/* Set backlash offset,
* managers only */
if(strcasecmp(name,"backlash_offset") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->backlash_offset = newValue;
return 1;
}
}
if (self->abs_encoder) { /* If we DO have an absolute encoder */
if (self->has_fsm) { /* If we DO have a finite state machine */
/* Set creep offset */
if (strcasecmp(name,"creep_offset") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
self->creep_offset = fabs(newValue);
return 1;
}
}
/* Set creep_precision */
if (strcasecmp(name,"creep_precision") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
self->creep_precision = fabs(newValue);
return 1;
}
}
}
}
else { /* If we do NOT have an absolute encoder */
/* Invoke Home Run routine in controller */
if(strcasecmp(name,"homerun") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
if (DMC2280MotionControl != 1 && newValue > 0.5) {
snprintf(pError, ERRLEN,"ERROR: Motion Control must be on");
SCWrite(pCon, pError, eError);
}
RunHomeRoutine(self, newValue);
return 1;
}
}
}
/* Set speed */
if(strcasecmp(name,SPEED) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, SPEED, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxSpeed ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, SPEED, self->maxSpeed);
SCWrite(pCon, pError, eError);
return 1;
}
self->speed = newValue;
snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self, self->speed));
if (FAILURE == DMC2280Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
}
/* Set Acceleration */
if(strcasecmp(name,ACCEL) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, ACCEL, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxAccel ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, ACCEL, self->maxAccel);
SCWrite(pCon, pError, eError);
return 1;
}
self->accel = newValue;
snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel));
if (FAILURE == DMC2280Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
}
/* Set Deceleration */
if(strcasecmp(name,DECEL) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, DECEL, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxDecel ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, DECEL, self->maxDecel);
SCWrite(pCon, pError, eError);
return 1;
}
self->decel = newValue;
snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel));
if (FAILURE == DMC2280Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
}
return 0;
}
/** \brief List the motor parameters to the client.
* \param self (r) provides access to the motor's data structure
* \param *name (r) name of motor.
* \param *pCon (r) connection object.
*/
static void DMC2280StrList(pDMC2280Driv self, char *name, SConnection *pCon){
char buffer[BUFFLEN];
snprintf(buffer, BUFFLEN, "%s.part = %s\n", name, self->part);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, self->long_name);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, self->axisLabel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, self->units);
SCWrite(pCon, buffer, eStatus);
return;
}
/** \brief List the motor parameters to the client.
* \param self (r) provides access to the motor's data structure
* \param *name (r) name of motor.
* \param *pCon (r) connection object.
*/
static void DMC2280List(void *pData, char *name, SConnection *pCon){
pDMC2280Driv self = (pDMC2280Driv) pData;
char buffer[BUFFLEN];
snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->home);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, self->speed);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, self->maxSpeed);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, self->accel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, self->maxAccel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, self->decel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, self->maxDecel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, self->motOffDelay);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, self->debug);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, self->settle);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, self->blockage_ckInterval);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, self->blockage_thresh);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, self->blockage_ratio);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, self->blockage_fail);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, self->backlash_offset);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, self->has_airpads);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, self->abs_encoder);
SCWrite(pCon, buffer, eStatus);
if (self->abs_encoder) {
snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome);
SCWrite(pCon, buffer, eStatus);
if (self->has_fsm) {
snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Creep_Precision = %f\n", name, self->creep_precision);
SCWrite(pCon, buffer, eStatus);
}
}
snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX);
SCWrite(pCon, buffer, eStatus);
return;
}
static void DMC_Notify(void* context, int event)
{
pDMC2280Driv self = (pDMC2280Driv) context;
char line[132];
switch (event) {
case AQU_DISCONNECT:
snprintf(line, 132, "Disconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: disconnect */
break;
case AQU_RECONNECT:
snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: reconnect */
if (self->has_fsm) {
/* Reset the state machine */
if (self->state_timer)
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
change_state(self, DMCState_Unknown);
/* Schedule a timer event as soon as possible */
NetWatchRegisterTimer(&self->state_timer,
0,
state_tmr_callback, self);
}
break;
}
return;
}
/** \brief Free memory if motor is removed
* \param *pData (rw) provides access to the motor's data structure
*/
static void KillDMC2280(/*@only@*/void *pData){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
assert(self != NULL);
if (self->name) {
free(self->name);
self->name = NULL;
}
if (self->errorMsg) {
free(self->errorMsg);
self->errorMsg = NULL;
}
if (self->asyncUnit) {
AsyncUnitDestroy(self->asyncUnit);
self->asyncUnit = NULL;
}
/* Not required as performed in caller
* free(self);
*/
return;
}
/** \brief Create a driver for the DMC2280 Galil controller.
*
* This is called by the Motor configuration command in the
* SICS configuration file when you create a DMC2280 motor.
*
* Usage:\n
* Motor stth DMC2280 paramArray\n
* - stth is the motor name
* - DMC2280 is the motor type that will lead to calling this function.
* - paramArray is a Tcl array of the motor parameters.
*
* \param *pCon (r) connection object.
* \param *motor (r) motor name
* \param *params (r) configuration parameter array.
* \return a reference to Motordriver structure
*
* NOTES:\n
* -Adding parameters
* - Add a field for the parameter to the DMC2280Driv struct
* - Get the parameter from the parameter array, see PARAMETERS: below
* - If the parameter requires an abs enc then add it after ABSENC:
*/
MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
pDMC2280Driv pNew = NULL;
char *pPtr = NULL;
char buffer[132];
char pError[ERRLEN];
Tcl_Interp *interp;
buffer[0]='\0';
interp = InterpGetTcl(pServ->pSics);
/*
allocate and initialize data structure
*/
pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv));
if(NULL == pNew){
snprintf(pError, ERRLEN, "ERROR: no memory when creating DMC2280 motor '%s'", motor);
SCWrite(pCon, pError, eError);
return NULL;
}
memset(pNew, 0, sizeof(DMC2280Driv));
/* Get AsyncQueue from the list of named parameters */
if ((pPtr=getParam(pCon, interp, params, "multichan", _OPTIONAL)) != NULL ||
(pPtr=getParam(pCon, interp, params, "asyncqueue", _OPTIONAL)) != NULL ||
(pPtr=getParam(pCon, interp, params, "asyncunit", _OPTIONAL)) != NULL) {
if (!AsyncUnitCreate(pPtr, &pNew->asyncUnit)) {
snprintf(pError, ERRLEN, "Cannot find AsyncQueue '%s' when creating DMC2280 motor '%s'",
pPtr, motor);
SCWrite(pCon,pError,eError);
KillDMC2280(pNew);
free(pNew);
return NULL;
}
AsyncUnitSetNotify(pNew->asyncUnit, pNew, DMC_Notify);
}
else if ((pPtr=getParam(pCon, interp, params, "host", _OPTIONAL)) != NULL) {
char* host = pPtr;
if ((pPtr=getParam(pCon, interp, params,"port",_REQUIRED)) == NULL) {
snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
SCWrite(pCon,pError,eError);
KillDMC2280(pNew);
free(pNew);
return NULL;
}
/* AsyncUnit */
if (!AsyncUnitCreateHost(host, pPtr, &pNew->asyncUnit)) {
snprintf(pError, ERRLEN,
"Cannot create AsyncUnit '%s:%s' for DMC2280 motor '%s'",
host, pPtr, motor);
SCWrite(pCon,pError,eError);
KillDMC2280(pNew);
free(pNew);
return NULL;
}
AsyncUnitSetNotify(pNew->asyncUnit, pNew, DMC_Notify);
}
else {
snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
SCWrite(pCon,pError,eError);
KillDMC2280(pNew);
free(pNew);
return NULL;
}
pNew->name = (char *)malloc(sizeof(char)*(strlen(motor)+1));
if (pNew->name == NULL) {
(void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",
eError);
KillDMC2280(pNew);
free(pNew);
return NULL;
}
pNew->pMot = NULL;
strcpy(pNew->name, motor);
pNew->home = 0.0;
pNew->fLower = 0.0;//(float)atof(argv[2]);
pNew->fUpper = 0.0;//(float)atof(argv[3]);
pNew->GetPosition = DMC2280GetPos;
pNew->RunTo = DMC2280Run;
pNew->GetStatus = DMC2280Status;
pNew->GetError = DMC2280Error;
pNew->TryAndFixIt = DMC2280Fix;
pNew->Halt = DMC2280Halt;
pNew->GetDriverPar = DMC2280GetPar;
pNew->SetDriverPar = DMC2280SetPar;
pNew->ListDriverPar = DMC2280List;
pNew->KillPrivate = KillDMC2280;
pNew->GetDriverTextPar = NULL;
pNew->blockage_ckInterval = 0.5;
pNew->blockage_thresh = 0.5; /* minimum distance in units */
pNew->blockage_ratio = 2.0; /* maximum ratio observed/expected */
pNew->blockage_fail = 1; /* fail the motor */
pNew->backlash_offset = 0.0;
pNew->myState = DMCState_Unknown;
/* PARAMETERS: Fetch parameter values */
/* Debug: this motor driver logs exchanges */
if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL)
pNew->debug=0;
else {
sscanf(pPtr,"%d",&(pNew->debug));
}
/* FSM: this driver uses the finite state machine model */
if ((pPtr=getParam(pCon, interp, params,"fsm",_OPTIONAL)) == NULL)
pNew->has_fsm=1;
else {
sscanf(pPtr,"%d",&(pNew->has_fsm));
}
if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) {
strncpy(pNew->long_name, pPtr, sizeof(pNew->long_name));
pNew->long_name[sizeof(pNew->long_name) - 1] = '\0';
}
if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->fLower));
if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->fUpper));
if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%s",pNew->units);
if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->speed));
pNew->maxSpeed = pNew->speed;
if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->accel));
pNew->maxAccel = pNew->accel;
if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->decel));
pNew->maxDecel = pNew->decel;
if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%c",&(pNew->axisLabel));
if ((pPtr=getParam(pCon, interp, params,"stepsperx",_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->stepsPerX));
if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL)
pNew->motorHome=0;
else
sscanf(pPtr,"%d",&(pNew->motorHome));
if ((pPtr=getParam(pCon, interp, params,"nopowersave",_OPTIONAL)) == NULL)
pNew->noPowerSave=_SAVEPOWER;
else
sscanf(pPtr,"%d",&(pNew->noPowerSave));
if ((pPtr=getParam(pCon, interp, params,"motoffdelay",_OPTIONAL)) == NULL)
pNew->motOffDelay=0;
else
sscanf(pPtr,"%d",&(pNew->motOffDelay));
/* SETTLE: this motor need time to settle */
if ((pPtr=getParam(pCon, interp, params,"settle",_OPTIONAL)) == NULL)
pNew->settle=0;
else {
sscanf(pPtr,"%d",&(pNew->settle));
}
/* BACKLASH: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"backlash_offset",_OPTIONAL)) == NULL)
pNew->backlash_offset=0.0;
else {
sscanf(pPtr,"%f",&(pNew->backlash_offset));
}
/* AIRPADS: this motor need airpads */
if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL)
pNew->has_airpads=0;
else {
sscanf(pPtr,"%d",&(pNew->has_airpads));
}
/* ABSENC: If the parameter requires an abs enc add it to the else block */
if ((pPtr=getParam(pCon, interp, params,"absenc",_OPTIONAL)) == NULL)
pNew->abs_encoder = 0;
else {
sscanf(pPtr,"%d",&(pNew->abs_encoder));
if ((pPtr=getParam(pCon, interp, params,"absenchome",_REQUIRED)) == NULL)
pNew->absEncHome = 0;
else
sscanf(pPtr,"%d",&(pNew->absEncHome));
if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL)
pNew->cntsPerX = 1.0;
else
sscanf(pPtr,"%f",&(pNew->cntsPerX));
if (pNew->has_fsm) {
/* CREEP_OFFSET: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL)
pNew->creep_offset = 0.0;
else {
sscanf(pPtr, "%f", &(pNew->creep_offset));
if (pNew->creep_offset < 0)
pNew->creep_offset = -pNew->creep_offset;
}
/* CREEP_PRECISION: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_precision",_OPTIONAL)) == NULL)
pNew->creep_precision = 0.0;
else {
sscanf(pPtr, "%f", &(pNew->creep_precision));
if (pNew->creep_precision < 0)
pNew->creep_precision = -pNew->creep_precision;
}
}
}
if (pNew->has_fsm) {
/* Schedule a timer event as soon as possible */
NetWatchRegisterTimer(&pNew->state_timer,
0,
state_tmr_callback, pNew);
}
else {
char cmd[CMDLEN];
/* Set speed */
snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed));
if (FAILURE == DMC2280Send(pNew, cmd))
return NULL;
/* Set acceleration */
snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel));
if (FAILURE == DMC2280Send(pNew, cmd))
return NULL;
/* Set deceleration */
snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel));
if (FAILURE == DMC2280Send(pNew, cmd))
return NULL;
/* TODO Initialise current position and target
* to get a sensible initial list output
*/
}
return (MotorDriver *)pNew;
}
int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[])
{
pMotor pm = (pMotor) pData;
pDMC2280Driv self = (pDMC2280Driv) pm->pDriver;
if (argc > 1) {
if (strcasecmp("send", argv[1]) == 0) {
char cmd[CMDLEN];
char rsp[CMDLEN];
int idx = 0;
int i, j;
cmd[0] = '\0';
for (i = 2; i < argc; ++i) {
j = snprintf(&cmd[idx], CMDLEN - idx, "%s%s",
(i > 2) ? " " : "",
argv[i]);
if (j < 0)
break;
idx += j;
}
DMC2280SendReceive(self, cmd, rsp);
SCWrite(pCon, rsp, eValue);
return 1;
}
else if(strcasecmp("data", argv[1]) == 0) {
char line[132];
snprintf(line, 132, "%s.motor_step = %f %s", self->name,
1.0 / self->stepsPerX, self->units);
SCWrite(pCon, line, eValue);
if (self->abs_encoder) {
snprintf(line, 132, "%s.encoder_count = %f %s", self->name,
1.0 / self->cntsPerX, self->units);
SCWrite(pCon, line, eValue);
snprintf(line, 132, "%s.steps_per_count = %f steps", self->name,
self->stepsPerX / self->cntsPerX);
SCWrite(pCon, line, eValue);
}
return 1;
}
else if (strcasecmp("units", argv[1]) == 0) {
if (argc > 2) {
strncpy(self->units, argv[2], sizeof(self->units));
self->units[sizeof(self->units) - 1] = '\0';
}
else {
char line[132];
snprintf(line, 132, "%s.units = %s", self->name, self->units);
SCWrite(pCon, line, eValue);
}
return 1;
}
else if (strcasecmp("long_name", argv[1]) == 0) {
if (argc > 2) {
strncpy(self->long_name, argv[2], sizeof(self->long_name));
self->long_name[sizeof(self->long_name) - 1] = '\0';
}
else {
char line[132];
snprintf(line, 132, "%s.long_name = %s", self->name, self->long_name);
SCWrite(pCon, line, eValue);
}
return 1;
}
else if (strcasecmp("part", argv[1]) == 0) {
if (argc > 2) {
strncpy(self->part, argv[2], sizeof(self->part));
self->part[sizeof(self->part) - 1] = '\0';
}
else {
char line[132];
snprintf(line, 132, "%s.part = %s", self->name, self->part);
SCWrite(pCon, line, eValue);
}
return 1;
}
else if (strcasecmp("list", argv[1]) == 0) {
/* Handle in generic motor driver */
}
else if (strcasecmp("slist", argv[1]) == 0) {
DMC2280StrList(self, argv[0], pCon);
return 1;
}
else if(strcasecmp(SETPOS, argv[1]) == 0) {
float oldZero, newZero, currPos, newValue;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
MotorGetPar(self->pMot, "softzero", &oldZero);
if (argc > 3) {
sscanf(argv[2], "%f", &currPos);
currPos += oldZero;
sscanf(argv[3], "%f", &newValue);
}
else if (argc > 2 ){
sscanf(argv[2], "%f", &newValue);
currPos = self->pMot->fPosition;
}
else {
char buffer[BUFFLEN];
snprintf(buffer, BUFFLEN, "%s.setPos = %f\n", self->name, oldZero);
SCWrite(pCon, buffer, eStatus);
return 1;
}
newZero = (currPos - newValue);
return MotorSetPar(self->pMot, pCon, "softzero", newZero);
}
else if(strcasecmp("reset", argv[1]) == 0) {
if (self->has_fsm) {
/* Reset the state machine */
if (self->state_timer)
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
change_state(self, DMCState_Unknown);
/* Schedule a timer event as soon as possible */
NetWatchRegisterTimer(&self->state_timer,
0,
state_tmr_callback, self);
}
/* Handle further in generic motor driver */
return MotorAction(pCon, pSics, pData, argc, argv);
}
else if(self->has_fsm && strcasecmp("state", argv[1]) == 0) {
char line[132];
snprintf(line, 132, "%s.state = %s (timer=%s)",
self->name, state_name(self->myState),
self->state_timer ? "active" : "inactive");
SCWrite(pCon, line, eValue);
return 1;
}
else if(self->has_fsm && strcasecmp("trace", argv[1]) == 0) {
if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
self->trace = pCon;
SCWrite(pCon, "TRACE ON", eValue);
}
else {
self->trace = 0;
SCWrite(pCon, "TRACE OFF", eValue);
}
return 1;
}
}
return MotorAction(pCon, pSics, pData, argc, argv);
}
void DMC2280InitProtocol(SicsInterp *pSics) {
if (DMC2280_Protocol == NULL) {
DMC2280_Protocol = AsyncProtocolCreate(pSics, "DMC2280", NULL, NULL);
DMC2280_Protocol->sendCommand = DMC_Tx;
DMC2280_Protocol->handleInput = DMC_Rx;
DMC2280_Protocol->handleEvent = DMC_Ev;
DMC2280_Protocol->prepareTxn = NULL;
DMC2280_Protocol->killPrivate = NULL;
}
}
Reference in New Issue View Git Blame Copy Permalink