linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
66caf6916bb6ad2c68505677e0ff2cd34ba6ee16
sics/site_ansto/motor_dmc2280.c
Douglas Clowes 66caf6916b Don't change anything on startup, defer settings until moving and then do each time 
We don't want to change anything on the controller when we start SICS so we don't interfere with whatever else is talking to it. So don't command it during init.

We also want to send Speed, Accel and Decel on each move so we defer those until then.
r3622 | dcl | 2012-06-28 15:39:47 +1000 (Thu, 28 Jun 2012) | 5 lines
2012-11-15 17:31:44 +11:00

4387 lines
132 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 <ctype.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"
#include "action.h"
#define UNITSLEN 256
#define TEXTPARLEN 1024
#define CMDLEN 1024
#define STATE_TRACE (200)
#define DECADIC_CREEP (10)
#define SPEED_ON_MOVE 1
/** \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 {
eStateEvent,
eTimerEvent,
eMessageEvent,
eCommandEvent,
eTimeoutEvent,
eTimerSet, /* pseudo-event for state trace only */
eTimerClear /* pseudo-event for state trace only */
};
typedef struct EvtEvent_s EvtEvent, *pEvtEvent;
typedef void (*StateFunc)(pDMC2280Driv self, pEvtEvent event);
typedef struct EvtState_s { } EvtState;
typedef struct EvtTimer_s {
int timerValue;
} EvtTimer;
typedef struct EvtMessage_s {
pAsyncTxn cmd;
} EvtMessage;
typedef struct EvtCommand_s {
enum commandtype cmd_type;
} EvtCommand;
typedef struct EvtTimeout_s { } EvtTimeout;
typedef struct TimerClear_s { /* pseudo-event for state trace only */
int timerValue;
} TimerClear;
typedef struct TimerSet_s { /* pseudo-event for state trace only */
int timerValue;
} TimerSet;
struct EvtEvent_s {
enum eventtype event_type;
union {
EvtState sta;
EvtTimer tmr;
EvtMessage msg;
EvtCommand cmd;
EvtTimeout tmo;
TimerSet tms;
TimerClear tmc;
} event;
};
#define VAR_REQ (1<<0)
#define VAR_RSP (1<<1)
#define VAR_RUN (1<<2)
#define VAR_DST (1<<3)
#define VAR_POS (1<<4)
#define VAR_HLT (1<<5)
#define VAR_ENC (1<<6)
#define VAR_SWI (1<<7)
#define VAR_HOM (1<<8)
#define VAR_STP (1<<9)
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 encoderAxis;
char lastCmd[CMDLEN];
char dmc2280Error[CMDLEN];
double fHome; /**< home position for axis, default=0 */
int motorHome; /**< motor home position in steps */
int noPowerSave; /**< Flag = 1 to leave motors on after a move */
double 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 */
double cntsPerX; /**< absolute encoder counts per physical unit */
int motOnDelay; /**< msec to wait after switching motor on */
int motOffDelay; /**< msec to wait before switching motor off */
int currFlags;
int currSteps;
int currCounts;
double currPosition; /**< Position at last position check */
double lastPosition; /**< Position at last position check */
int lastSteps;
int lastCounts;
double origPosition; /**< Position at last position check */
int origSteps;
int origCounts;
double minRatio;
double maxRatio;
int thread0; /**< last read of _XQ0 */
unsigned short int stopCode; /**< last read of _SCx */
unsigned short int inputByte; /**< last read of _TIx */
bool ampError; /**< amplifier error */
bool runError; /**< motor error */
bool threadError; /**< thread error */
bool moving; /**< true if moving */
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 protocol; /**< protocol version 0..3 */
float backlash_offset; /**< signed offset to drive from */
double fTarget; /**< target passed from SICS to timer callback */
double fPreseek; /**< preseek target when preseek is active */
int settle; /**< motor settling time in seconds */
struct timeval time_settle_done; /**< time when settling will be over */
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; /**< how far away we can stop creeping */
int creep_val;
int preseek; /**< Flag = 1 if current move is a backlash preseek */
int run_flag; /**< Flag = 1 if RUN command has been issued and deferred */
/**< Flag = -1 if HALT command has been issued and deferred */
int driver_status;
bool faultPending; /**< set when waiting for a failing motor to stop */
int timerValue; /**< save for debug printing */
StateFunc myState; /**< pointer to state action method */
StateFunc myPrevState; /**< save for debug printing */
int subState; /**< tracks substate within state method */
bool waitResponse; /**< true is a message sent and we wait for response */
pNWTimer state_timer; /**< motor state timer */
SConnection *trace;
int posit_count;
double* positions;
int variables;
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
int state_trace_idx;
char state_trace_text[STATE_TRACE][CMDLEN];
struct timeval state_trace_time[STATE_TRACE];
#endif
int bias_bits; /**< number of bits to mask */
int bias_bias; /**< bias to add to encoder value */
char ao_id[256];
};
int DMC2280MotionControl = 1; /* defaults to enabled */
static bool trace_switches = false;
#define AIR_POLL_TIMER 1000
#define MOTOR_POLL_FAST 100
#define MOTOR_POLL_SLOW 500
#define DEFAULT_DELAY_MOTOR_ON 200 /* msec to wait after switching motor on */
#define DEFAULT_DELAY_MOTOR_OFF 0 /* msec to wait before switching motor off */
#define MAX_CREEP_STEPS 100
#define MAX_RESTARTS 0
/*------------------- error codes ----------------------------------*/
#define BADADR -1 // NOT SET: Unknown host/port?
#define BADBSY -2
#define BADCMD -3
#define BADPAR -4 // 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 AMPERROR -22 /* Amplifier OK not asserted */
#define RUNERROR -23 /* Error from RUN variable */
/*--------------------------------------------------------------------*/
#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 MOTONDELAY "motondelay"
#define MOTOFFDELAY "motoffdelay"
#define AIRPADS "airpads"
#define SETTLE "settle"
#define LONG_NAME "long_name"
#define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval"
/* State Machine Events */
static int state_snd_callback(pAsyncTxn pCmd);
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 void report_event(pDMC2280Driv self, pEvtEvent event);
static void state_trace_prn(pDMC2280Driv self);
static char* state_name(StateFunc func);
static int DMC2280Halt(void *pData);
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue);
static bool check_positions(pDMC2280Driv self) {
char line[CMDLEN];
int missing = 0;
int direction = 0;
int i;
if (self->posit_count < 2) {
snprintf(line, ERRLEN, "Insufficient positions on motor '%s': %d",
self->name, self->posit_count);
SICSLogWrite(line,eError);
return false;
}
for (i = 0; i < self->posit_count; ++i)
if (self->positions[i] < -9999999)
++missing;
if (missing) {
snprintf(line, ERRLEN, "Missing positions on motor '%s': %d",
self->name, missing);
SICSLogWrite(line,eError);
return false;
}
for (i = 1; i < self->posit_count; ++i) {
if (i == 1) {
if (self->positions[i] > self->positions[i - 1])
direction = 1;
else if (self->positions[i] < self->positions[i - 1])
direction = -1;
else
direction = 0;
}
if (direction == 0) {
snprintf(line, ERRLEN, "Position order on motor '%s' : %d",
self->name, i);
SICSLogWrite(line,eError);
}
else {
switch (direction) {
case -1:
if (!(self->positions[i] < self->positions[i - 1])) {
direction = 0;
}
break;
case 1:
if (!(self->positions[i] > self->positions[i - 1])) {
direction = 0;
}
break;
}
}
}
if (direction != 0)
return true;
for (i = 0; i < self->posit_count; ++i) {
snprintf(line, ERRLEN, "Position %2d motor '%s' : %f",
i + 1,
self->name,
self->positions[i]);
/* TODO log me */
}
return false;
}
static long long unit2count(pDMC2280Driv self, double target) {
double absolute;
long long result;
if (self->abs_encoder == 0) {
char line[CMDLEN];
snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder",
self->name);
SICSLogWrite(line, eStatus);
return -1;
}
/* distance of target from home in units */
absolute = (double) target - self->fHome;
/* convert to encoder counts */
absolute *= (double) self->cntsPerX;
/* add home position in counts */
absolute += (double) self->absEncHome;
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, "unit2count Rounding %f to %lld", absolute, result);
SICSLogWrite(line, eStatus);
}
return result;
}
static double count2unit(pDMC2280Driv self, long long counts) {
double fPos;
if (self->abs_encoder == 0) {
char line[CMDLEN];
snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder",
self->name);
SICSLogWrite(line, eStatus);
return -1;
}
fPos = (counts - self->absEncHome) / self->cntsPerX + self->fHome;
return fPos;
}
static double unit2posit(pDMC2280Driv self, double target) {
int i, j;
double fPos;
if (!check_positions(self))
return 0;
if (self->positions[0] < self->positions[1]) {
for (i = 1; i < self->posit_count - 1; ++i) {
if (target < self->positions[i]) {
break;
}
}
}
else {
for (i = 1; i < self->posit_count - 1; ++i) {
if (target > self->positions[i]) {
break;
}
}
}
--i;
j = i + 1;
fPos = (double) j +
((double)target - self->positions[i]) /
((double)self->positions[j] - self->positions[i]);
return fPos;
}
static double posit2unit(pDMC2280Driv self, double target) {
double result;
int i = ((int) target) - 1;
if (i < 0) {
i = 0;
} else if (i > self->posit_count - 2) {
i = self->posit_count - 2;
}
result = (double)self->positions[i]
+ ((double)target - ((double)i + 1)) *
((double)self->positions[i + 1] - self->positions[i]);
return result;
}
static long long posit2count(pDMC2280Driv self, double target) {
return unit2count(self, posit2unit(self, target));
}
static double count2posit(pDMC2280Driv self, long long counts) {
return unit2posit(self, count2unit(self, counts));
}
/** \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, double axisSpeed) {
int speed;
speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5);
return speed;
}
/** \brief Convert axis acceleration in physical units to
* motor acceleration 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, double 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, double 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 double motPosit(pDMC2280Driv self) {
double fPos, curr, home, counts;
if (self->abs_encoder) {
curr = self->currCounts;
home = self->absEncHome;
counts = self->cntsPerX;
} else {
curr = self->currSteps;
home = self->motorHome;
counts = self->stepsPerX;
}
fPos = (curr - home) / counts + self->fHome;
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, double target) {
double absolute;
int result;
if (self->abs_encoder) {
/* distance of target from home in units */
absolute = target - self->fHome;
/* 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 = (target - self->fHome) * 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, "motAbsol 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 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, double 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->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);
}
if (self->creep_val > 0) /* moving down handle as positive */
offset = -offset;
if (offset > fabs(self->stepsPerX * self->creep_precision)) {
#ifdef DECADIC_CREEP
/* if half offset is more than creep_offset warp to creep_offset */
if (offset > (int) (2.0 * fabs(self->stepsPerX * self->creep_offset)))
offset = offset - fabs(self->stepsPerX * self->creep_offset);
else {
/* if closer than one count, "single step" else go half way */
if (offset <= fabs(self->stepsPerX / self->cntsPerX)) {
offset = offset / DECADIC_CREEP;
if (offset < 1)
offset = 1;
}
else
offset = offset / 2;
}
#else
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;
}
#endif
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_SendReq(pDMC2280Driv self, char* command) {
self->waitResponse = true;
return AsyncUnitSendTxn(self->asyncUnit,
command, strlen(command),
state_msg_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 ':': /* prompt */
case ' ': /* leading blank */
case '-': /* leading minus sign */
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;
}
/** \brief Sends a DMC2280 command to the motor controller.
*
* This function does not expect or care about the response.
*
* If the command fails it writes the DMC2280 error message 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 DMC_Send(pDMC2280Driv self, char *command) {
return AsyncUnitSendTxn(self->asyncUnit,
command, strlen(command),
state_snd_callback, self, CMDLEN);
}
/**
* \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 DMC_SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
int status;
int cmd_len = CMDLEN;
status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len);
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;
}
/*
* Clear the timer if it is set
*/
static void DMC_ClearTimer(pDMC2280Driv self) {
/* only if the timer is really set */
if (self->state_timer) {
/* log this event in the trace buffer */
EvtEvent event;
event.event_type = eTimerClear;
event.event.tmc.timerValue = self->timerValue;
report_event(self, &event);
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
}
}
static void DMC_SetTimer(pDMC2280Driv self, int msecs) {
EvtEvent event;
/* if the timer is already set, log the logic error and clear it */
if (self->state_timer != 0) {
char line[133];
snprintf(line, 132, "Motor %s, state %s(%d): setting new timer (%d mSec) when old (%d mSec) active, clearing",
self->name,
state_name(self->myState),
self->subState,
msecs,
self->timerValue);
SICSLogWrite(line, eError);
DMC_ClearTimer(self);
state_trace_prn(self);
}
/* log this event in the trace buffer */
event.event_type = eTimerSet;
event.event.tms.timerValue = msecs;
report_event(self, &event);
self->timerValue = msecs;
NetWatchRegisterTimer(&self->state_timer, msecs, state_tmr_callback, self);
}
/** \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, int steps, int counts) {
assert(self != NULL);
self->lastSteps = steps;
self->lastCounts = counts;
gettimeofday(&(self->time_lastPos_set), NULL);
}
static void report_motion(pDMC2280Driv self) {
SConnection *pDumCon;
MotCallback sCall;
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);
}
/** \brief Reads absolute encoder.
*
* \param *pos is the absolute encoder reading on SUCCESS.
* \return
* SUCCESS
* FAILURE
*/
static int readAbsEnc(pDMC2280Driv self, long *pos) {
char reply[CMDLEN];
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "TP%c", self->encoderAxis ? self->encoderAxis : self->axisLabel);
if (FAILURE == DMC_SendReceive(self, cmd, reply))
return FAILURE;
long iCounts = atol(reply);
if (self->bias_bits > 0)
iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1);
*pos = iCounts;
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 == DMC_SendReceive(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 == DMC_SendReceive(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 == DMC_Send(self, cmd))
return FAILURE;
return SUCCESS;
}
snprintf(cmd, CMDLEN, "XQ #HOME,7");
if (FAILURE == DMC_Send(self, cmd))
return FAILURE;
return SUCCESS;
}
/** \brief Check if the axis is still within all the limits
*
* The position is checked against upper and lower limits
*
* \param *pData provides access to a motor's data
* \return
* - 1 MOTOR OK, position is within the specified limits
* - 0 MOTOR BAD, position is not within hard and soft limits
*/
static int checkPosition(pDMC2280Driv self) {
float fTarget;
float fPrecision;
bool moving_forward = false;
MotorGetPar(self->pMot, "precision", &fPrecision);
if (self->preseek)
fTarget = self->fPreseek;
else
fTarget = self->fTarget;
if (fTarget > self->lastPosition)
moving_forward = true;
else
moving_forward = false;
if (moving_forward) {
if (self->currPosition > self->fUpper + fPrecision)
return 0;
if (self->currPosition > fTarget + fPrecision)
return 0;
}
else {
if (self->currPosition < self->fLower - fPrecision)
return 0;
if (self->currPosition < fTarget - fPrecision)
return 0;
}
return 1;
}
/** \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(pDMC2280Driv self) {
int iRet = 1;
double ratio_obs, ratio_exp, ratio_cmp;
long int usec_TimeDiff;
struct timeval now;
bool bNotYet = false; /* set true if too soon */
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 */
set_lastMotion(self, self->currSteps, self->currCounts);
bNotYet = true;
}
/*
* calculate the time since the last check
*/
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 too soon, say not yet
*/
if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
bNotYet = true;
/*
* calculate the distance since the last check
* If too short, say not yet
*/
#if 0
if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
/* just update the timestamp */
set_lastMotion(self, self->lastSteps, self->lastCounts);
bNotYet = true;
}
#else
/* TODO: make relative to lastPosition */
if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
/* just update the timestamp */
set_lastMotion(self, self->lastSteps, self->lastCounts);
bNotYet = true;
}
#endif
/* calculate observed and expected steps per count ratios */
if (self->currCounts == self->lastCounts) /* prevent divide by zero */
ratio_obs = (self->currSteps - self->lastSteps);
else {
ratio_obs = (double) (self->currSteps - self->lastSteps);
ratio_obs /= (double) (self->currCounts - self->lastCounts);
}
ratio_exp = (double) self->stepsPerX / (double) self->cntsPerX;
ratio_cmp = ratio_obs / ratio_exp;
if (self->minRatio == 0.0 || self->minRatio > ratio_cmp)
self->minRatio = ratio_cmp;
if (self->maxRatio == 0.0 || self->maxRatio < ratio_cmp)
self->maxRatio = ratio_cmp;
/* 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];
double cmp = ratio_cmp;
if (fabs(cmp) > -1.0 && fabs(cmp) < 1.0)
cmp = 1.0 / cmp;
if (self->debug || bNotYet == false) {
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=%d-%d, counts=%d-%d, exp=%f/%f",
self->currSteps, self->lastSteps,
self->currCounts, self->lastCounts,
self->stepsPerX, self->cntsPerX);
SICSLogWrite(msg, eError);
}
if (bNotYet == false) {
if (self->blockage_fail)
iRet = 0;
else
set_lastMotion(self, self->currSteps, self->currCounts);
}
} else {
if (self->debug) {
char msg[132];
double 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);
snprintf(msg, sizeof(msg), "steps=%d-%d, counts=%d-%d, exp=%f/%f",
self->currSteps, self->lastSteps,
self->currCounts, self->lastCounts,
self->stepsPerX, self->cntsPerX);
SICSLogWrite(msg, eError);
}
if (bNotYet == false) {
set_lastMotion(self, self->currSteps, self->currCounts);
}
}
return iRet;
}
/**
* \brief test if there is a variable of the form VVVx where VVV is the
* variable name and x is the axis label.
*
* The variable name is contstructed by appending the axis label to the given
* name and looking for the construct "VVVx=" at the start of a line in the
* variable list. This will reject finds which are just the last part of a
* variable like "ZVVV="
*
* \param self motor data
* \param vars string returned from LV command
* \param VVV part of variable name
*
* \return true if found else false
*/
static bool has_var(pDMC2280Driv self, const char* vars, const char* name) {
char* p;
char tmp[20];
snprintf(tmp, 20, "%s=", name);
p = strstr(vars, tmp);
if (p == NULL) /* not found at all */
return false;
if (p == vars) /* found at start of first line */
return true;
if (p[-1] == '\n') /* found at start of another line */
return true;
return false; /* found partial (ZVVVx=) only */
}
static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
char tmp[20];
snprintf(tmp, 20, "%s%c", name, self->axisLabel);
return has_var(self, vars, tmp);
}
/**
* \brief request standard state information for the motor and controller
*
* \param self access to the drive data
*/
static int cmdStatus(pDMC2280Driv self) {
char cmd[CMDLEN];
char encoder = self->axisLabel;
if (self->encoderAxis && !(self->variables & VAR_ENC))
encoder = self->encoderAxis;
/* Use POSx, ENCx, RUNx, SWIx if it has these variables */
snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,%s%c,_TI0,_TI1,_XQ0",
(self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel,
(self->variables & VAR_ENC) ? "ENC" : "_TP", encoder,
(self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel,
(self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel,
(self->variables & VAR_STP) ? "STP" : "_SC", self->axisLabel);
return DMC_SendReq(self, cmd);
}
static int cmdVars(pDMC2280Driv self) {
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "LV");
return DMC_SendReq(self, cmd);
}
static int cmdOn(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->protocol == 1) {
snprintf(cmd, CMDLEN, "FTUBE=1");
}
else if (self->protocol == 2 || self->protocol == 3) {
snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel);
}
else {
snprintf(cmd, CMDLEN, "SH%c", self->axisLabel);
}
return DMC_SendReq(self, cmd);
}
static int cmdPosition(pDMC2280Driv self, int target) {
char cmd[CMDLEN];
self->lastPosition = motPosit(self);
if (self->protocol == 3)
snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target);
else
snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, target);
return DMC_SendReq(self, cmd);
}
static int cmdBegin(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->protocol == 3)
snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel);
else
snprintf(cmd, CMDLEN, "BG%c", self->axisLabel);
return DMC_SendReq(self, cmd);
}
static int cmdPoll(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->protocol == 2 || self->protocol == 3)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else if (self->protocol == 1)
snprintf(cmd, CMDLEN, "MG APDONE");
return DMC_SendReq(self, cmd);
}
static void cmdHalt(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->protocol == 3) {
snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel);
/* Note that this does not expect a reply or confirmation */
(void) DMC_Send(self, cmd);
} else {
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
(void) DMC_Send(self, cmd);
if (self->variables & VAR_HLT) {
snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel);
/* Note that this does not expect a reply or confirmation */
(void) DMC_Send(self, cmd);
}
}
}
static int cmdOff(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->protocol == 1) {
snprintf(cmd, CMDLEN, "FTUBE=0");
}
else if (self->protocol == 2 || self->protocol == 3) {
snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
}
else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
}
return DMC_SendReq(self, cmd);
}
static int cmdSpeed(pDMC2280Driv self) {
char cmd[CMDLEN];
int value;
value = motSpeed(self, self->speed);
snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
return 0; /* FIXME should signal a HWFault */
}
return 1;
}
static int cmdAccel(pDMC2280Driv self) {
char cmd[CMDLEN];
int value;
value = motAccel(self, self->accel);
snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
return 0; /* FIXME should signal a HWFault */
}
return 1;
}
static int cmdDecel(pDMC2280Driv self) {
char cmd[CMDLEN];
int value;
value = motDecel(self, self->decel);
snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
return 0; /* FIXME should signal a HWFault */
}
return 1;
}
static int rspStatus(pDMC2280Driv self, const char* text) {
int iRet, iFlags;
int iSteps, iCounts;
int iTIzero, iTIone, iIOByte, iStopCode, iXQ0, iBG;
iRet = sscanf(text, "%d %d %d %d %d %d %d %d",
&iSteps, &iCounts, &iFlags, &iBG,
&iStopCode, &iTIzero, &iTIone, &iXQ0);
if (iRet != 8)
return 0;
/* TODO Put the following in specialised response handlers */
if (self->ao_id[0] != '\0') {
AO_istatus(iTIone, "QKK:TI1");
}
if (self->axisLabel >= 'A' && self->axisLabel <= 'D')
iIOByte = iTIzero;
else
iIOByte = iTIone;
if ((trace_switches || self->debug) && self->currFlags != iFlags) {
char line[CMDLEN];
char *sw;
if ((self->currFlags & (8 + 4)) != (iFlags & (8 + 4))) {
switch (iFlags & (8 + 4)) {
case (8 + 4): sw = "none"; break;
case (8 ): sw = "reverse"; break;
case ( 4): sw = "forward"; break;
case ( 0 ): sw = "both"; break;
}
snprintf(line, CMDLEN, "Motor %s limits: %s", self->name, sw);
if (trace_switches)
ServerWriteGlobal(line, eStatus);
else
SICSLogWrite(line, eStatus);
}
if ((self->currFlags & (32)) != (iFlags & (32))) {
if (iFlags & (32)) {
sw = "off";
}
else
{
sw = "on";
}
snprintf(line, CMDLEN, "Motor %s motor: %s", self->name, sw);
if (trace_switches)
ServerWriteGlobal(line, eStatus);
else
SICSLogWrite(line, eStatus);
}
if ((self->currFlags & (128)) != (iFlags & (128))) {
if (iFlags & (128)) {
sw = "moving";
}
else
{
sw = "stopped";
}
snprintf(line, CMDLEN, "Motor %s motor: %s", self->name, sw);
if (trace_switches)
ServerWriteGlobal(line, eStatus);
else
SICSLogWrite(line, eStatus);
}
}
self->currFlags = iFlags;
self->currSteps = iSteps;
if (self->bias_bits > 0)
iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1);
self->currCounts = iCounts;
self->currPosition = motPosit(self);
self->thread0 = iXQ0;
self->stopCode = iStopCode;
self->inputByte = iIOByte;
#ifdef AMPERROR
if (self->variables & VAR_RUN)
self->ampError = 0;
else if (self->axisLabel >= 'A' && self->axisLabel <= 'D')
self->ampError = !(1 & (iIOByte >> (self->axisLabel - 'A')));
else
self->ampError = !(1 & (iIOByte >> (self->axisLabel - 'E')));
#endif
self->runError = iBG < 0 ? iBG : 0;
if (iBG < 0)
snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER RUN ERROR: %d", iBG);
self->threadError = self->thread0 < 0;
self->moving = iBG > 0;
if (self->protocol == 3 && !(self->variables & VAR_STP)) {
if (self->moving)
self->stopCode = 0;
else
self->stopCode = 1;
}
return 1;
}
static int rspVars(pDMC2280Driv self, const char* text) {
self->variables = 0;
if (has_var(self, text, "HOMERUN"))
self->variables |= VAR_HOM;
if (has_var_x(self, text, "REQ"))
self->variables |= VAR_REQ;
if (has_var_x(self, text, "RSP"))
self->variables |= VAR_RSP;
if (has_var_x(self, text, "RUN"))
self->variables |= VAR_RUN;
if (has_var_x(self, text, "DST"))
self->variables |= VAR_DST;
if (has_var_x(self, text, "POS"))
self->variables |= VAR_POS;
if (has_var_x(self, text, "HLT"))
self->variables |= VAR_HLT;
if (has_var_x(self, text, "ENC"))
self->variables |= VAR_ENC;
if (has_var_x(self, text, "SWI"))
self->variables |= VAR_SWI;
if (has_var_x(self, text, "STP"))
self->variables |= VAR_STP;
if ((self->variables & VAR_REQ) &&
(self->variables & VAR_RSP)) {
self->protocol = 2;
if ((self->variables & VAR_RUN) &&
(self->variables & VAR_DST) &&
(self->variables & VAR_POS)) {
self->protocol = 3;
}
}
return 1;
}
static int rspPoll(pDMC2280Driv self, const char* text) {
int iReply = atoi(text);
if (iReply < 0)
snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER REQ ERROR: %d",
iReply);
return iReply;
}
static void state_trace_prn(pDMC2280Driv self) {
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
int idx = self->state_trace_idx;
SICSLogWrite("state_trace_prn trace listing start", eStatus);
do {
char* line;
struct timeval *tp;
tp = &self->state_trace_time[self->state_trace_idx];
line = &self->state_trace_text[self->state_trace_idx++][0];
if (self->state_trace_idx >= STATE_TRACE)
self->state_trace_idx = 0;
if (*line)
SICSLogWriteTime(line, eStatus, tp);
} while (idx != self->state_trace_idx);
SICSLogWrite("state_trace_prn trace listing end", eStatus);
#else
SICSLogWrite("state_trace_prn not implemented", eStatus);
#endif
}
/* State Functions */
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorStart(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_MotorStop(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Error(pDMC2280Driv self, pEvtEvent event);
static char* state_name(StateFunc func) {
if (func == DMCState_Unknown) return "DMCState_Unknown";
if (func == DMCState_Idle) return "DMCState_Idle";
if (func == DMCState_MotorStart) return "DMCState_MotorStart";
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_MotorStop) return "DMCState_MotorStop";
if (func == DMCState_Error) return "DMCState_Error";
return "<unknown_state>";
}
static 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 eStateEvent:
snprintf(text, length, "eStateEvent");
return text;
case eTimerEvent:
snprintf(text, length, "eTimerEvent (%d mSec)", event->event.tmr.timerValue);
return text;
case eTimerSet:
snprintf(text, length, "eTimerSet (%d mSec)", event->event.tms.timerValue);
return text;
case eTimerClear:
snprintf(text, length, "eTimerClear (%d mSec)", event->event.tmc.timerValue);
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 defined(STATE_TRACE) && (STATE_TRACE > 0)
if (true) {
char* line;
gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL);
line = &self->state_trace_text[self->state_trace_idx++][0];
if (self->state_trace_idx >= STATE_TRACE)
self->state_trace_idx = 0;
#else
if (self->debug || self->trace) {
char line[CMDLEN];
#endif
char text[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), event=%s",
self->name,
state_name(self->myState),
self->subState,
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 defined(STATE_TRACE) && (STATE_TRACE > 0)
if (true) {
char* line;
gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL);
line = &self->state_trace_text[self->state_trace_idx++][0];
if (self->state_trace_idx >= STATE_TRACE)
self->state_trace_idx = 0;
#else
if (self->debug || self->trace) {
char line[CMDLEN];
#endif
snprintf(line, CMDLEN, "Motor=%s, OldState=%s(%d), NewState=%s",
self->name,
state_name(self->myState),
self->subState,
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;
EvtEvent evt;
evt.event_type = eStateEvent;
self->myState(self, &evt);
}
static void unhandled_event(pDMC2280Driv self, pEvtEvent event) {
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
char* line;
gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL);
line = &self->state_trace_text[self->state_trace_idx++][0];
if (self->state_trace_idx >= STATE_TRACE)
self->state_trace_idx = 0;
#else
char line[CMDLEN];
#endif
char text[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), unhandled event=%s",
self->name,
state_name(self->myState),
self->subState,
event_name(event, text, CMDLEN));
SICSLogWrite(line, eError);
}
static void handle_event(pDMC2280Driv self, pEvtEvent event) {
StateFunc oldState;
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
#else
if (self->debug || self->trace)
#endif
report_event(self, event);
oldState = self->myState;
self->myState(self, event);
}
static int state_snd_callback(pAsyncTxn pCmd) {
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
SendCallback(pCmd);
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
char* line;
char text[CMDLEN];
gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL);
line = &self->state_trace_text[self->state_trace_idx++][0];
if (self->state_trace_idx >= STATE_TRACE)
self->state_trace_idx = 0;
#else
char line[CMDLEN];
char text[CMDLEN];
#endif
text[0] = '\0';
str_n_cat(text, CMDLEN, pCmd->out_buf);
str_n_cat(text, CMDLEN, "|");
str_n_cat(text, CMDLEN, pCmd->inp_buf);
snprintf(line, CMDLEN, "Motor=%s, State=%s(%d), send complete=%s",
self->name,
state_name(self->myState),
self->subState,
text);
if (self->debug)
SICSLogWrite(line, eStatus);
if (self->trace)
SCWrite(self->trace, line, eStatus);
return 0;
}
static int state_msg_callback(pAsyncTxn pCmd) {
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
EvtEvent event;
SendCallback(pCmd);
if (pCmd->txn_status == ATX_TIMEOUT) {
event.event_type = eTimeoutEvent;
event.event.msg.cmd = pCmd;
}
else {
event.event_type = eMessageEvent;
event.event.msg.cmd = pCmd;
}
self->waitResponse = false;
handle_event(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;
event.event.tmr.timerValue = self->timerValue;
handle_event(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;
handle_event(self, &event);
return 0;
}
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
int value;
switch (event->event_type) {
case eStateEvent:
self->run_flag = 0;
self->driver_status = HWBusy;
self->errorCode = 0;
/* handle pending message event */
if (self->waitResponse) {
self->subState = 0;
/* FIXME: FIXME_DOUG */
return;
}
#ifdef SPEED_ON_MOVE
/* Defer Speed, Accel and Decel to move command */
#else
/* Set speed */
value = motSpeed(self, self->speed);
snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
break;
}
/* Set acceleration */
value = motAccel(self, self->accel);
snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
break;
}
/* Set deceleration */
value = motDecel(self, self->decel);
snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
break;
}
#endif
cmdVars(self);
self->subState = 1;
return;
case eTimerEvent:
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
if (self->pMot) {
self->pMot->fTarget = self->pMot->fPosition = motPosit(self);
change_state(self, DMCState_Idle);
return;
}
DMC_SetTimer(self, MOTOR_POLL_SLOW);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 0) { /* waitResponse */
change_state(self, DMCState_Unknown);
return;
}
if (self->subState == 1) { /* Vars */
rspVars(self, pCmd->inp_buf);
cmdStatus(self);
self->subState = 2;
return;
}
if (self->subState == 2) { /* Status Request */
int iRet;
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
set_lastMotion(self, self->currSteps, self->currCounts);
DMC_SetTimer(self, 0);
self->subState = 0;
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
self->driver_status = HWBusy;
self->run_flag = 1;
return;
case CMD_HALT:
/* handle halt command */
self->run_flag = -1;
return;
}
break;
case eTimeoutEvent:
/* handle timeout */
change_state(self, DMCState_Unknown);
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
return;
}
static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
DMC_ClearTimer(self);
if (self->driver_status == HWBusy)
self->driver_status = HWIdle;
#ifdef RESET_RUN_ON_IDLE
if (self->variables & VAR_RUN) {
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel);
(void) DMC_Send(self, cmd);
}
#endif
DMC_SetTimer(self, 0);
self->subState = 0;
return;
case eTimerEvent:
cmdStatus(self);
self->subState = 1;
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* Status Response */
int iRet;
long lDelta;
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
/* action deferred RUN command */
if (self->run_flag == 1) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
/* if the motor moved, update any observers */
if (self->abs_encoder)
lDelta = fabs(self->currCounts - self->lastCounts);
else
lDelta = fabs(self->currSteps - self->lastSteps);
if (lDelta > 10) {
set_lastMotion(self, self->currSteps, self->currCounts);
report_motion(self);
}
if (trace_switches || self->debug)
DMC_SetTimer(self, MOTOR_POLL_FAST);
else
DMC_SetTimer(self, MOTOR_POLL_SLOW);
self->subState = 0;
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
self->driver_status = HWBusy;
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
}
else {
self->run_flag = 1;
}
return;
case CMD_HALT:
/* we are already halted so just reset run_flag*/
self->run_flag = 0;
return;
}
break;
case eTimeoutEvent:
/* handle message timeout */
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
DMC_SetTimer(self, MOTOR_POLL_SLOW);
self->subState = 0;
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
return;
}
static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
self->run_flag = 0;
DMC_ClearTimer(self);
/* 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;
}
else if (self->threadError) {
self->errorCode = THREADZERO;
self->driver_status = HWFault;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->driver_status = HWFault;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
self->driver_status = HWFault;
}
#endif
else {
int iFlags;
int fwd_limit_active, rvrs_limit_active, errorlimit;
/* Handle limit switches from latest status response */
iFlags = self->currFlags;
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) {
change_state(self, DMCState_Idle);
return;
}
cmdOn(self);
self->subState = 1;
return;
case eTimerEvent:
cmdPoll(self);
self->subState = 2;
return;
case eMessageEvent:
if (self->run_flag > 0) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
else if (self->run_flag < 0) {
self->run_flag = 0;
change_state(self, DMCState_MotorHalt);
return;
}
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) {
if (self->protocol == 0) { /* cmdOn */
change_state(self, DMCState_MotorOn);
return;
}
else {
DMC_SetTimer(self, AIR_POLL_TIMER);
self->subState = 0;
return;
}
}
else if (self->subState == 2) { /* Poll */
int iRet;
iRet = rspPoll(self, pCmd->inp_buf);
if (iRet > 0) { /* state has changed to ON */
change_state(self, DMCState_MotorOn);
return;
}
else if (iRet < 0) {
/* Handle error return */
self->errorCode = RUNERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorStop);
return;
}
/* TODO handle airpad timeout */
DMC_SetTimer(self, AIR_POLL_TIMER);
self->subState = 0;
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
self->driver_status = HWBusy;
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
}
else {
self->run_flag = 1;
}
return;
case CMD_HALT:
/* handle halt command */
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStop);
}
else {
self->run_flag = -1;
}
return;
}
break;
case eTimeoutEvent:
/* handle message timeout */
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
change_state(self, DMCState_MotorStop);
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
return;
}
static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
DMC_SetTimer(self, self->motOnDelay);
return;
case eTimerEvent:
cmdStatus(self);
self->subState = 1;
return;
case eMessageEvent:
if (self->run_flag > 0) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
else if (self->run_flag < 0) {
self->run_flag = 0;
change_state(self, DMCState_MotorHalt);
return;
}
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* Status Response */
int iRet, absolute;
double target;
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
if (self->threadError) {
self->errorCode = THREADZERO;
self->driver_status = HWFault;
change_state(self, DMCState_MotorStop);
return;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorStop);
return;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorStop);
return;
}
#endif
#ifdef SPEED_ON_MOVE
cmdSpeed(self); /* No Response */
cmdAccel(self); /* No Response */
cmdDecel(self); /* No Response */
#endif
set_lastMotion(self, self->currSteps, self->currCounts);
/* compute position for PA command */
target = self->fTarget;
self->preseek = 0;
self->fPreseek = self->fTarget;
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;
self->fPreseek = target;
}
}
else if (self->backlash_offset < 0) {
/*
* We want to be moving from low to 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->fPreseek = target;
}
}
}
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n",
self->name,
self->currPosition,
self->fPreseek,
self->fTarget);
SICSLogWrite(line, eStatus);
}
self->creep_val = 0;
absolute = motCreep(self, target);
cmdPosition(self, absolute);
self->subState = 2;
/* save pre-motion values for logging */
self->origPosition = self->currPosition;
self->origCounts = self->currCounts;
self->origSteps = self->currSteps;
self->minRatio = 0.0;
self->maxRatio = 0.0;
return;
}
else if (self->subState == 2) { /* PA */
change_state(self, DMCState_Moving);
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
self->driver_status = HWBusy;
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
}
else {
self->run_flag = 1;
}
return;
case CMD_HALT:
/* handle halt command */
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorHalt);
}
else
self->run_flag = -1;
return;
}
break;
case eTimeoutEvent:
/* handle message timeout */
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
return;
}
static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
cmdBegin(self);
self->stepCount = 1;
self->subState = 1;
return;
case eTimerEvent:
/* Note that the substate has already been set */
cmdStatus(self);
return;
case eMessageEvent:
if (self->run_flag != 0) {
change_state(self, DMCState_MotorHalt);
return;
}
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* BG */
/* Check if BG worked (reply != '?') */
if (pCmd->inp_buf[0] == '?') {
/* TODO: what happens when BGx fails? */
self->errorCode = BADCMD;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
self->subState = 2;
DMC_SetTimer(self, MOTOR_POLL_FAST);
return;
}
else if (self->subState == 3) { /* PA */
cmdBegin(self);
self->stepCount++;
self->subState = 1;
return;
}
else if (self->subState == 2 || self->subState == 4) { /* Status */
int iRet, iFlags;
bool fwd_limit_active, rvrs_limit_active, errorlimit;
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
iFlags = self->currFlags;
fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
errorlimit = (iFlags & STATUSERRORLIMIT);
if (self->threadError) {
self->errorCode = THREADZERO;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return;
}
#endif
if (self->stopCode != 0 && self->stopCode != 1) {
if (self->stopCode == 2) {
self->errorCode = FWDLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (self->stopCode == 3) {
self->errorCode = RVRSLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (self->stopCode == 4) {
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (fwd_limit_active) {
self->errorCode = FWDLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
}
self->errorCode = RUNERROR;
snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d",
self->stopCode);
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
if (self->moving) {
/* If Motion Control is off, report HWFault */
if (DMC2280MotionControl != 1) {
if (DMC2280MotionControl == 0)
self->errorCode = MOTIONCONTROLOFF;
else
self->errorCode = MOTIONCONTROLUNK;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
if (self->abs_encoder) {
if (checkMotion(self) == 0) {
/* handle blocked */
self->errorCode = BLOCKED;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
if (checkPosition(self) == 0) {
/* handle runaway */
self->errorCode = POSFAULT; /* recoverable fault */
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return;
}
}
self->subState = 2;
DMC_SetTimer(self, MOTOR_POLL_FAST);
return;
}
/* Motor has stopped */
/* Handle limit switches */
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) {
change_state(self, DMCState_OffTimer);
return;
}
/*
* If this was a pre-seek then compute the next iteration
*/
if (self->preseek) {
double target;
int absolute;
self->preseek = 0;
target = self->fTarget;
self->fPreseek = target;
/* if we are not creeping it is backlash correction */
if (self->creep_val == 0) {
float precision;
/* take precision into account */
MotorGetPar(self->pMot, "precision", &precision);
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;
self->fPreseek = target;
}
}
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;
self->fPreseek = target;
}
}
if (self->preseek && self->stepCount > 10) {
/* limit the maximum number of tries */
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;
absolute = motCreep(self, target);
}
else if (self->preseek) {
absolute = motCreep(self, target);
}
else {
/* change of direction, reset motion check */
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s changed direction",
self->name);
SICSLogWrite(line, eStatus);
}
set_lastMotion(self, self->currSteps, self->currCounts);
absolute = motCreep(self, target);
}
}
else
absolute = motCreep(self, target);
if (self->debug && self->preseek) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n",
self->name,
self->currPosition,
self->fPreseek,
self->fTarget);
SICSLogWrite(line, eStatus);
}
/*
* 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 {
if (self->settle
&& self->subState == 2
&& abs(absolute - self->currSteps) < 100)
{
self->subState = 4;
DMC_SetTimer(self, self->settle);
return;
}
cmdPosition(self, absolute);
self->subState = 3;
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;
self->subState = 2;
DMC_SetTimer(self, self->settle);
return;
}
/* We get here when the motor stops normally
*/
if (true /*self->debug*/) {
double units = self->currPosition - self->origPosition;
long int steps = self->currSteps - self->origSteps;
long int counts = self->currCounts - self->origCounts;
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f,"
" steps=%ld, counts=%ld, stepsPerX=%.6f,"
" minRatio=%.6f, maxRatio=%.6f",
self->name,
units,
steps,
counts,
(double) steps / units,
self->minRatio,
self->maxRatio);
SICSLogWrite(line, eStatus);
}
change_state(self, DMCState_OffTimer);
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* TODO: FIXME RUN command while running */
if (self->driver_status == HWIdle)
self->driver_status = HWBusy;
self->run_flag = 1;
if (self->waitResponse == false) {
change_state(self, DMCState_MotorHalt);
}
return;
case CMD_HALT:
/* handle halt command, send message */
self->run_flag = -1;
if (self->waitResponse == false) {
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;
change_state(self, DMCState_MotorHalt);
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
return;
}
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
DMC_ClearTimer(self);
cmdHalt(self);
/* Note fall through */
case eTimerEvent:
cmdStatus(self);
self->subState = 1;
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* Status Response */
int iRet;
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
if (self->moving) {
DMC_SetTimer(self, MOTOR_POLL_FAST);
return;
}
if (self->faultPending) {
self->run_flag = 0;
}
if (self->run_flag == 1)
change_state(self, DMCState_MotorStart);
else
change_state(self, DMCState_MotorStop);
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN: /* RUN command deferred*/
if (self->driver_status == HWIdle)
self->driver_status = HWBusy;
self->run_flag = 1;
return;
case CMD_HALT: /* HALT command ignored */
self->run_flag = 0;
return;
}
break;
case eTimeoutEvent:
/* handle message timeout */
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
return;
}
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
if (self->run_flag == 0) {
if (self->driver_status == HWBusy)
/*
* To allow the airpads to come down before counting, we defer
* notification if there is no motor off delay and creeping is on. We
* should not need to reposition in motor.c in these circumstances.
*/
if (self->motOffDelay > 0 || self->creep_offset == 0.0)
self->driver_status = HWIdle;
if (self->motOffDelay) {
DMC_SetTimer(self, self->motOffDelay);
return;
}
}
DMC_SetTimer(self, 0);
return;
case eTimerEvent:
if (self->run_flag <= 0)
change_state(self, DMCState_MotorStop);
else
change_state(self, DMCState_MotorOn);
return;
case eMessageEvent:
/* no need to handle message event */
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* handle run command, convert to motor on timer expired */
DMC_ClearTimer(self);
if (self->driver_status == HWIdle)
self->driver_status = HWBusy;
change_state(self, DMCState_MotorOn);
return;
case CMD_HALT:
/* handle halt command, convert to motor off timer expired */
DMC_ClearTimer(self);
change_state(self, DMCState_MotorStop);
return;
}
return;
case eTimeoutEvent:
/* no need to handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
return;
}
static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
self->run_flag = 0;
if (self->noPowerSave) {
if (self->faultPending) {
self->faultPending = false;
self->driver_status = HWFault;
}
change_state(self, DMCState_Idle);
} else {
cmdOff(self);
}
return;
case eTimerEvent:
cmdPoll(self);
self->subState = 1;
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 0) { /* Off command */
if (self->protocol == 0) {
if (self->faultPending) {
self->faultPending = false;
self->driver_status = HWFault;
}
if (self->driver_status != HWFault && self->run_flag == 1) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
change_state(self, DMCState_Idle);
return;
}
}
else if (self->subState == 1) { /* Poll */
int iRet;
iRet = rspPoll(self, pCmd->inp_buf);
if (iRet == 0) { /* state has changed to OFF */
if (self->faultPending) {
self->faultPending = false;
self->driver_status = HWFault;
}
if (self->driver_status != HWFault && self->run_flag == 1) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
change_state(self, DMCState_Idle);
return;
}
else if (iRet < 0) {
/* Handle error return */
self->errorCode = RUNERROR;
self->driver_status = HWFault;
change_state(self, DMCState_Idle);
return;
}
/* TODO handle airpad timeout */
}
DMC_SetTimer(self, AIR_POLL_TIMER);
} while (0);
return;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* handle run command */
if (self->faultPending)
return;
if (self->driver_status == HWIdle)
self->driver_status = HWBusy;
self->run_flag = 1;
return;
case CMD_HALT:
/* ignore it as we are stopped */
self->run_flag = 0;
return;
}
break;
case eTimeoutEvent:
/* handle message timeout */
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
change_state(self, DMCState_MotorStop);
return;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
return;
}
static void DMCState_Error(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
state_trace_prn(self);
return;
case eTimerEvent:
case eMessageEvent:
return;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* TODO: handle run command */
break;
case CMD_HALT:
/* TODO: handle halt command */
break;
}
break;
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->driver_status = HWFault;
self->errorCode = STATEERROR;
return;
}
/** \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;
self = (pDMC2280Driv)pData;
assert(self != NULL);
*fPos = motPosit(self);
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->settle)
self->time_settle_done.tv_sec = 0;
do {
float currPos;
DMC2280GetPos(pData, &currPos);
self->currPosition = currPos;
self->lastPosition = currPos;
self->time_lastPos_set.tv_sec = 0;
} while (0);
/* Reject Infinity and Not a Number (NaN) */
switch (fpclassify(fValue)) {
case FP_ZERO:
case FP_NORMAL:
break;
default:
self->errorCode = BADPAR;
return HWFault;
}
/* Only do it if it is within our hard limits */
if (fValue >= self->fLower && fValue <= self->fUpper) {
self->fTarget = fValue;
state_cmd_execute(self, CMD_RUN);
return 1;
}
/* If we didn't do it then it must have been bad */
self->errorCode = BADPAR;
return HWFault;
}
/** \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;
self = (pDMC2280Driv)pData;
assert(self != NULL);
return self->driver_status;
}
/** \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){
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, are they plugged in?", (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;
#ifdef AMPERROR
case AMPERROR:
strncpy(error, "DRIVE AMPLIFIER ERROR", (size_t)errLen);
break;
#endif
case RUNERROR:
strncpy(error, self->dmc2280Error, (size_t)errLen);
break;
default:
/* 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:
#ifdef AMPERROR
case AMPERROR:
#endif
case RUNERROR:
return MOTFAIL;
case POSFAULT:
self->faultPending = false;
return MOTREDO;
case STATEERROR:
/* recover state error */
DMC_ClearTimer(self);
change_state(self, DMCState_Unknown);
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;
self = (pDMC2280Driv)pData;
assert(self != NULL);
state_cmd_execute(self, CMD_HALT);
state_trace_prn(self);
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->fHome;
return 1;
}
if(strcasecmp(name, "stepsPerX") == 0) {
*fValue = self->stepsPerX;
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,MOTONDELAY) == 0) {
*fValue = self->motOnDelay;
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 || strcasecmp(name,"protocol") == 0) {
*fValue = self->protocol;
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(strcasecmp(name,"bias_bias") == 0) {
*fValue = self->bias_bias;
return 1;
}
if(strcasecmp(name,"bias_bits") == 0) {
*fValue = self->bias_bits;
return 1;
}
if (self->posit_count > 0) {
if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) {
int index;
*fValue = -1.0;
index = strtol(&name[6], NULL, 10);
// if (index < 1 || index > self->posit_count)
// return 0;
*fValue = posit2count(self, index);
return 1;
}
}
if (strcasecmp(name,"absencoder") == 0) {
if (self->abs_encoder)
*fValue = 1.0;
else
*fValue = 0.0;
return 1;
}
if (self->abs_encoder != 0) {
if (strcasecmp(name,"absenc") == 0) {
long lValue;
if (readAbsEnc(self, &lValue) == SUCCESS) {
*fValue = (double) lValue;
return 1;
}
else {
*fValue = -1.0;
return 0;
}
}
if (strcasecmp(name,"absenchome") == 0) {
*fValue = self->absEncHome;
return 1;
}
if (strcasecmp(name,"cntsperx") == 0) {
*fValue = self->cntsPerX;
return 1;
}
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;
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 on delay, managers only */
if(strcasecmp(name,MOTONDELAY) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->motOnDelay = newValue;
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 || strcasecmp(name,"protocol") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->protocol = 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 */
/* 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;
#if 1
return cmdSpeed(self); /* FIXME should signal a HWFault */
#else
snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self, self->speed));
if (FAILURE == DMC_Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
#endif
}
/* 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;
#if 1
return cmdAccel(self); /* FIXME should signal a HWFault */
#else
snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel));
if (FAILURE == DMC_Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
#endif
}
/* 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;
#if 1
return cmdDecel(self); /* FIXME should signal a HWFault */
#else
snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel));
if (FAILURE == DMC_Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
#endif
}
if (strcasecmp("hardlowerlim", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->fLower = newValue;
return 1;
}
if (strcasecmp("hardupperlim", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->fUpper = newValue;
return 1;
}
if (strcasecmp("stepsPerX", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->stepsPerX = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("cntsPerX", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->cntsPerX = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("bias_bias", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->bias_bias = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("bias_bits", name) == 0) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
self->bias_bits = newValue;
return 1;
}
/* XXX Maybe move this to a configuration parameter.*/
/* Set home, managers only. Users should set softposition */
if(strcasecmp(name, HOME) == 0) {
/* Debug Managers only */
if(!SCMatchRights(pCon,usInternal))
return 1;
else {
if ( (self->fLower - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"WARNING:'%s %s' is lower than the lower limit %f", self->name, HOME, self->fLower);
SCWrite(pCon, pError, eWarning);
}
if ( (newValue - self->fUpper) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"WARNING:'%s %s' is greater than the upper limit %f", self->name, HOME, self->fUpper);
SCWrite(pCon, pError, eWarning);
}
self->fHome = newValue;
return 1;
}
}
if (self->posit_count > 0) {
int index;
if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) {
index = strtol(&name[6], NULL, 10);
if (index < 1 || index > self->posit_count)
return 0;
self->positions[index - 1] = count2unit(self, newValue);
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);
if (self->encoderAxis) {
snprintf(buffer, BUFFLEN, "%s.encoderAxis = %c\n", name, self->encoderAxis);
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->fHome);
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.motOnDelay = %d\n", name, self->motOnDelay);
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.Protocol = %d\n", name, self->protocol);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.absEncoder = %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);
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);
}
if (self->posit_count > 0) {
int i;
snprintf(buffer, BUFFLEN, "%s.posit_count = %d\n", name,
self->posit_count);
SCWrite(pCon, buffer, eStatus);
for (i = 0; i < self->posit_count; ++i) {
snprintf(buffer, BUFFLEN, "%s.posit_%d = %lld\n", name, i + 1,
posit2count(self, i + 1));
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 */
change_state(self, DMCState_Error);
break;
case AQU_RECONNECT:
snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
SICSLogWrite(line, eStatus);
/* TODO: reconnect */
/* Reset the state machine */
DMC_ClearTimer(self);
change_state(self, DMCState_Unknown);
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;
}
if (self->positions) {
free(self->positions);
self->positions = 0;
}
self->posit_count = 0;
/* 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->fHome = 0.0;
pNew->fLower = 0.0;
pNew->fUpper = 0.0;
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 = NULL;
/* 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));
}
pNew->ao_id[0] = '\0';
if ((pPtr=getParam(pCon, interp, params, "action", _OPTIONAL)) != NULL) {
strncpy(pNew->ao_id, pPtr, sizeof(pNew->ao_id));
pNew->ao_id[sizeof(pNew->ao_id) - 1] = '\0';
}
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 (islower(pNew->axisLabel))
pNew->axisLabel = toupper(pNew->axisLabel);
if (!(pNew->axisLabel >= 'A' && pNew->axisLabel <= 'H')) {
snprintf(pError, ERRLEN, "\tInvalid axis on motor '%s':%c", motor,
pNew->axisLabel);
SCWrite(pCon,pError,eError);
KillDMC2280(pNew);
free(pNew);
return NULL;
}
if ((pPtr=getParam(pCon, interp, params,"encoderaxis",_OPTIONAL)) == NULL)
pNew->encoderAxis=0;
else {
sscanf(pPtr,"%c",&(pNew->encoderAxis));
if (islower(pNew->encoderAxis))
pNew->encoderAxis = toupper(pNew->encoderAxis);
}
if ((pPtr=getParam(pCon, interp, params,"stepsperx",_REQUIRED)) == NULL) {
KillDMC2280(pNew);
free(pNew);
return NULL;
}
sscanf(pPtr,"%lg",&(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,"motondelay",_OPTIONAL)) == NULL)
pNew->motOnDelay=DEFAULT_DELAY_MOTOR_ON;
else
sscanf(pPtr,"%d",&(pNew->motOnDelay));
if ((pPtr=getParam(pCon, interp, params,"motoffdelay",_OPTIONAL)) == NULL)
pNew->motOffDelay=DEFAULT_DELAY_MOTOR_OFF;
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) {
if ((pPtr=getParam(pCon, interp, params,"protocol",_OPTIONAL)) == NULL)
pNew->protocol=0;
else
sscanf(pPtr,"%d",&(pNew->protocol));
}
else {
sscanf(pPtr,"%d",&(pNew->protocol));
}
/* 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,"%lg",&(pNew->cntsPerX));
/* 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;
}
/* BIAS for encoder - add this to value read */
if ((pPtr=getParam(pCon, interp, params,"bias_bias",_OPTIONAL)) == NULL)
pNew->bias_bias = 0;
else {
sscanf(pPtr, "%d", &(pNew->bias_bias));
}
/* BIAS for encoder - mask this many bits */
if ((pPtr=getParam(pCon, interp, params,"bias_bits",_OPTIONAL)) == NULL)
pNew->bias_bits = 0;
else {
sscanf(pPtr, "%d", &(pNew->bias_bits));
if (pNew->bias_bits < 0)
pNew->bias_bits = -pNew->bias_bits;
}
}
if ((pPtr=getParam(pCon, interp, params,"posit_count",_OPTIONAL)) == NULL)
pNew->posit_count = 0;
else {
sscanf(pPtr,"%d",&(pNew->posit_count));
if (pNew->posit_count < 1 || pNew->posit_count > 99) {
snprintf(pError, ERRLEN, "Invalid posit_count on motor '%s': %d", motor,
pNew->posit_count);
SCWrite(pCon,pError,eError);
pNew->posit_count = 0;
}
else {
int i;
char line[80];
pNew->positions = malloc(pNew->posit_count * sizeof(*pNew->positions));
for (i = 0; i < pNew->posit_count; ++i) {
snprintf(line, 80, "posit_%d", i + 1);
if ((pPtr=getParam(pCon, interp, params,line,_OPTIONAL)) == NULL) {
pNew->positions[i] = 0.0;
}
else {
pNew->positions[i] = count2unit(pNew, strtol(pPtr, NULL, 10));
}
}
}
}
change_state(pNew, DMCState_Unknown);
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;
/* Managers only */
if (!SCMatchRights(pCon, usMugger))
return 0;
cmd[0] = '\0';
for (i = 2; i < argc; ++i) {
int j, k;
j = snprintf(&cmd[idx], CMDLEN - idx, "%s%s",
(i > 2) ? " " : "",
argv[i]);
if (j < 0)
break;
for ( k = 0; k < j && cmd[idx + k]; ++k) {
if (cmd[idx + k] == '%' || cmd[idx + k] == '`')
cmd[idx + k] = self->axisLabel;
if (islower(cmd[idx + k]))
cmd[idx + k] = toupper(cmd[idx + k]);
}
idx += j;
}
DMC_SendReceive(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 = %10g %s", self->name,
1.0 / self->stepsPerX, self->units);
SCWrite(pCon, line, eValue);
if (self->abs_encoder) {
snprintf(line, 132, "%s.encoder_count = %10g %s", self->name,
1.0 / self->cntsPerX, self->units);
SCWrite(pCon, line, eValue);
snprintf(line, 132, "%s.steps_per_count = %10g steps", self->name,
self->stepsPerX / self->cntsPerX);
SCWrite(pCon, line, eValue);
}
return 1;
}
else if (strcasecmp("axis", argv[1]) == 0) {
if (argc > 2) {
if (islower(argv[2][0]))
argv[2][0] = toupper(argv[2][0]);
if (argv[2][0] >= 'A' && argv[2][0] <= 'H')
self->axisLabel = argv[2][0];
else {
char line[132];
snprintf(line, 132, "Invalid axis on motor '%s':%c", self->name,
argv[2][0]);
SCWrite(pCon,line,eError);
return 0;
}
}
else {
char line[132];
snprintf(line, 132, "%s.axis = %c", self->name, self->axisLabel);
SCWrite(pCon, line, eValue);
}
return 1;
}
else if (strcasecmp("encoderaxis", argv[1]) == 0) {
if (argc > 2) {
if (islower(argv[2][0]))
argv[2][0] = toupper(argv[2][0]);
if (argv[2][0] >= 'A' && argv[2][0] <= 'H')
self->encoderAxis = argv[2][0];
else {
char line[132];
snprintf(line, 132, "Invalid encoder axis on motor '%s':%c", self->name,
argv[2][0]);
SCWrite(pCon,line,eError);
return 0;
}
}
else {
char line[132];
snprintf(line, 132, "%s.encoderAxis = %c", self->name, self->encoderAxis);
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) {
/* Reset the state machine */
DMC_ClearTimer(self);
self->driver_status = HWIdle;
change_state(self, DMCState_Unknown);
while (self->myState == DMCState_Unknown) {
pTaskMan pTasker = GetTasker();
TaskYield(pTasker);
}
/* Handle further in generic motor driver */
return MotorAction(pCon, pSics, pData, argc, argv);
}
else if(strcasecmp("state", argv[1]) == 0) {
char line[132];
snprintf(line, 132, "%s.state = %s(%d) (timer=%s)",
self->name,
state_name(self->myState),
self->subState,
self->state_timer ? "active" : "inactive");
SCWrite(pCon, line, eValue);
return 1;
}
else if(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;
}
else if(strcasecmp("trace_switches", argv[1]) == 0) {
if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
trace_switches = true;
SCWrite(pCon, "TRACE SWITCHES ON", eValue);
}
else {
trace_switches = false;
SCWrite(pCon, "TRACE SWITCHES OFF", eValue);
}
return 1;
}
else if(strcasecmp("unit2count", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = strtod(argv[2], NULL);
snprintf(line, 132, "%s.unit2count = %lld",
self->name,
unit2count(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("count2unit", argv[1]) == 0) {
char line[132];
if (argc > 2) {
long long target;
target = strtoll(argv[2], NULL, 10);
snprintf(line, 132, "%s.count2unit = %f",
self->name,
count2unit(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("positions", argv[1]) == 0) {
if (argc == 2) {
int i, k = 0;
char line[1320];
k += snprintf(line, sizeof(line) - k, "%s.positions ", self->name);
for (i = 0; i < self->posit_count; ++i) {
k += snprintf(&line[k], sizeof(line) - k, " %f",
self->positions[i]);
}
SCWrite(pCon, line, eValue);
return 1;
}
if (self->posit_count > 0) {
self->posit_count = 0;
}
if (self->positions) {
free(self->positions);
self->positions = NULL;
self->posit_count = 0;
}
if (argc > 3 && strcasecmp("erase", argv[2]) == 0) {
char line[132];
snprintf(line, 132, "%s.posit_count = %d", self->name, self->posit_count);
SCWrite(pCon, line, eValue);
return 1;
}
self->posit_count = argc - 2;
self->positions = malloc(self->posit_count * sizeof(*self->positions));
int i;
for (i = 0; i < self->posit_count; ++i) {
self->positions[i] = strtod(argv[i + 2], NULL);
}
return 1;
}
if (self->abs_encoder && strcasecmp("absEncHome", argv[1]) == 0) {
if (argc > 2) {
/* Debug Managers only */
if (!(self->debug && SCMatchRights(pCon, usMugger)))
return 0;
long lValue = strtol(argv[2], NULL, 10);
self->absEncHome = lValue;
return 1;
}
else {
char line[132];
snprintf(line, 132, "%s.absEncHome = %d.000000",
self->name,
self->absEncHome);
SCWrite(pCon, line, eValue);
return 1;
}
}
if (self->abs_encoder && strcasecmp("absenc", argv[1]) == 0) {
long lValue;
if (readAbsEnc(self, &lValue) != SUCCESS) {
lValue = -1;
}
char line[132];
snprintf(line, 132, "%s.absEnc = %ld.000000",
self->name,
lValue);
SCWrite(pCon, line, eValue);
return 1;
}
if (self->posit_count > 0) {
if(strcasecmp("posit2count", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = strtod(argv[2], NULL);
snprintf(line, 132, "%s.posit2count = %lld",
self->name,
posit2count(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("count2posit", argv[1]) == 0) {
char line[132];
if (argc > 2) {
long long target;
target = strtoll(argv[2], NULL, 10);
snprintf(line, 132, "%s.count2posit = %f",
self->name,
count2posit(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("unit2posit", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = strtod(argv[2], NULL);
snprintf(line, 132, "%s.unit2posit = %f",
self->name,
unit2posit(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("posit2unit", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = strtod(argv[2], NULL);
snprintf(line, 132, "%s.posit2unit = %f",
self->name,
posit2unit(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("posit", argv[1]) == 0) {
char line[132];
int target = self->currCounts;
if (argc > 2)
target = strtol(argv[2], NULL, 10);
snprintf(line, 132, "%s.posit = %f",
self->name,
count2posit(self, target));
SCWrite(pCon, line, 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