linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
deb6716f9b3fac3b6954f456891b1cafcf1cd9bc
sics/site_ansto/motor_dmc2280.c
Douglas Clowes cd80f567fe Only do the rotary_bits if the previous counts is valid 
This saves decrementing it the first time through because it is in
the fourth quadrant and the initial value (zero) is in the first.
2014-07-10 13:14:27 +10:00

6339 lines
194 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 <limits.h>
#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 <arpa/inet.h>
#include <errno.h>
#include "anstoutil.h"
#include "action.h"
#include "sicsglobal.h"
#define UNITSLEN 256
#define TEXTPARLEN 1024
#define CMDLEN 1024
#define STATE_TRACE (200)
#define MAGIC_MAN (isDuringInitialization || (self->debug && SCMatchRights(pCon, usMugger)))
extern double DoubleTime(void);
/** \brief Used to ensure that the getDMCSetting function is called
* with valid values.
* \see getDMCSetting
*/
typedef enum report_type {eReportMotion, eReportIdle, eReportFinal} ReportType;
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 {
StateFunc oldState;
StateFunc newState;
} 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;
};
typedef struct TrackEntry_s {
double Timestamp;
double Position;
int Steps;
int Counts;
} TrackEntry;
#define TrackEntryCount 100
typedef struct TrackEntryBuffer_s {
struct TrackEntryBuffer_s* nextBuff;
int index;
TrackEntry buff[TrackEntryCount];
} TrackEntryBuffer;
#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)
#define VAR_SIM (1<<10)
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 motorPollFast; /**< msec between polls when busy */
int motorPollSlow; /**< msec between polls when idle */
int airPollTimer; /**< msec between polls when waiting for airpads */
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 lastReportTime;
int lastReportedCounts;
int lastReportedSteps;
double origPosition; /**< Position at motor start */
double origTime; /**< Time at motor start */
int origSteps;
int origCounts;
double moveStartTime;
double moveStopTime;
double minPosition; /**< min position during move */
double maxPosition; /**< max position during move */
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 milliseconds */
struct timeval time_settle_done; /**< time when settling will be over */
bool doSettle; /**< flag to request settle after move, autoreset */
int debug;
int verbose;
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 */
float creep_factor; /**< how much of the offset to creep (0.01 <= creep_factor <= 0.99) */
float creep_substep; /**< how much of the last count to creep (0.01 <= creep_substep <= 0.99) */
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 */
StateFunc myNextState; /**< state to return to */
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;
int posit_name_count;
double* positions;
char** position_names;
int variables;
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
int state_trace_idx;
int state_trace_old;
char state_trace_done[STATE_TRACE];
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 */
int rotary_bits; /**< number of bits in rotary encoder */
int rotary_count; /**< count of rotations */
char ao_id[256];
bool legacy_fsm; /**< flag for legacy_fsm new code */
bool status_valid; /**< flag for status has been set from controller */
bool doStats; /**< flag to request stats collection */
double S_x;
double S_y;
double S_xx;
double S_yy;
double S_xy;
double S_n;
double S_m;
double S_b;
double S_r;
bool doTracking;
TrackEntryBuffer *trackHead, *trackTail;
bool doReportMotion; /**< flag for reporting motion when not driving */
bool doOscillate; /**< flag for motor oscillation */
bool oscillate_up; /**< next oscillation move is to high position */
bool oscillate_up_first; /**< first oscillation move is to high position */
bool oscillate_init; /**< initial move is not counted */
int oscillate_count; /**< number of moves to make or -1 for continuous */
int oscillate_counter; /**< number of moves made - half-oscillations */
double oscillate_low; /**< soft position at low end */
double oscillate_high; /**< soft position at high end */
};
int DMC2280MotionControl = 1; /* defaults to enabled */
static bool trace_switches = false;
#define AIR_POLL_TIMER 250
#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 DISCONNECTED -24
/*--------------------------------------------------------------------*/
#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 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 double getMotorParam(pDMC2280Driv self, char *param) {
int iRet = 0;
float fParam = 0.0;
iRet = MotorGetPar(self->pMot, param, &fParam);
if (!iRet) {
SICSLogPrintf(eLogError, "Motor parameter not found: \"%s\"", param);
}
return fParam;
}
static bool parseNumber(const char *str, double *pNumber) {
char *endPtr;
double fNumber;
if (!pNumber)
return false;
fNumber = strtod(str, &endPtr);
if (endPtr == str || errno == ERANGE)
return false;
/* Reject Infinity and Not a Number (NaN) */
switch (fpclassify(fNumber)) {
case FP_ZERO:
case FP_NORMAL:
break;
default:
return false;
}
*pNumber = fNumber;
return true;
}
static bool getHardFromSoft(pDMC2280Driv self, SConnection *pCon, double fSoft, double *pHard) {
float fLower, fUpper, fZero, fSign, fLim;
double fHard;
bool result = true;
MotorGetPar(self->pMot, "softlowerlim", &fLower);
MotorGetPar(self->pMot, "softupperlim", &fUpper);
MotorGetPar(self->pMot, "softzero", &fZero);
MotorGetPar(self->pMot, "sign", &fSign);
fHard = fSoft;
fHard += fZero;
if (fSign != 0.0)
fHard *= fSign;
if (fSoft > fUpper) {
if (pCon)
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
fSoft, fUpper, self->name);
result = false;
}
if (fSoft < fLower) {
if (pCon)
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
fSoft, fLower, self->name);
result = false;
}
if (fHard > self->fUpper) {
if (pCon)
SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s",
fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name);
result = false;
} else if (fHard < self->fLower) {
if (pCon)
SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s",
fSoft, self->fLower * fSign + fZero, self->fLower, self->name);
result = false;
}
*pHard = fHard;
return result;
}
static bool getSoftFromHard(pDMC2280Driv self, SConnection *pCon, double fHard, double *pSoft) {
float fLower, fUpper, fZero, fSign, fLim;
double fSoft;
bool result = true;
MotorGetPar(self->pMot, "softlowerlim", &fLower);
MotorGetPar(self->pMot, "softupperlim", &fUpper);
MotorGetPar(self->pMot, "softzero", &fZero);
MotorGetPar(self->pMot, "sign", &fSign);
fSoft = fHard;
if (fSign != 0.0)
fSoft /= fSign;
fSoft -= fZero;
if (fHard > self->fUpper) {
if (pCon)
SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s",
fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name);
result = false;
} else if (fHard < self->fLower) {
if (pCon)
SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s",
fSoft, self->fLower * fSign + fZero, self->fLower, self->name);
result = false;
}
if (fSoft > fUpper) {
if (pCon)
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
fSoft, fUpper, self->name);
result = false;
}
if (fSoft < fLower) {
if (pCon)
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
fSoft, fLower, self->name);
result = false;
}
*pSoft = fSoft;
return result;
}
static void TrackingRelease(pDMC2280Driv self) {
while (self->trackHead) {
TrackEntryBuffer *victim;
victim = self->trackHead;
self->trackHead = self->trackHead->nextBuff;
free(victim);
}
self->trackTail = NULL;
}
static TrackEntryBuffer* TrackingBufferAllocate(pDMC2280Driv self) {
/* give the last one if still space available */
if (self->trackTail)
if (self->trackTail->index < TrackEntryCount)
return self->trackTail;
/* allocate and chain in a new one */
if (NULL == self->trackHead) {
self->trackHead = (TrackEntryBuffer *) malloc(sizeof(TrackEntryBuffer));
self->trackTail = self->trackHead;
if (NULL == self->trackTail) {
SICSLogPrintf(eLogError, "Out of memory in TrackingAllocate");
return NULL;
}
} else {
self->trackTail->nextBuff = (TrackEntryBuffer *) malloc(sizeof(TrackEntryBuffer));
if (NULL == self->trackTail->nextBuff) {
SICSLogPrintf(eLogError, "Out of memory in TrackingAllocate");
return NULL;
}
self->trackTail = self->trackTail->nextBuff;
}
memset(self->trackTail, 0, sizeof(TrackEntryBuffer));
return self->trackTail;
}
static TrackEntry *TrackEntryAllocate(pDMC2280Driv self) {
TrackEntryBuffer *bp = TrackingBufferAllocate(self);
if (NULL == bp)
return NULL;
return &bp->buff[bp->index++];
}
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, eLog);
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->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "unit2count Rounding %f to %lld", absolute, result);
SICSLogWrite(line, eLog);
}
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, eLog);
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 arg2posit(pDMC2280Driv self, const char* arg) {
double fPosit = 0.0;
int i;
if (self->posit_name_count > 0 && self->position_names)
for (i = 0; i < self->posit_name_count; ++i)
if (strcasecmp(arg, self->position_names[i]) == 0)
return (double) i + 1;
if (parseNumber(arg, &fPosit))
return fPosit;
return 0.0;
}
static double posit2hard(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 double posit2soft(pDMC2280Driv self, double target) {
double fSoft, fHard;
fHard = posit2hard(self, target);
getSoftFromHard(self, NULL, fHard, &fSoft);
return fSoft;
}
static long long posit2count(pDMC2280Driv self, double target) {
return unit2count(self, posit2hard(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 target in absolute 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->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "motAbsol Rounding %f to %d", absolute, result);
SICSLogWrite(line, eLog);
}
return result;
}
/** \brief Convert motor target in physical units to
* motor target in absolute 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 */
/*
* Legacy case for non-creep scenarios
*/
return target_steps;
}
else {
/*
* This is the creep scenario - and self->preseek is zero
*
* calculate the offset of the target from the current position in motor steps
*/
int offset = target_steps - self->currSteps;
/*
* If this is our first time, initialize creep_val depending on direction
*/
if (self->creep_val == 0) {
if (offset > 0) /* moving up */
self->creep_val = -1;
else if (offset < 0) /* moving down */
self->creep_val = +1;
} else {
int iRet = 0;
/*
* not first, bump the direction sensitive counters
*/
if (self->creep_val > 0) { /* moving down */
/* Handle the case of overrunning the target by reversing direction */
if (offset >= 0)
iRet = 1;
++self->creep_val;
} else {
/* Handle the case of overrunning the target by reversing direction */
if (offset <= 0)
iRet = 1;
--self->creep_val;
}
/*
* In all repetitive things, there just has to be a limit
*/
if (abs(self->creep_val) > MAX_CREEP_STEPS) {
if (self->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s creep stopped, stepcount = %d",
self->name, self->stepCount);
SICSLogWrite(line, eLog);
}
iRet = 1;
}
if (iRet > 0) {
/*
* self->preseek remains zero to say we are finished
*/
return target_steps;
}
}
/*
* We may still move, calculate if we will and how far
*/
if (self->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d",
self->currSteps, target_steps, offset, self->currSteps + offset);
SICSLogWrite(line, eLog);
}
/*
* We don't want to handle each case separately, so fold negative into positive case
*/
if (self->creep_val > 0) /* moving down, handle as positive */
offset = -offset;
/*
* Only move if offset is outside of the creep_precision in steps
*/
if (offset > fabs(self->stepsPerX * self->creep_precision)) {
/*
* if the offset is more than double the creep_offset the we just
* warp to the target minus the creep_offset
*/
if (offset > (int) (2.0 * fabs(self->stepsPerX * self->creep_offset))) {
offset = offset - fabs(self->stepsPerX * self->creep_offset);
} else {
/*
* if the offset is less than or equal to the steps for one encoder count
* we have to "single step" otherwise we want to go part way
*/
if (offset <= fabs(self->stepsPerX / self->cntsPerX)) {
offset = fabs(self->stepsPerX / self->cntsPerX) * self->creep_substep;
if (offset < 1)
offset = 1;
} else {
offset = offset * self->creep_factor;
}
}
/*
* Since we want to move, set preseek to flag it to the caller
*/
self->preseek = 1;
}
/*
* If we folded the negative case, then unfold it
*/
if (self->creep_val > 0) /* moving down, restore to negative */
offset = - offset;
if (self->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "CREEP: Motor=%s, preseek=%d, creep_val=%d, cur_steps=%d",
self->name, self->preseek, self->creep_val, self->currSteps);
SICSLogWrite(line, eLog);
snprintf(line, CMDLEN, "CREEP: Motor=%s, fPreseek=%f, fTarget=%f, target=%f, tgt_steps=%d",
self->name, self->fPreseek, self->fTarget, target, target_steps);
SICSLogWrite(line, eLog);
snprintf(line, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d",
self->currSteps, target_steps, offset, self->currSteps + offset);
SICSLogWrite(line, eLog);
}
/*
* The new absolute step target is the current step position plus
* the possibly modified offset
*/
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", eLog);
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->verbose) {
SICSLogWrite(pCmd->out_buf, eLog);
SICSLogWrite("<TIMEOUT>", eLog);
}
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->verbose) {
SICSLogWrite(cmnd, eLog);
SICSLogWrite(resp, eLog);
}
break;
case '?':
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
strncpy(self->dmc2280Error, &resp[1], CMDLEN);
SICSLogWrite("SendCallback: BADCMD", eError);
SICSLogWrite(cmnd, eError);
SICSLogWrite(resp, eError);
self->errorCode = BADCMD;
break;
default:
if ((cmnd[0] == 'M' && cmnd[1] == 'G') || (cmnd[0] == 'L' && cmnd[1] == 'V')) {
/* MG and LV commands can produce this result */
if (self->verbose) {
SICSLogWrite(cmnd, eLog);
SICSLogWrite(resp, eLog);
}
break;
}
SICSLogWrite("SendCallback: BADUNKNOWN", eError);
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->verbose)
SICSLogWrite(cmd, eLog);
if (status == -1)
self->errorCode = MOTCMDTMO;
else
self->errorCode = BADUNKNOWN;
return FAILURE;
}
switch (reply[0]) {
case ':': /* prompt */
if (self->verbose) {
SICSLogWrite(cmd, eLog);
SICSLogWrite(reply, eLog);
}
return SUCCESS;
case ' ': /* leading blank */
case '-': /* leading minus sign */
if (self->verbose) {
SICSLogWrite(cmd, eLog);
SICSLogWrite(reply, eLog);
}
return SUCCESS;
case '?':
strncpy(self->lastCmd, cmd, CMDLEN);
strncpy(self->dmc2280Error, &reply[1], CMDLEN);
SICSLogWrite("DMC_SendReceive: BADCMD", eError);
SICSLogWrite(cmd, eError);
SICSLogWrite(reply, eError);
self->errorCode = BADCMD;
return FAILURE;
default:
if ((cmd[0] == 'M' && cmd[1] == 'G') || (cmd[0] == 'L' && cmd[1] == 'V')) {
/* MG and LV commands can produce this result */
if (self->verbose) {
SICSLogWrite(cmd, eLog);
SICSLogWrite(reply, eLog);
}
return SUCCESS;
}
strncpy(self->lastCmd, cmd, CMDLEN);
strncpy(self->dmc2280Error, &reply[0], CMDLEN);
SICSLogWrite("DMC_SendReceive: BADUNKNOWN", eError);
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);
}
/*
* TODO: idle and final reporting
* when final, always report MOTDRIVE and MOTEND
* when idle, if time and motion > threshold, report MOTDRIVE and MOTEND
* when neither, if time and motion > threshold, report MOTDRIVE but not MOTEND
*/
static bool report_motion(pDMC2280Driv self, ReportType reportType) {
double current_time;
bool doMotDrive = false;
bool doMotEnd = false;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
current_time = DoubleTime();
if (reportType == eReportFinal) {
doMotDrive = true;
doMotEnd = true;
} else {
double skip_time;
skip_time = 0.001 * getMotorParam(self, "movecount");
if (self->lastReportTime + skip_time <= current_time) {
long lDelta;
if (self->abs_encoder)
lDelta = fabs(self->currCounts - self->lastReportedCounts);
else
lDelta = fabs(self->currSteps - self->lastReportedSteps);
if (reportType == eReportIdle) {
if (lDelta > 10) {
doMotDrive = true;
doMotEnd = true;
}
} else
doMotDrive = true;
}
}
if (doMotDrive || doMotEnd) {
SConnection *pDumCon;
MotCallback sCall;
pDumCon = SCCreateDummyConnection(pServ->pSics);
MotorGetSoftPosition(self->pMot, pDumCon, &sCall.fVal);
SCDeleteConnection(pDumCon);
sCall.pName = self->pMot->name;
if (doMotDrive) {
InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall);
if (doMotEnd)
InvokeCallBack(self->pMot->pCall, MOTEND, &sCall);
self->lastReportedCounts = self->currCounts;
self->lastReportedSteps = self->currSteps;
self->lastReportTime = current_time;
return true;
}
}
return false;
}
/** \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->verbose || 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->verbose) {
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);
}
/*
* Send configuration information to a simulated (fake) motor
*/
static void cmdConfig(pDMC2280Driv self) {
char cmd[CMDLEN];
char name[21];
int i, j;
j = 0;
/* copy selected/restricted characters in name */
for (i = 0; i < 20; ++i) {
if (self->name[i] == '\0')
break;
if (self->name[i] >= 'a' && self->name[i] <= 'z')
name[j++] = self->name[i];
else if (self->name[i] >= 'A' && self->name[i] <= 'Z')
name[j++] = self->name[i];
else if (self->name[i] >= '0' && self->name[i] <= '9')
name[j++] = self->name[i];
else if (self->name[i] == '-' || self->name[i] == '_')
name[j++] = self->name[i];
}
name[j] = '\0';
snprintf(cmd, CMDLEN, "MG \"CONFIG%c=SPX=%g,CPX=%g,RL=%g,FL=%g,UH=%g,%s=%d,ROT=%d,NAM='%s'\"",
self->axisLabel,
self->stepsPerX,
self->cntsPerX,
self->fLower,
self->fUpper,
self->fHome,
self->abs_encoder ? "EH" : "MH",
self->abs_encoder ? self->absEncHome : self->motorHome,
self->rotary_bits,
name);
DMC_Send(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;
char cmd[CMDLEN];
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->verbose) && 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, eLog);
else
SICSLogWrite(line, eLog);
}
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, eLog);
else
SICSLogWrite(line, eLog);
}
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, eLog);
else
SICSLogWrite(line, eLog);
}
}
self->currFlags = iFlags;
self->currSteps = iSteps;
if (self->bias_bits > 0)
iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1);
if (self->rotary_bits > 0 && self->status_valid) {
int shift = 2;
int mask = (1 << shift) - 1;
int old_bits = (self->currCounts >> (self->rotary_bits - shift)) & mask;
int new_bits = (iCounts >> (self->rotary_bits - shift)) & mask;
if (old_bits == 0 && new_bits == mask) {
self->rotary_count -= 1;
SICSLogPrintf(eLog, "%s: Rotary count decrement to %d", self->name, self->rotary_count);
}
if (old_bits == mask && new_bits == 0) {
self->rotary_count += 1;
SICSLogPrintf(eLog, "%s: Rotary count increment to %d", self->name, self->rotary_count);
}
iCounts += self->rotary_count << self->rotary_bits;
}
self->currCounts = iCounts;
self->currPosition = motPosit(self);
if (self->currPosition < self->minPosition)
self->minPosition = self->currPosition;
if (self->currPosition > self->maxPosition)
self->maxPosition = self->currPosition;
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);
if (self->protocol == 3) {
snprintf(cmd, CMDLEN, "RUN%cAK=1", self->axisLabel);
DMC_Send(self, cmd);
}
}
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;
}
self->status_valid = true;
return 1;
}
static int rspVars(pDMC2280Driv self, const char* text) {
self->variables = 0;
if (has_var(self, text, "SIMULATE"))
self->variables |= VAR_SIM;
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);
char cmd[CMDLEN];
if (iReply < 0) {
snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER REQ ERROR: %d",
iReply);
snprintf(cmd, CMDLEN, "RSP%cAK=1", self->axisLabel);
DMC_Send(self, cmd);
}
return iReply;
}
static void state_trace_prn(pDMC2280Driv self) {
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
int idx = self->state_trace_idx;
int lines_printed = 0;
struct timeval tv;
gettimeofday(&tv, NULL);
do {
char* line;
struct timeval *tp;
tp = &self->state_trace_time[idx];
if (self->state_trace_done[idx])
line = NULL;
else
line = &self->state_trace_text[idx][0];
if (line && *line) {
if (lines_printed == 0)
/* print the opening message only if there are lines to print */
SICSLogTimePrintf(eLog, &tv, "Motor: %s, state_trace_prn trace history listing start", self->name);
SICSLogWriteTime(line, eLog, tp);
lines_printed++;
}
self->state_trace_done[idx] = 1;
if (++idx >= STATE_TRACE)
idx = 0;
} while (idx != self->state_trace_idx);
/* print a close message, even if there are zero lines */
SICSLogTimePrintf(eLog, &tv, "Motor: %s, state_trace_prn trace history listing end (%d entries)", self->name, lines_printed);
#else
SICSLogPrintf(eLog, "Motor: %s, state_trace_prn not implemented", self->name);
#endif
}
/* State Functions */
static void DMCState_Disconnected(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Init(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_StepMove(pDMC2280Driv self, pEvtEvent event);
static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event);
static void DMCState_BacklashStart(pDMC2280Driv self, pEvtEvent event);
static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event);
static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event);
static void DMCState_CreepCont(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_Disconnected) return "DMCState_Disconnected";
if (func == DMCState_Init) return "DMCState_Init";
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_StepMove) return "DMCState_StepMove";
if (func == DMCState_SimpleMove) return "DMCState_SimpleMove";
if (func == DMCState_Oscillate) return "DMCState_Oscillate";
if (func == DMCState_BacklashStart) return "DMCState_BacklashStart";
if (func == DMCState_BacklashCont) return "DMCState_BacklashCont";
if (func == DMCState_CreepStart) return "DMCState_CreepStart";
if (func == DMCState_CreepCont) return "DMCState_CreepCont";
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 - 1 && *p) {
if (*p == '\r') {
if (i >= len -3)
break;
s1[i++] = '\\';
s1[i++] = 'r';
++p;
}
else if (*p == '\n') {
if (i >= len -3)
break;
s1[i++] = '\\';
s1[i++] = 'n';
++p;
}
else if (*p == '\t') {
if (i >= len -3)
break;
s1[i++] = '\\';
s1[i++] = 't';
++p;
}
else if (*p < ' ' || *p > '~') {
const char hex[] = "0123456789ABCDEF";
if (i >= len -5)
break;
s1[i++] = '<';
s1[i++] = hex[(*p >> 4) & 0xF];
s1[i++] = hex[(*p) & 0xF];
s1[i++] = '>';
++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 (%s => %s)",
state_name(event->event.sta.oldState),
state_name(event->event.sta.newState));
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 state_trace_ins(pDMC2280Driv self, const char *fmt, ...) {
va_list ap;
char line[CMDLEN];
char *lp = line;
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
gettimeofday(&self->state_trace_time[self->state_trace_idx], NULL);
lp = &self->state_trace_text[self->state_trace_idx][0];
self->state_trace_done[self->state_trace_idx] = 0;
if (true)
#else
if (self->verbose || self->trace)
#endif
{
char format[CMDLEN];
snprintf(format, CMDLEN, "Motor=%s, State=%s(%d), %s",
self->name,
state_name(self->myState),
self->subState,
fmt);
va_start(ap, fmt);
vsnprintf(lp, CMDLEN, format, ap);
va_end(ap);
if (self->verbose)
SICSLogWrite(lp, eLog);
if (self->trace)
SCWrite(self->trace, lp, eLog);
}
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
if (++self->state_trace_idx >= STATE_TRACE)
self->state_trace_idx = 0;
#endif
}
static void report_event(pDMC2280Driv self, pEvtEvent event) {
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
if (true)
#else
if (self->verbose || self->trace)
#endif
{
char text[CMDLEN];
state_trace_ins(self, "Event=%s",
event_name(event, text, CMDLEN));
}
}
static void unhandled_event(pDMC2280Driv self, pEvtEvent event) {
char text[CMDLEN];
state_trace_ins(self, "Unhandled Event=%s",
event_name(event, text, CMDLEN));
}
static void handle_event(pDMC2280Driv self, pEvtEvent event) {
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
if (true)
#else
if (self->verbose || self->trace)
#endif
{
report_event(self, event);
}
self->myState(self, event);
}
static void change_state(pDMC2280Driv self, StateFunc func) {
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
if (true)
#else
if (self->verbose || self->trace)
#endif
{
state_trace_ins(self, "NewState=%s",
state_name(func));
}
self->myPrevState = self->myState;
self->myState = func;
self->subState = 0;
EvtEvent evt;
evt.event_type = eStateEvent;
evt.event.sta.oldState = self->myPrevState;
evt.event.sta.newState = self->myState;
self->myState(self, &evt);
}
/*
* Move the status handling out of line to reduce clutter.
* If we can handle the status response and take the action then,
* return true to the caller else return false.
*/
static bool motorHandleStatus(pDMC2280Driv self) {
int iFlags;
bool fwd_limit_active, rvrs_limit_active, errorlimit;
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 true;
}
else if (self->runError) {
self->errorCode = RUNERROR;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
#ifdef AMPERROR
else if (self->ampError) {
self->errorCode = AMPERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return true;
}
#endif
if (self->stopCode != 0 && self->stopCode != 1) {
/*
* Handle abnormal stop codes
*/
if (self->stopCode == 2) {
self->errorCode = FWDLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
else if (self->stopCode == 3) {
self->errorCode = RVRSLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
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 true;
}
else if (fwd_limit_active) {
self->errorCode = FWDLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
}
self->errorCode = RUNERROR;
snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d",
self->stopCode);
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
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 true;
}
if (self->abs_encoder) {
if (checkMotion(self) == 0) {
/* handle blocked */
self->errorCode = BLOCKED;
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
if (checkPosition(self) == 0) {
/* handle runaway */
SICSLogPrintf(eWarning, "%s: Position overrun at %f (%d)",
self->name,
self->currPosition,
self->currCounts);
self->errorCode = POSFAULT; /* recoverable fault */
self->faultPending = true; /* defer fault */
change_state(self, DMCState_MotorHalt);
return true;
}
}
/*
* The motor is still moving and everything seems OK,
* set the poll timer and continue
*/
self->subState = 2;
DMC_SetTimer(self, self->motorPollFast);
return true;
}
/*
* The motor has stopped, check the 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 true;
}
/*
* The motor has stopped and everything seems OK
*/
return false;
}
static int state_snd_callback(pAsyncTxn pCmd) {
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
SendCallback(pCmd);
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
if (true)
#else
if (self->verbose || self->trace)
#endif
{
char text[CMDLEN];
text[0] = '\0';
str_n_cat(text, CMDLEN, pCmd->out_buf);
str_n_cat(text, CMDLEN, "|");
str_n_cat(text, CMDLEN, pCmd->inp_buf);
state_trace_ins(self, "Send Complete=%s",
text);
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_Disconnected(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
self->run_flag = 0;
self->driver_status = HWIdle;
if (AsyncUnitIsQueueConnected(self->asyncUnit) != 1)
self->errorCode = DISCONNECTED;
/* set a timer and wait until the device is connected */
DMC_SetTimer(self, self->motorPollSlow);
return;
case eTimerEvent:
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
if (self->pMot && AsyncUnitIsQueueConnected(self->asyncUnit) == 1) {
/* reconnected but state is unknown */
change_state(self, DMCState_Init);
return;
}
DMC_SetTimer(self, self->motorPollSlow);
return;
case eMessageEvent:
return;
case eCommandEvent:
return;
case eTimeoutEvent:
return;
}
}
static void DMCState_Init(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;
}
/* Defer Speed, Accel and Decel to move command */
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, self->motorPollSlow);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 0) { /* waitResponse */
change_state(self, DMCState_Init);
return;
}
if (self->subState == 1) { /* Vars */
rspVars(self, pCmd->inp_buf);
if (self->variables & VAR_SIM)
cmdConfig(self);
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_Init);
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;
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->doReportMotion) {
(void) report_motion(self, eReportFinal);
self->doReportMotion = false;
} else {
(void) report_motion(self, eReportIdle);
}
if (trace_switches || self->verbose)
DMC_SetTimer(self, self->motorPollFast);
else
DMC_SetTimer(self, self->motorPollSlow);
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 driver_status and run_flag*/
if (self->driver_status == HWBusy)
self->driver_status = HWIdle;
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, self->motorPollSlow);
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, self->airPollTimer);
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, self->airPollTimer);
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
cmdSpeed(self); /* No Response */
cmdAccel(self); /* No Response */
cmdDecel(self); /* No Response */
self->minPosition = self->currPosition;
self->maxPosition = self->currPosition;
self->stepCount = 0;
self->moveStartTime = DoubleTime();
if (!self->legacy_fsm) {
if (self->trackHead)
TrackingRelease(self);
self->myNextState = NULL;
if (self->doOscillate) {
change_state(self, DMCState_Oscillate);
return;
}
if (self->backlash_offset != 0) {
change_state(self, DMCState_BacklashStart);
return;
} else if (self->creep_offset != 0) {
change_state(self, DMCState_CreepStart);
return;
}
change_state(self, DMCState_SimpleMove);
return;
}
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->verbose) {
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, eLog);
}
self->creep_val = 0;
absolute = motCreep(self, target);
cmdPosition(self, absolute);
self->subState = 2;
/* save pre-motion values for logging */
self->origTime = DoubleTime();
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;
}
/*
* This state moves the motor from its current position to fTarget.
* There is no backlash and no creep.
* If there is a fault it transfers out
*/
static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) {
double target;
int absolute;
switch (event->event_type) {
case eStateEvent:
self->myNextState = NULL;
target = self->fTarget;
absolute = motAbsol(self, target);
self->doSettle = self->settle > 0;
/* decide if we should be letting the motor settle */
cmdPosition(self, absolute);
return;
case eTimerEvent:
cmdStatus(self);
return;
case eMessageEvent:
if (self->run_flag != 0) {
change_state(self, DMCState_MotorHalt);
return;
}
/* set the next state after this move */
self->myNextState = DMCState_OffTimer;
change_state(self, DMCState_StepMove);
return;
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;
}
/*
* This state sets up to moves the motor from its current position to fTarget + backlash_offset.
* If there is creep, it happens later.
* If there is a fault it transfers out
*/
static void DMCState_BacklashStart(pDMC2280Driv self, pEvtEvent event) {
double target;
int absolute;
bool doPreseek;
switch (event->event_type) {
case eStateEvent:
self->myNextState = NULL;
target = self->fTarget;
doPreseek = false;
if (self->backlash_offset != 0) {
if (self->backlash_offset > 0) {
/*
* We want to be moving from high to low, if the target is higher
* than the current position we must pre-seek to the higher side
*/
if (target > self->currPosition) {
doPreseek = true;
target += self->backlash_offset;
if (target > self->fUpper)
target = self->fUpper;
}
}
else if (self->backlash_offset < 0) {
/*
* We want to be moving from low to high, if the target is lower than
* the current position we must pre-seek to the lower side
*/
if (target < self->currPosition) {
doPreseek = true;
target += self->backlash_offset;
if (target < self->fLower)
target = self->fLower;
}
}
}
if (doPreseek == false) {
/* preseek is not required, handle as a simple move */
if (self->creep_offset != 0) {
change_state(self, DMCState_CreepStart);
} else {
change_state(self, DMCState_SimpleMove);
}
return;
}
self->preseek = 1;
self->fPreseek = target;
absolute = motAbsol(self, target);
/* decide if we should be letting the motor settle */
self->doSettle = self->settle > 0;
cmdPosition(self, absolute);
return;
case eTimerEvent:
cmdStatus(self);
return;
case eMessageEvent:
if (self->run_flag != 0) {
change_state(self, DMCState_MotorHalt);
return;
}
/* set the next state after this move */
self->myNextState = DMCState_BacklashCont;
change_state(self, DMCState_StepMove);
return;
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;
}
/*
* This state moves the motor from its current position to fTarget + backlash_offset.
* If there is creep, it happens after this.
* If there is a fault it transfers out
*/
static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event) {
double target;
float precision;
int absolute;
switch (event->event_type) {
case eStateEvent:
self->myNextState = NULL;
target = self->fTarget;
self->preseek = 0;
/* 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->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
self->name, self->stepCount);
SICSLogWrite(line, eLog);
}
self->preseek = 0;
}
if (self->preseek == 0) {
/* preseek is not required, handle as a simple move */
if (self->creep_offset != 0) {
change_state(self, DMCState_CreepStart);
} else {
change_state(self, DMCState_SimpleMove);
}
return;
}
self->preseek = 1;
self->fPreseek = target;
absolute = motAbsol(self, target);
/* decide if we should be letting the motor settle */
self->doSettle = self->settle > 0;
cmdPosition(self, absolute);
return;
case eTimerEvent:
cmdStatus(self);
return;
case eMessageEvent:
if (self->run_flag != 0) {
change_state(self, DMCState_MotorHalt);
return;
}
/* set the next state after this move */
self->myNextState = DMCState_BacklashCont;
change_state(self, DMCState_StepMove);
return;
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;
}
/*
* This state sets up to move the motor from its current position to fTarget by creeping.
* If there was a backlash correction, it has already happened.
* If there is a fault it transfers out
*/
static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event) {
double target;
int absolute;
switch (event->event_type) {
case eStateEvent:
self->myNextState = NULL;
/* initialize the creep control variable */
self->creep_val = 0;
/*
* calculate the absolute step position of the adjusted target
*/
target = self->fTarget;
self->preseek = 0;
absolute = motCreep(self, target);
if (self->preseek == 0) {
/* it is finished */
change_state(self, DMCState_OffTimer);
return;
}
/*
* decide if we should be letting the motor settle
*/
self->doSettle = self->settle > 0;
if (self->doSettle) {
if (abs(absolute - self->currSteps) > fabs(self->creep_offset * self->stepsPerX))
self->doSettle = false;
if (self->creep_precision > 0.0)
if (abs(absolute - self->currSteps) > 10.0 * fabs(self->creep_precision * self->stepsPerX))
self->doSettle = false;
}
self->fPreseek = target;
cmdPosition(self, absolute);
return;
case eTimerEvent:
cmdStatus(self);
return;
case eMessageEvent:
if (self->run_flag != 0) {
change_state(self, DMCState_MotorHalt);
return;
}
/* set the next state after this move */
self->myNextState = DMCState_CreepCont;
change_state(self, DMCState_StepMove);
return;
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;
}
/*
* This state moves the motor from its current position to fTarget by creeping.
* There is no backlash.
* If there is a fault it transfers out
*/
static void DMCState_CreepCont(pDMC2280Driv self, pEvtEvent event) {
double target;
int absolute;
switch (event->event_type) {
case eStateEvent:
self->myNextState = NULL;
if (self->preseek == 0) {
/* it is finished */
change_state(self, DMCState_OffTimer);
return;
}
target = self->fTarget;
self->preseek = 0;
absolute = motCreep(self, target);
if (self->preseek == 0) {
/* it is finished */
change_state(self, DMCState_OffTimer);
return;
}
/* decide if we should be letting the motor settle */
self->doSettle = self->settle > 0;
if (self->doSettle) {
if (abs(absolute - self->currSteps) > fabs(self->creep_offset * self->stepsPerX))
self->doSettle = false;
if (self->creep_precision > 0.0)
if (abs(absolute - self->currSteps) > 10.0 * fabs(self->creep_precision * self->stepsPerX))
self->doSettle = false;
}
self->fPreseek = target;
cmdPosition(self, absolute);
return;
case eTimerEvent:
cmdStatus(self);
return;
case eMessageEvent:
if (self->run_flag != 0) {
change_state(self, DMCState_MotorHalt);
return;
}
self->myNextState = DMCState_CreepCont;
change_state(self, DMCState_StepMove);
return;
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;
}
/*
* This state oscillates the motor between two positions
*/
static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event) {
double target;
int absolute;
switch (event->event_type) {
case eStateEvent:
/* Cancel any return address */
self->myNextState = NULL;
/* Calculate our next move */
if (self->oscillate_up) {
double fHard;
if (getHardFromSoft(self, NULL, self->oscillate_high, &fHard))
self->fTarget = fHard;
self->oscillate_up = false;
} else {
double fHard;
if (getHardFromSoft(self, NULL, self->oscillate_low, &fHard))
self->fTarget = fHard;
self->oscillate_up = true;
}
/*
* We do not want to count the first move-to-end.
* We want to count each time we are at the same point as the first time.
* So, we remember what we were doing the first time
* And we count each time it is the same.
*/
if (self->oscillate_init) {
if (true /*self->verbose*/) {
SICSLogPrintf(eLog, "Motor=%s oscillate: first move is to %f, next is %s",
self->name,
self->fTarget,
self->oscillate_up ? "UP" : "DOWN");
}
self->oscillate_up_first = self->oscillate_up;
self->oscillate_init = false;
} else {
if (self->oscillate_up_first == self->oscillate_up ? false : true) {
self->oscillate_counter++;
}
/* handle termination conditions (e.g. count) */
if (self->oscillate_count > 0 && self->oscillate_counter >= self->oscillate_count) {
self->doOscillate = false;
} else {
if (true /*self->verbose*/) {
SICSLogPrintf(eLog, "Motor=%s oscillate: count %d move is to %f, next is %s",
self->name,
self->oscillate_counter,
self->fTarget,
self->oscillate_up ? "UP" : "DOWN");
}
}
}
/* handle termination conditions (e.g. count) */
if (!self->doOscillate) {
if (true /*self->verbose*/) {
SICSLogPrintf(eLog, "Motor=%s oscillate: stopped at count %d",
self->name,
self->oscillate_counter);
}
(void) report_motion(self, eReportFinal);
change_state(self, DMCState_OffTimer); /* TODO: check this is OK */
return;
}
/* TODO: set timer for optional pause */
/* Note: fallthrough is intentional */
case eTimerEvent:
target = self->fTarget;
self->preseek = 0;
absolute = motCreep(self, target);
self->doSettle = false;
cmdPosition(self, absolute);
return;
case eMessageEvent:
if (self->run_flag != 0) {
change_state(self, DMCState_MotorHalt);
return;
}
/* set the next state after this move */
self->myNextState = DMCState_Oscillate;
change_state(self, DMCState_StepMove);
return;
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;
}
/*
* This state moves the motor from its current position to the current destination
* set by PA (or PR) by the caller in myNextState.
* If there is a fault it transfers out, otherwise it moves to the myNextState
* state when the move is successfully completed.
*/
static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
/*
* The PA has already been confirmed by the caller, now do the move
*/
case eStateEvent:
/* save pre-motion values for logging */
set_lastMotion(self, self->currSteps, self->currCounts);
self->origTime = DoubleTime();
self->origPosition = self->currPosition;
self->origCounts = self->currCounts;
self->origSteps = self->currSteps;
self->minRatio = 0.0;
self->maxRatio = 0.0;
/*
* Record how the motor is tracking
*/
if (self->doTracking) {
TrackEntry *ent = TrackEntryAllocate(self);
if (ent) {
ent->Timestamp = DoubleTime();
ent->Position = self->currPosition;
ent->Steps = self->currSteps;
ent->Counts = self->currCounts;
}
}
/* begin moving */
cmdBegin(self);
self->stepCount++;
self->subState = 1;
return;
case eTimerEvent:
/*
* Find out how the motor is travelling,
* 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, self->motorPollFast);
return;
} else if (self->subState == 2) { /* Status */
int iRet;
/* update the stop time now in case of error exit */
self->moveStopTime = DoubleTime();
/* parse the status response and set error codes */
iRet = rspStatus(self, pCmd->inp_buf);
/* if the parse failed break out of here */
if (iRet == 0)
break;
/*
* Record how the motor is tracking
*/
if (self->doTracking) {
TrackEntry *ent = TrackEntryAllocate(self);
if (ent) {
ent->Timestamp = DoubleTime();
ent->Position = self->currPosition;
ent->Steps = self->currSteps;
ent->Counts = self->currCounts;
}
}
if (self->doReportMotion)
(void) report_motion(self, eReportMotion);
/* if the status response can be handled here, return */
if (motorHandleStatus(self))
return;
/*
* We get here when the motor stops normally
*/
if (self->doSettle) {
self->doSettle = false;
DMC_SetTimer(self, self->settle);
return;
}
if (true /*self->verbose*/ || true /*self->doStats*/) {
double units = self->currPosition - self->origPosition;
long int steps = self->currSteps - self->origSteps;
long int counts = self->currCounts - self->origCounts;
double time = DoubleTime() - self->origTime;
char line[CMDLEN];
if (true /*self->verbose*/) {
snprintf(line, CMDLEN, "Motor=%s stopped: p=%.6f(%.6f-%.6f), u=%.6f,"
" s=%ld, c=%ld, s/X=%.6f,"
" r=(%.6f-%.6f), t=%.3fS",
self->name,
self->currPosition,
self->minPosition,
self->maxPosition,
units,
steps,
counts,
(double) steps / units,
self->minRatio,
self->maxRatio,
time);
SICSLogWrite(line, eLog);
}
if (self->abs_encoder && true /*self->doStats*/) {
if (labs(steps) > fabs(self->stepsPerX) && labs(counts) > fabs(self->cntsPerX)) {
self->S_x += (double) counts;
self->S_y += (double) steps;
self->S_xy += (double) counts * (double) steps;
self->S_xx += (double) counts * (double) counts;
self->S_yy += (double) steps * (double) steps;
self->S_n += (double) 1;
self->S_m = self->S_n * self->S_xy - self->S_x * self->S_y;
self->S_m /= self->S_n * self->S_xx - self->S_x * self->S_x;
self->S_b = self->S_y / self->S_n - self->S_m * self->S_x / self->S_n;
self->S_r = self->S_n * self->S_xy - self->S_x * self->S_y;
self->S_r /= sqrt((self->S_n * self->S_xx - self->S_x * self->S_x)
* (self->S_n * self->S_yy - self->S_y * self->S_y));
if (self->S_n > 2) {
snprintf(line, CMDLEN, "Motor=%s stats: n=%.0f, S_x=%.0f, S_y=%.0f, S_xx=%.0f, S_yy=%.0f, S_xy=%.0f",
self->name,
self->S_n,
self->S_x,
self->S_y,
self->S_xx,
self->S_yy,
self->S_xy);
SICSLogWrite(line, eLog);
snprintf(line, CMDLEN, "Motor=%s stats: n=%.0f, m=%f, b=%f, r=%f, stepsPerX=%f",
self->name,
self->S_n,
self->S_m,
self->S_b,
self->S_r,
self->cntsPerX * self->S_m);
SICSLogWrite(line, eLog);
}
}
}
}
change_state(self, self->myNextState);
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_Moving(pDMC2280Driv self, pEvtEvent event) {
/*
* Substates:
* 0 On entry , immediately changed to 1
* 1 BG sent, waiting for reply
* 2 poll_timer/status poll sent
* 3 PA sent waiting for reply
* 4 settle_timer/status poll sent
*/
switch (event->event_type) {
case eStateEvent:
/*
* The PA has already been confirmed by the caller, now do the move
*/
cmdBegin(self);
self->stepCount = 1;
self->subState = 1;
return;
case eTimerEvent:
/*
* Find out how the motor is travelling,
* 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, self->motorPollFast);
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;
/* update the stop time now in case of error exit */
self->moveStopTime = DoubleTime();
/* parse the status response and set error codes */
iRet = rspStatus(self, pCmd->inp_buf);
/* if the parse failed break out of here */
if (iRet == 0)
break;
/* if the status response can be handled here, return */
if (motorHandleStatus(self))
return;
/*
* If we get here, the motor has stopped and everything seems OK.
* We must decide on the next course of action.
*/
if (self->preseek) {
/*
* this was a pre-seek so compute the next iteration
*/
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->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
self->name, self->stepCount);
SICSLogWrite(line, eLog);
}
self->preseek = 0;
absolute = motCreep(self, target);
}
else if (self->preseek) {
absolute = motCreep(self, target);
if (self->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s unchanged direction: "
"preseek=%d, fPreseek=%f, steps=%d, creep_val=%d",
self->name,
self->preseek,
self->fPreseek,
absolute - self->currSteps,
self->creep_val);
SICSLogWrite(line, eLog);
}
}
else {
/* change of direction, reset motion check */
set_lastMotion(self, self->currSteps, self->currCounts);
absolute = motCreep(self, target);
if (self->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s changed direction: "
"preseek=%d, fPreseek=%f, steps=%d, creep_val=%d",
self->name,
self->preseek,
self->fPreseek,
absolute - self->currSteps,
self->creep_val);
SICSLogWrite(line, eLog);
}
}
}
else
absolute = motCreep(self, target);
if (self->verbose && 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, eLog);
}
/*
* If we are still iterating, continue
*/
if (self->preseek) {
if (absolute == self->currSteps) {
if (self->verbose) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s motion stopped, absolute = %d",
self->name, absolute);
SICSLogWrite(line, eLog);
}
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->verbose*/) {
double units = self->currPosition - self->origPosition;
long int steps = self->currSteps - self->origSteps;
long int counts = self->currCounts - self->origCounts;
double time = DoubleTime() - self->origTime;
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f,"
" steps=%ld, counts=%ld, stepsPerX=%.6f,"
" ratio=(%.6f-%.6f), time=%.3f",
self->name,
units,
steps,
counts,
(double) steps / units,
self->minRatio,
self->maxRatio,
time);
SICSLogWrite(line, eLog);
}
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, self->motorPollFast);
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, self->airPollTimer);
} 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:
if (self->myPrevState != self->myState)
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;
self->doOscillate = false;
self->doReportMotion = false;
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);
*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 reverse limit switch", (size_t)errLen);
break;
case FWDLIM:
strncpy(error, "Crashed into forward limit switch", (size_t)errLen);
break;
case POSFAULT:
strncpy(error, "Positioning fault, overrun 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;
case DISCONNECTED:
strncpy(error, "MOTION CONTROL DEVICE DISCONNECTED", (size_t)errLen);
break;
default:
/* What's the default */
snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
break;
}
/* Allocate errLen bytes for error messages */
if (self->errorMsg)
free(self->errorMsg);
self->errorMsg = (char *) malloc(errLen + 1);
if (self->errorMsg == NULL) {
*iCode = 0;
return;
}
strncpy(self->errorMsg, error, (size_t)errLen);
self->errorMsg[errLen] = '\0';
}
/** \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:
case DISCONNECTED:
return MOTFAIL;
case POSFAULT:
self->faultPending = false;
return MOTREDO;
case STATEERROR:
/* recover state error */
DMC_ClearTimer(self);
change_state(self, DMCState_Init);
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,"verbose") == 0) {
*fValue = self->verbose;
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(strcasecmp(name,"rotary_count") == 0) {
*fValue = self->rotary_count;
return 1;
}
if(strcasecmp(name,"rotary_bits") == 0) {
*fValue = self->rotary_bits;
return 1;
}
if(strcasecmp(name,"motorPollFast") == 0) {
*fValue = self->motorPollFast;
return 1;
}
if(strcasecmp(name,"motorPollSlow") == 0) {
*fValue = self->motorPollSlow;
return 1;
}
if(strcasecmp(name,"airPollTimer") == 0) {
*fValue = self->airPollTimer;
return 1;
}
if(strcasecmp(name,"posit_count") == 0) {
*fValue = self->posit_count;
return 1;
}
if(strcasecmp(name,"motorhome") == 0) {
*fValue = self->motorHome;
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;
}
if(strcasecmp(name,"creep_factor") == 0) {
*fValue = self->creep_factor;
return 1;
}
if(strcasecmp(name,"creep_substep") == 0) {
*fValue = self->creep_substep;
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;
/* 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;
}
}
/* Verbose, managers only */
if(strcasecmp(name,"verbose") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->verbose = 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;
}
}
/* Set creep_factor */
if (strcasecmp(name,"creep_factor") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
self->creep_factor = fabs(newValue);
if (self->creep_factor < 0)
self->creep_factor = -self->creep_factor;
if (self->creep_factor <= 0.01)
self->creep_factor = 0.01;
if (self->creep_factor >= 0.99)
self->creep_factor = 0.99;
return 1;
}
}
/* Set creep_substep */
if (strcasecmp(name,"creep_substep") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
self->creep_substep = fabs(newValue);
if (self->creep_substep < 0)
self->creep_substep = -self->creep_substep;
if (self->creep_substep <= 0.01)
self->creep_substep = 0.01;
if (self->creep_substep >= 0.99)
self->creep_substep = 0.99;
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 (!(MAGIC_MAN))
return 0;
self->fLower = newValue;
return 1;
}
if (strcasecmp("hardupperlim", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->fUpper = newValue;
return 1;
}
if (strcasecmp("stepsPerX", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->stepsPerX = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("cntsPerX", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->cntsPerX = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("bias_bias", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->bias_bias = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("bias_bits", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->bias_bits = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("rotary_count", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->rotary_count = newValue;
return 1;
}
if (self->abs_encoder && strcasecmp("rotary_bits", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->rotary_bits = newValue;
return 1;
}
if (strcasecmp("motorPollFast", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->motorPollFast = newValue;
return 1;
}
if (strcasecmp("motorPollSlow", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->motorPollSlow = newValue;
return 1;
}
if (strcasecmp("airPollTimer", name) == 0) {
/* Debug Managers only */
if (!(MAGIC_MAN))
return 0;
self->airPollTimer = 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(strcasecmp(name, "motorhome") == 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->motorHome = 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){
SCPrintf(pCon, eValue, "%s.part = %s\n", name, self->part);
SCPrintf(pCon, eValue, "%s.long_name = %s\n", name, self->long_name);
SCPrintf(pCon, eValue, "%s.axis = %c\n", name, self->axisLabel);
SCPrintf(pCon, eValue, "%s.legacy_fsm = %s\n", name, self->legacy_fsm ? "ON" : "OFF");
if (self->encoderAxis) {
SCPrintf(pCon, eValue, "%s.encoderAxis = %c\n", name, self->encoderAxis);
}
SCPrintf(pCon, eValue, "%s.units = %s\n", name, self->units);
if (self->asyncUnit) {
mkChannel* sock = AsyncUnitGetSocket(self->asyncUnit);
if (sock) {
char addr[80];
int port = ntohs(sock->adresse.sin_port);
strncpy(addr, inet_ntoa(sock->adresse.sin_addr), 80);
addr[79] = '\0';
SCPrintf(pCon, eValue, "%s.address = %s:%d\n", self->name, addr, port);
}
}
if (self->errorCode) {
int iCode;
char error[CMDLEN];
DMC2280Error(self, &iCode, error, CMDLEN);
SCPrintf(pCon, eValue, "%s.error = %d:%s", self->name, iCode, error);
}
if (self->posit_name_count > 0 && self->position_names) {
int i, k = 0;
char line[1320];
k += snprintf(line, sizeof(line) - k, "%s.position_names =", self->name);
for (i = 0; i < self->posit_name_count; ++i) {
k += snprintf(&line[k], sizeof(line) - k, " %s",
self->position_names[i]);
}
SCWrite(pCon, line, eValue);
}
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;
SCPrintf(pCon, eValue, "%s.home = %f\n", name, self->fHome);
SCPrintf(pCon, eValue, "%s.speed = %f\n", name, self->speed);
SCPrintf(pCon, eValue, "%s.maxSpeed = %f\n", name, self->maxSpeed);
SCPrintf(pCon, eValue, "%s.accel = %f\n", name, self->accel);
SCPrintf(pCon, eValue, "%s.maxAccel = %f\n", name, self->maxAccel);
SCPrintf(pCon, eValue, "%s.decel = %f\n", name, self->decel);
SCPrintf(pCon, eValue, "%s.maxDecel = %f\n", name, self->maxDecel);
SCPrintf(pCon, eValue, "%s.motOnDelay = %d\n", name, self->motOnDelay);
SCPrintf(pCon, eValue, "%s.motOffDelay = %d\n", name, self->motOffDelay);
SCPrintf(pCon, eValue, "%s.motorPollFast = %d\n", name, self->motorPollFast);
SCPrintf(pCon, eValue, "%s.motorPollSlow = %d\n", name, self->motorPollSlow);
SCPrintf(pCon, eValue, "%s.airPollTimer = %d\n", name, self->airPollTimer);
SCPrintf(pCon, eValue, "%s.Debug = %d\n", name, self->debug);
SCPrintf(pCon, eValue, "%s.Verbose = %d\n", name, self->verbose);
SCPrintf(pCon, eValue, "%s.Settle = %d\n", name, self->settle);
SCPrintf(pCon, eValue, "%s.Blockage_Check_Interval = %f\n", name, self->blockage_ckInterval);
SCPrintf(pCon, eValue, "%s.Blockage_Thresh = %f\n", name, self->blockage_thresh);
SCPrintf(pCon, eValue, "%s.Blockage_Ratio = %f\n", name, self->blockage_ratio);
SCPrintf(pCon, eValue, "%s.Blockage_Fail = %d\n", name, self->blockage_fail);
SCPrintf(pCon, eValue, "%s.Backlash_offset = %f\n", name, self->backlash_offset);
SCPrintf(pCon, eValue, "%s.Protocol = %d\n", name, self->protocol);
SCPrintf(pCon, eValue, "%s.absEncoder = %d\n", name, self->abs_encoder);
if (self->abs_encoder) {
SCPrintf(pCon, eValue, "%s.absEncHome = %d\n", name, self->absEncHome);
SCPrintf(pCon, eValue, "%s.cntsPerX = %f\n", name, self->cntsPerX);
SCPrintf(pCon, eValue, "%s.Creep_Offset = %f\n", name, self->creep_offset);
SCPrintf(pCon, eValue, "%s.Creep_Precision = %f\n", name, self->creep_precision);
SCPrintf(pCon, eValue, "%s.Creep_Factor = %f\n", name, self->creep_factor);
SCPrintf(pCon, eValue, "%s.Creep_Substep = %f\n", name, self->creep_substep);
}
if (self->posit_count > 0) {
int i;
SCPrintf(pCon, eValue, "%s.posit_count = %d\n", name,
self->posit_count);
for (i = 0; i < self->posit_count; ++i) {
SCPrintf(pCon, eValue, "%s.posit_%d = %lld\n", name, i + 1,
posit2count(self, i + 1));
}
}
SCPrintf(pCon, eValue, "%s.stepsPerX = %f\n", name, self->stepsPerX);
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, eLog);
/* TODO: disconnect */
change_state(self, DMCState_Disconnected);
break;
case AQU_RECONNECT:
snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
SICSLogWrite(line, eLog);
/* TODO: reconnect */
/* Reset the state machine */
DMC_ClearTimer(self);
change_state(self, DMCState_Disconnected);
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 */
/* Verbose: this motor driver logs exchanges */
if ((pPtr=getParam(pCon, interp, params,"verbose",_OPTIONAL)) == NULL)
pNew->verbose=0;
else {
sscanf(pPtr,"%d",&(pNew->verbose));
}
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));
if ((pPtr=getParam(pCon, interp, params,"motorPollFast",_OPTIONAL)) == NULL)
pNew->motorPollFast=MOTOR_POLL_FAST;
else
sscanf(pPtr,"%d",&(pNew->motOffDelay));
if ((pPtr=getParam(pCon, interp, params,"motorPollSlow",_OPTIONAL)) == NULL)
pNew->motorPollSlow=MOTOR_POLL_SLOW;
else
sscanf(pPtr,"%d",&(pNew->motOffDelay));
if ((pPtr=getParam(pCon, interp, params,"airPollTimer",_OPTIONAL)) == NULL)
pNew->airPollTimer=AIR_POLL_TIMER;
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;
}
/* CREEP_FACTOR: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_factor",_OPTIONAL)) == NULL)
pNew->creep_factor = 0.5;
else {
sscanf(pPtr, "%f", &(pNew->creep_factor));
if (pNew->creep_factor < 0)
pNew->creep_factor = -pNew->creep_factor;
if (pNew->creep_factor <= 0.01)
pNew->creep_factor = 0.01;
if (pNew->creep_factor >= 0.99)
pNew->creep_factor = 0.99;
}
/* CREEP_substep: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_substep",_OPTIONAL)) == NULL)
pNew->creep_substep = 0.1;
else {
sscanf(pPtr, "%f", &(pNew->creep_substep));
if (pNew->creep_substep < 0)
pNew->creep_substep = -pNew->creep_substep;
if (pNew->creep_substep <= 0.01)
pNew->creep_substep = 0.01;
if (pNew->creep_substep >= 0.99)
pNew->creep_substep = 0.99;
}
/* 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;
}
/* Rotary count for encoder - add this to value read */
if ((pPtr=getParam(pCon, interp, params,"rotary_count",_OPTIONAL)) == NULL)
pNew->rotary_count = 0;
else {
sscanf(pPtr, "%d", &(pNew->rotary_count));
}
/* Rotary size for encoder - mask this many bits */
if ((pPtr=getParam(pCon, interp, params,"rotary_bits",_OPTIONAL)) == NULL)
pNew->rotary_bits = 0;
else {
sscanf(pPtr, "%d", &(pNew->rotary_bits));
if (pNew->rotary_bits < 0)
pNew->rotary_bits = -pNew->rotary_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_Disconnected);
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("address", argv[1]) == 0) {
if (self->asyncUnit) {
mkChannel* sock = AsyncUnitGetSocket(self->asyncUnit);
if (sock) {
char line[132];
char addr[80];
int port = ntohs(sock->adresse.sin_port);
strncpy(addr, inet_ntoa(sock->adresse.sin_addr), 80);
addr[79] = '\0';
snprintf(line, 132, "%s.address = %s:%d\n", self->name, addr, port);
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("reset", argv[1]) == 0) {
/* Reset the state machine */
DMC_ClearTimer(self);
self->driver_status = HWIdle;
change_state(self, DMCState_Init);
while (self->myState == DMCState_Init) {
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("error", argv[1]) == 0) {
if (self->errorCode) {
int iCode;
char error[CMDLEN];
DMC2280Error(self, &iCode, error, CMDLEN);
SCPrintf(pCon, eValue, "%s.error = %d:%s", self->name, iCode, error);
} else {
SCPrintf(pCon, eValue, "%s.error = %d:%s", self->name, 0, "No error");
}
return 1;
}
else if(strcasecmp("oscillate_count", argv[1]) == 0) {
if (argc > 2)
self->oscillate_count = atoi(argv[2]);
else
SCPrintf(pCon, eValue, "%s.oscillate_count = %d", self->name, self->oscillate_count);
return 1;
}
else if (strcasecmp("oscillate_high", argv[1]) == 0) {
double fSoft = 0.0;
double fHard = 0.0;
if (argc > 2) {
if (parseNumber(argv[2], &fSoft) &&
getHardFromSoft(self, pCon, fSoft, &fHard))
self->oscillate_high = fSoft;
}
else
SCPrintf(pCon, eValue, "%s.oscillate_high = %g", self->name, self->oscillate_high);
return 1;
}
else if (strcasecmp("oscillate_low", argv[1]) == 0) {
double fSoft = 0.0;
double fHard = 0.0;
if (argc > 2) {
if (parseNumber(argv[2], &fSoft) &&
getHardFromSoft(self, pCon, fSoft, &fHard))
self->oscillate_low = fSoft;
}
else
SCPrintf(pCon, eValue, "%s.oscillate_low = %g", self->name, self->oscillate_low);
return 1;
}
else if (strcasecmp("oscillate_counter", argv[1]) == 0) {
SCPrintf(pCon, eValue, "%s.oscillate_counter = %d", self->name, self->oscillate_counter);
return 1;
}
else if(strcasecmp("osc", argv[1]) == 0 || strcasecmp("oscillate", argv[1]) == 0) {
if (argc > 2) {
if (strcasecmp("list", argv[2]) == 0) {
/* TODO: list the osc params */
SCPrintf(pCon, eValue, "%s.oscillate_count = %d", self->name, self->oscillate_count);
SCPrintf(pCon, eValue, "%s.oscillate_counter = %d", self->name, self->oscillate_counter);
SCPrintf(pCon, eValue, "%s.oscillate_low = %g", self->name, self->oscillate_low);
SCPrintf(pCon, eValue, "%s.oscillate_high = %g", self->name, self->oscillate_high);
return 1;
}
if (strcasecmp("start", argv[2]) == 0) {
float fFixed;
double fHard;
/* Check that it is OK to do this */
MotorGetPar(self->pMot, "fixed", &fFixed);
if (fFixed >= 0) {
SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name);
return 1;
}
if (!getHardFromSoft(self, pCon, self->oscillate_low, &fHard)) {
SCPrintf(pCon, eWarning, "Oscillate low point is in error");
return 1;
}
if (!getHardFromSoft(self, pCon, self->oscillate_high, &fHard)) {
SCPrintf(pCon, eWarning, "Oscillate high point is in error");
return 1;
}
/* choose the starting end, either as told or the nearest */
if (argc > 3 && strcasecmp("high", argv[3]) == 0)
self->oscillate_up = true;
else if (argc > 3 && strcasecmp("low", argv[3]) == 0)
self->oscillate_up = false;
else if (fabs(self->oscillate_high - motPosit(self)) < fabs(self->oscillate_low - motPosit(self)))
self->oscillate_up = true;
else
self->oscillate_up = false;
/* Set up the initial conditions */
self->doOscillate = true;
self->doReportMotion = true;
self->oscillate_init = true;
self->oscillate_counter = -1 /* allow for move-to-start */;
state_cmd_execute(self, CMD_RUN);
return 1;
}
if (strcasecmp("stop", argv[2]) == 0) {
self->doOscillate = false;
return 1;
}
if (strcasecmp("halt", argv[2]) == 0) {
self->doOscillate = false;
state_cmd_execute(self, CMD_HALT);
return 1;
}
} else {
SCPrintf(pCon, eValue, "%s.oscillation = %s", self->name, self->doOscillate ? "on" : "off");
return 1;
}
}
else if (strcasecmp("driver", argv[1]) == 0 && strcasecmp("run", argv[2]) == 0) {
if (argc > 2) {
float fFixed;
double fSoft;
double fHard;
char *endPtr;
errno = 0;
fSoft = strtod(argv[3], &endPtr);
if (endPtr == argv[3] || errno == ERANGE) {
self->errorCode = BADPAR;
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]);
return 1;
}
/* Reject Infinity and Not a Number (NaN) */
switch (fpclassify(fSoft)) {
case FP_ZERO:
case FP_NORMAL:
break;
default:
self->errorCode = BADPAR;
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]);
return 1;
}
MotorGetPar(self->pMot, "fixed", &fFixed);
if (fFixed >= 0) {
SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name);
} else if (getHardFromSoft(self, pCon, fSoft, &fHard)) {
self->fTarget = fHard;
SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard);
self->doOscillate = false;
self->doReportMotion = true;
state_cmd_execute(self, CMD_RUN);
}
}
else {
char *txt = NULL;
if (OKOK == self->driver_status)
txt = "OKOK";
else if (HWIdle == self->driver_status)
txt = "HWIdle";
else if (HWBusy == self->driver_status)
txt = "HWBusy";
else if (HWFault == self->driver_status)
txt = "HWFault";
else if (HWPosFault == self->driver_status)
txt = "HWPosFault";
else if (HWCrash == self->driver_status)
txt = "HWCrash";
else if (NOMEMORY == self->driver_status)
txt = "NOMEMORY";
else if (HWNoBeam == self->driver_status)
txt = "HWNoBeam";
else if (HWPause == self->driver_status)
txt = "HWPause";
else if (HWWarn == self->driver_status)
txt = "HWWarn";
else if (HWRedo == self->driver_status)
txt = "HWRedo";
else
txt = "HWUnknown";
SCPrintf(pCon, eValue, "%s.run = %s", self->name, txt);
}
return 1;
}
else if(strcasecmp("status", argv[1]) == 0) {
char *txt = NULL;
if (OKOK == self->driver_status)
txt = "OKOK";
else if (HWIdle == self->driver_status)
txt = "HWIdle";
else if (HWBusy == self->driver_status)
txt = "HWBusy";
else if (HWFault == self->driver_status)
txt = "HWFault";
else if (HWPosFault == self->driver_status)
txt = "HWPosFault";
else if (HWCrash == self->driver_status)
txt = "HWCrash";
else if (NOMEMORY == self->driver_status)
txt = "NOMEMORY";
else if (HWNoBeam == self->driver_status)
txt = "HWNoBeam";
else if (HWPause == self->driver_status)
txt = "HWPause";
else if (HWWarn == self->driver_status)
txt = "HWWarn";
else if (HWRedo == self->driver_status)
txt = "HWRedo";
else
txt = "HWUnknown";
SCPrintf(pCon, eValue, "%s.status = %s", self->name, txt);
return 1;
}
else if(strcasecmp("halt", argv[1]) == 0) {
state_cmd_execute(self, CMD_HALT);
return 1;
} else if(self->abs_encoder && strcasecmp("tracking", argv[1]) == 0) {
if (argc < 3) {
SCPrintf(pCon, eValue, "%s.tracking %s", self->name, self->doTracking ? "ON" : "OFF");
return 1;
}
if (strcasecmp(argv[2], "on") == 0) {
self->doTracking = true;
SCPrintf(pCon, eValue, "%s.tracking %s", self->name, self->doTracking ? "ON" : "OFF");
} else if (strcasecmp(argv[2], "off") == 0) {
self->doTracking = false;
SCPrintf(pCon, eValue, "%s.tracking %s", self->name, self->doTracking ? "ON" : "OFF");
} else if (strcasecmp(argv[2], "clear") == 0) {
TrackingRelease(self);
SCSendOK(pCon);
} else if (strcasecmp(argv[2], "stats") == 0) {
TrackEntryBuffer *bp;
TrackEntry *ep;
int i;
double S_x = 0.0;
double S_y = 0.0;
double S_xx = 0.0;
double S_yy = 0.0;
double S_xy = 0.0;
double S_n = 0.0;
double S_m = 0.0;
double S_b = 0.0;
double S_r = 0.0;
for (bp = self->trackHead; bp; bp = bp->nextBuff) {
for (i = 0; i < bp->index; i++) {
ep = &bp->buff[i];
/* TODO */
S_x += (double) ep->Counts;
S_y += (double) ep->Steps;
S_xy += (double) ep->Counts * (double) ep->Steps;
S_xx += (double) ep->Counts * (double) ep->Counts;
S_yy += (double) ep->Steps * (double) ep->Steps;
S_n += (double) 1;
}
}
S_m = S_n * S_xy - S_x * S_y;
S_m /= S_n * S_xx - S_x * S_x;
S_b = S_y / S_n - S_m * S_x / S_n;
S_r = S_n * S_xy - S_x * S_y;
S_r /= sqrt((S_n * S_xx - S_x * S_x)
* (S_n * S_yy - S_y * S_y));
SCPrintf(pCon, eValue, "Motor=%s stats: n=%.0f, m=%f, b=%f, r=%f, stepsPerX=%f",
self->name, S_n, S_m, S_b, S_r, self->cntsPerX * S_m);
} else if (strcasecmp(argv[2], "save") == 0) {
TrackEntryBuffer *bp;
TrackEntry *ep;
int i;
FILE *fp;
char *rp;
if (argc < 4) {
SCPrintf(pCon, eError, "Motor %s, %s subcommand %s needs filename",
argv[0], argv[1], argv[2]);
return 1;
}
rp = realpath(argv[3], NULL);
fp = fopen(argv[3], "w");
if (NULL == fp) {
SCPrintf(pCon, eError, "Motor %s, %s subcommand %s failed to open %s",
argv[0], argv[1], argv[2], rp ? rp : argv[3]);
if (rp)
free(rp);
return 1;
}
fprintf(fp, "Timestamp,MotorPosition,MotorSteps,EncoderCounts\n");
for (bp = self->trackHead; bp; bp = bp->nextBuff) {
for (i = 0; i < bp->index; i++) {
ep = &bp->buff[i];
/* TODO timestamp in HH:MM:SS.cc, softPosition maybe ??*/
fprintf(fp, "%.6f,%.6f,%d,%d\n", ep->Timestamp, ep->Position, ep->Steps, ep->Counts);
}
}
fclose(fp);
SCPrintf(pCon, eLog, "Motor %s, tracking data saved to %s",
argv[0], rp ? rp : argv[3]);
if (rp)
free(rp);
} else {
SCPrintf(pCon, eError, "Motor %s, %s subcommand %s not recognized",
argv[0], argv[1], argv[2]);
}
return 1;
} else if(self->abs_encoder && strcasecmp("stats", argv[1]) == 0) {
SCPrintf(pCon, eValue, "%s.stats=%s samples=%.0f, slope=%f, intercept=%f, r=%f, stepsPerX=%f",
self->name,
self->doStats ? "ON" : "OFF",
self->S_n,
self->S_m,
self->S_b,
self->S_r,
self->cntsPerX * self->S_m);
if (argc > 2) {
if (strcasecmp(argv[2], "reset") == 0) {
/* reset the stats */
self->S_x = 0.0;
self->S_y = 0.0;
self->S_xx = 0.0;
self->S_yy = 0.0;
self->S_xy = 0.0;
self->S_n = 0.0;
self->S_m = 0.0;
self->S_b = 0.0;
self->S_r = 0.0;
} else if (strcasecmp(argv[2], "on") == 0) {
self->doStats = true;
} else if (strcasecmp(argv[2], "off") == 0) {
self->doStats = false;
}
}
return 1;
}
else if(strcasecmp("minPosition", argv[1]) == 0) {
char line[132];
snprintf(line, 132, "%s.minPosition = %.6f",
self->name,
self->minPosition);
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("maxPosition", argv[1]) == 0) {
char line[132];
snprintf(line, 132, "%s.maxPosition = %.6f",
self->name,
self->maxPosition);
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("stepCount", argv[1]) == 0) {
char line[132];
snprintf(line, 132, "%s.stepCount = %d",
self->name,
self->stepCount);
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("moveTime", argv[1]) == 0) {
char line[132];
snprintf(line, 132, "%s.moveTime = %.6f",
self->name,
self->moveStopTime - self->moveStartTime);
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("legacy_fsm", argv[1]) == 0) {
if (argc > 2) {
if (strcasecmp("on", argv[2]) == 0) {
self->legacy_fsm = true;
SCWrite(pCon, "LEGACY_FSM ON", eValue);
}
else {
self->legacy_fsm = false;
SCWrite(pCon, "LEGACY_FSM OFF", eValue);
}
} else {
SCPrintf(pCon, eValue, "%s.LEGACY_FSM = %s",
self->name,
self->legacy_fsm ? "ON" : "OFF");
}
return 1;
}
else if(strcasecmp("trace", argv[1]) == 0) {
if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
self->trace = SCCopyConnection(pCon);
SCWrite(pCon, "TRACE ON", eValue);
}
else {
if (self->trace) {
SCDeleteConnection(self->trace);
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("hard2soft", argv[1]) == 0) {
double fHard, fSoft;
if (parseNumber(argv[2], &fHard)) {
getSoftFromHard(self, pCon, fHard, &fSoft);
SCPrintf(pCon, eValue, "%s.hard2soft = %g", self->name, fSoft);
}
return 1;
}
else if(strcasecmp("soft2hard", argv[1]) == 0) {
double fHard, fSoft;
if (parseNumber(argv[2], &fSoft)) {
getHardFromSoft(self, pCon, fSoft, &fHard);
SCPrintf(pCon, eValue, "%s.soft2hard = %g", self->name, fHard);
}
return 1;
}
else if(self->abs_encoder && 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(self->abs_encoder && 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("legacy_fsm", argv[1]) == 0) {
SCPrintf(pCon, eValue, "%s.legacy_fsm = %d", self->name, self->legacy_fsm);
return 1;
}
else if(strcasecmp("posit_count", argv[1]) == 0) {
SCPrintf(pCon, eValue, "%s.posit_count = %d", self->name, self->posit_count);
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 ((MAGIC_MAN)) {
if (self->positions) {
free(self->positions);
self->positions = NULL;
self->posit_count = 0;
}
if (self->posit_count > 0) {
self->posit_count = 0;
}
if (argc > 2 && 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;
}
}
else if(strcasecmp("position_names", argv[1]) == 0) {
if (argc == 2) {
int i, k = 0;
char line[1320];
k += snprintf(line, sizeof(line) - k, "%s.position_names =", self->name);
for (i = 0; i < self->posit_name_count; ++i) {
k += snprintf(&line[k], sizeof(line) - k, " %s",
self->position_names[i]);
}
SCWrite(pCon, line, eValue);
return 1;
}
if ((MAGIC_MAN)) {
if (self->position_names) {
int i;
for (i = 0; i < self->posit_name_count; ++i) {
free(self->position_names[i]);
self->position_names[i] = NULL;
}
free(self->position_names);
self->position_names = NULL;
self->posit_name_count = 0;
}
if (self->posit_name_count > 0) {
self->posit_name_count = 0;
}
if (argc > 2 && strcasecmp("erase", argv[2]) == 0) {
char line[132];
snprintf(line, 132, "%s.posit_name_count = %d", self->name, self->posit_name_count);
SCWrite(pCon, line, eValue);
return 1;
}
self->posit_name_count = argc - 2;
self->position_names = malloc(self->posit_name_count * sizeof(*self->position_names));
int i;
for (i = 0; i < self->posit_name_count; ++i) {
self->position_names[i] = strdup(argv[i + 2]);
}
return 1;
}
}
if (self->abs_encoder && strcasecmp("absEncHome", argv[1]) == 0) {
if (argc > 2) {
/* Debug Managers only */
if (!(MAGIC_MAN))
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 = arg2posit(self, argv[2]);
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("posit2hard", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = arg2posit(self, argv[2]);
snprintf(line, 132, "%s.posit2hard = %f",
self->name,
posit2hard(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("posit2soft", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = arg2posit(self, argv[2]);
snprintf(line, 132, "%s.posit2soft = %f",
self->name,
posit2soft(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("posit2unit", argv[1]) == 0) { /* Legacy */
char line[132];
if (argc > 2) {
double target;
target = arg2posit(self, argv[2]);
snprintf(line, 132, "%s.posit2unit = %f",
self->name,
posit2hard(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