6077 lines
186 KiB
C
6077 lines
186 KiB
C
/** \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"
|
|
|
|
#define UNITSLEN 256
|
|
#define TEXTPARLEN 1024
|
|
#define CMDLEN 1024
|
|
#define STATE_TRACE (200)
|
|
|
|
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 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;
|
|
double* positions;
|
|
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 */
|
|
char ao_id[256];
|
|
bool legacy_fsm; /**< flag for legacy_fsm new code */
|
|
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_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;
|
|
|
|
MotorGetPar(self->pMot, "softlowerlim", &fLower);
|
|
MotorGetPar(self->pMot, "softupperlim", &fUpper);
|
|
MotorGetPar(self->pMot, "softzero", &fZero);
|
|
MotorGetPar(self->pMot, "sign", &fSign);
|
|
if (fSoft > fUpper) {
|
|
if (pCon)
|
|
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
|
|
fSoft, fUpper, self->name);
|
|
return false;
|
|
}
|
|
if (fSoft < fLower) {
|
|
if (pCon)
|
|
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
|
|
fSoft, fLower, self->name);
|
|
return false;
|
|
}
|
|
fHard = fSoft;
|
|
fHard += fZero;
|
|
fHard *= fSign;
|
|
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);
|
|
return 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);
|
|
return false;
|
|
}
|
|
*pHard = fHard;
|
|
return true;
|
|
}
|
|
|
|
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, eStatus);
|
|
return -1;
|
|
}
|
|
/* distance of target from home in units */
|
|
absolute = (double) target - self->fHome;
|
|
/* convert to encoder counts */
|
|
absolute *= (double) self->cntsPerX;
|
|
/* add home position in counts */
|
|
absolute += (double) self->absEncHome;
|
|
if (absolute > 0)
|
|
result = (int) (absolute + 0.5);
|
|
else if (absolute < 0)
|
|
result = (int) (absolute - 0.5);
|
|
else
|
|
result = (int) absolute;
|
|
if (self->debug) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "unit2count Rounding %f to %lld", absolute, result);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
static double count2unit(pDMC2280Driv self, long long counts) {
|
|
double fPos;
|
|
if (self->abs_encoder == 0) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder",
|
|
self->name);
|
|
SICSLogWrite(line, eStatus);
|
|
return -1;
|
|
}
|
|
fPos = (counts - self->absEncHome) / self->cntsPerX + self->fHome;
|
|
return fPos;
|
|
}
|
|
|
|
static double unit2posit(pDMC2280Driv self, double target) {
|
|
int i, j;
|
|
double fPos;
|
|
if (!check_positions(self))
|
|
return 0;
|
|
|
|
if (self->positions[0] < self->positions[1]) {
|
|
for (i = 1; i < self->posit_count - 1; ++i) {
|
|
if (target < self->positions[i]) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
for (i = 1; i < self->posit_count - 1; ++i) {
|
|
if (target > self->positions[i]) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
--i;
|
|
j = i + 1;
|
|
fPos = (double) j +
|
|
((double)target - self->positions[i]) /
|
|
((double)self->positions[j] - self->positions[i]);
|
|
return fPos;
|
|
}
|
|
|
|
static double posit2unit(pDMC2280Driv self, double target) {
|
|
double result;
|
|
int i = ((int) target) - 1;
|
|
if (i < 0) {
|
|
i = 0;
|
|
} else if (i > self->posit_count - 2) {
|
|
i = self->posit_count - 2;
|
|
}
|
|
result = (double)self->positions[i]
|
|
+ ((double)target - ((double)i + 1)) *
|
|
((double)self->positions[i + 1] - self->positions[i]);
|
|
return result;
|
|
}
|
|
|
|
static long long posit2count(pDMC2280Driv self, double target) {
|
|
return unit2count(self, posit2unit(self, target));
|
|
}
|
|
|
|
static double count2posit(pDMC2280Driv self, long long counts) {
|
|
return unit2posit(self, count2unit(self, counts));
|
|
}
|
|
|
|
/** \brief Convert axis speed in physical units to
|
|
* motor speed in steps/sec.
|
|
*
|
|
* \param self (r) provides access to the motor's data structure
|
|
* \param speed in physical units, eg mm/sec degrees/sec
|
|
* \return the speed in motor steps/sec
|
|
*/
|
|
static int motSpeed(pDMC2280Driv self, double axisSpeed) {
|
|
int speed;
|
|
speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5);
|
|
return speed;
|
|
}
|
|
|
|
/** \brief Convert axis acceleration in physical units to
|
|
* motor acceleration in steps/sec^2
|
|
*
|
|
* \param self (r) provides access to the motor's data structure
|
|
* \param acceleration in physical units, eg mm/sec^2 degrees/sec^2
|
|
* \return the acceleration in motor steps/sec^2
|
|
*/
|
|
static int motAccel(pDMC2280Driv self, double axisAccel) {
|
|
int accel;
|
|
accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5);
|
|
return accel;
|
|
}
|
|
|
|
/** \brief Convert axis deceleration in physical units to
|
|
* motor deceleration in steps/sec^2
|
|
*
|
|
* \param self (r) provides access to the motor's data structure
|
|
* \param deceleration in physical units, eg mm/sec^2 degrees/sec^2
|
|
* \return the deceleration in motor steps/sec^2
|
|
*/
|
|
static int motDecel(pDMC2280Driv self, double axisDecel) {
|
|
int decel;
|
|
decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5);
|
|
return decel;
|
|
}
|
|
|
|
/** \brief Convert motor position to physical units
|
|
* using the current motor steps or encoder counts
|
|
*
|
|
* \param self (r) provides access to the motor's data structure
|
|
* \return the motor position in physical units
|
|
*/
|
|
static double motPosit(pDMC2280Driv self) {
|
|
double fPos, curr, home, counts;
|
|
if (self->abs_encoder) {
|
|
curr = self->currCounts;
|
|
home = self->absEncHome;
|
|
counts = self->cntsPerX;
|
|
} else {
|
|
curr = self->currSteps;
|
|
home = self->motorHome;
|
|
counts = self->stepsPerX;
|
|
}
|
|
fPos = (curr - home) / counts + self->fHome;
|
|
return fPos;
|
|
}
|
|
|
|
|
|
/** \brief Convert motor target in physical units to
|
|
* motor 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->debug) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "motAbsol Rounding %f to %d", absolute, result);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/** \brief Convert motor target in physical units to
|
|
* motor 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->debug) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "Motor=%s creep stopped, stepcount = %d",
|
|
self->name, self->stepCount);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
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->debug) {
|
|
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, eStatus);
|
|
}
|
|
/*
|
|
* 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->debug) {
|
|
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, eStatus);
|
|
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, eStatus);
|
|
snprintf(line, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d",
|
|
self->currSteps, target_steps, offset, self->currSteps + offset);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
/*
|
|
* 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", eStatus);
|
|
for (i = myCmd->out_len; i <= myCmd->inp_idx; ++i) {
|
|
myCmd->inp_buf[i - myCmd->out_len] = myCmd->inp_buf[i];
|
|
}
|
|
}
|
|
iRet = 0;
|
|
}
|
|
if (iRet == 0) { /* end of command */
|
|
return AQU_POP_CMD;
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
static int DMC_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
|
|
if (event == AQU_TIMEOUT) {
|
|
/* handle command timeout */
|
|
pTxn->txn_status = ATX_TIMEOUT;
|
|
return AQU_POP_CMD;
|
|
}
|
|
return AQU_POP_CMD;
|
|
}
|
|
|
|
static int DMC_SendReq(pDMC2280Driv self, char* command) {
|
|
self->waitResponse = true;
|
|
return AsyncUnitSendTxn(self->asyncUnit,
|
|
command, strlen(command),
|
|
state_msg_callback, self, CMDLEN);
|
|
}
|
|
|
|
/**
|
|
* \brief SendCallback is the callback for the general command.
|
|
*/
|
|
static int SendCallback(pAsyncTxn pCmd) {
|
|
char* cmnd = pCmd->out_buf;
|
|
char* resp = pCmd->inp_buf;
|
|
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
|
|
|
|
if (pCmd->txn_status == ATX_TIMEOUT) {
|
|
if (self->debug) {
|
|
SICSLogWrite(pCmd->out_buf, eStatus);
|
|
SICSLogWrite("<TIMEOUT>", eStatus);
|
|
}
|
|
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
|
|
self->errorCode = MOTCMDTMO;
|
|
}
|
|
else {
|
|
switch (resp[0]) {
|
|
case ':': /* prompt */
|
|
case ' ': /* leading blank */
|
|
case '-': /* leading minus sign */
|
|
if (self->debug) {
|
|
SICSLogWrite(cmnd, eStatus);
|
|
SICSLogWrite(resp, eStatus);
|
|
}
|
|
break;
|
|
case '?':
|
|
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
|
|
strncpy(self->dmc2280Error, &resp[1], CMDLEN);
|
|
SICSLogWrite("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->debug) {
|
|
SICSLogWrite(cmnd, eStatus);
|
|
SICSLogWrite(resp, eStatus);
|
|
}
|
|
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->debug)
|
|
SICSLogWrite(cmd, eStatus);
|
|
if (status == -1)
|
|
self->errorCode = MOTCMDTMO;
|
|
else
|
|
self->errorCode = BADUNKNOWN;
|
|
return FAILURE;
|
|
}
|
|
|
|
switch (reply[0]) {
|
|
case ':': /* prompt */
|
|
if (self->debug) {
|
|
SICSLogWrite(cmd, eStatus);
|
|
SICSLogWrite(reply, eStatus);
|
|
}
|
|
return SUCCESS;
|
|
case ' ': /* leading blank */
|
|
case '-': /* leading minus sign */
|
|
if (self->debug) {
|
|
SICSLogWrite(cmd, eStatus);
|
|
SICSLogWrite(reply, eStatus);
|
|
}
|
|
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->debug) {
|
|
SICSLogWrite(cmd, eStatus);
|
|
SICSLogWrite(reply, eStatus);
|
|
}
|
|
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->debug || bNotYet == false) {
|
|
snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f",
|
|
self->name, ratio_obs, ratio_exp, cmp);
|
|
SICSLogWrite(msg, eError);
|
|
snprintf(msg, sizeof(msg), "steps=%d-%d, counts=%d-%d, exp=%f/%f",
|
|
self->currSteps, self->lastSteps,
|
|
self->currCounts, self->lastCounts,
|
|
self->stepsPerX, self->cntsPerX);
|
|
SICSLogWrite(msg, eError);
|
|
}
|
|
if (bNotYet == false) {
|
|
if (self->blockage_fail)
|
|
iRet = 0;
|
|
else
|
|
set_lastMotion(self, self->currSteps, self->currCounts);
|
|
}
|
|
} else {
|
|
if (self->debug) {
|
|
char msg[132];
|
|
double cmp = ratio_cmp;
|
|
if (fabs(cmp) > 0 && fabs(cmp) < 1)
|
|
cmp = 1.0 / cmp;
|
|
snprintf(msg, sizeof(msg), "Motor %s pass: obs=%f, exp=%f, cmp=%f",
|
|
self->name, ratio_obs, ratio_exp, cmp);
|
|
SICSLogWrite(msg, eError);
|
|
snprintf(msg, sizeof(msg), "steps=%d-%d, counts=%d-%d, exp=%f/%f",
|
|
self->currSteps, self->lastSteps,
|
|
self->currCounts, self->lastCounts,
|
|
self->stepsPerX, self->cntsPerX);
|
|
SICSLogWrite(msg, eError);
|
|
}
|
|
if (bNotYet == false) {
|
|
set_lastMotion(self, self->currSteps, self->currCounts);
|
|
}
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
/**
|
|
* \brief test if there is a variable of the form VVVx where VVV is the
|
|
* variable name and x is the axis label.
|
|
*
|
|
* The variable name is contstructed by appending the axis label to the given
|
|
* name and looking for the construct "VVVx=" at the start of a line in the
|
|
* variable list. This will reject finds which are just the last part of a
|
|
* variable like "ZVVV="
|
|
*
|
|
* \param self motor data
|
|
* \param vars string returned from LV command
|
|
* \param VVV part of variable name
|
|
*
|
|
* \return true if found else false
|
|
*/
|
|
static bool has_var(pDMC2280Driv self, const char* vars, const char* name) {
|
|
char* p;
|
|
char tmp[20];
|
|
snprintf(tmp, 20, "%s=", name);
|
|
p = strstr(vars, tmp);
|
|
if (p == NULL) /* not found at all */
|
|
return false;
|
|
if (p == vars) /* found at start of first line */
|
|
return true;
|
|
if (p[-1] == '\n') /* found at start of another line */
|
|
return true;
|
|
return false; /* found partial (ZVVVx=) only */
|
|
}
|
|
|
|
static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
|
|
char tmp[20];
|
|
snprintf(tmp, 20, "%s%c", name, self->axisLabel);
|
|
return has_var(self, vars, tmp);
|
|
}
|
|
|
|
/**
|
|
* \brief request standard state information for the motor and controller
|
|
*
|
|
* \param self access to the drive data
|
|
*/
|
|
static int cmdStatus(pDMC2280Driv self) {
|
|
char cmd[CMDLEN];
|
|
char encoder = self->axisLabel;
|
|
if (self->encoderAxis && !(self->variables & VAR_ENC))
|
|
encoder = self->encoderAxis;
|
|
|
|
|
|
/* Use POSx, ENCx, RUNx, SWIx if it has these variables */
|
|
snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,%s%c,_TI0,_TI1,_XQ0",
|
|
(self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel,
|
|
(self->variables & VAR_ENC) ? "ENC" : "_TP", encoder,
|
|
(self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel,
|
|
(self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel,
|
|
(self->variables & VAR_STP) ? "STP" : "_SC", self->axisLabel);
|
|
return DMC_SendReq(self, cmd);
|
|
}
|
|
|
|
static int cmdVars(pDMC2280Driv self) {
|
|
char cmd[CMDLEN];
|
|
snprintf(cmd, CMDLEN, "LV");
|
|
return DMC_SendReq(self, cmd);
|
|
}
|
|
|
|
/*
|
|
* 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,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,
|
|
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->debug) && self->currFlags != iFlags) {
|
|
char line[CMDLEN];
|
|
char *sw;
|
|
if ((self->currFlags & (8 + 4)) != (iFlags & (8 + 4))) {
|
|
switch (iFlags & (8 + 4)) {
|
|
case (8 + 4): sw = "none"; break;
|
|
case (8 ): sw = "reverse"; break;
|
|
case ( 4): sw = "forward"; break;
|
|
case ( 0 ): sw = "both"; break;
|
|
}
|
|
snprintf(line, CMDLEN, "Motor %s limits: %s", self->name, sw);
|
|
if (trace_switches)
|
|
ServerWriteGlobal(line, eStatus);
|
|
else
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
if ((self->currFlags & (32)) != (iFlags & (32))) {
|
|
if (iFlags & (32)) {
|
|
sw = "off";
|
|
}
|
|
else
|
|
{
|
|
sw = "on";
|
|
}
|
|
snprintf(line, CMDLEN, "Motor %s motor: %s", self->name, sw);
|
|
if (trace_switches)
|
|
ServerWriteGlobal(line, eStatus);
|
|
else
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
if ((self->currFlags & (128)) != (iFlags & (128))) {
|
|
if (iFlags & (128)) {
|
|
sw = "moving";
|
|
}
|
|
else
|
|
{
|
|
sw = "stopped";
|
|
}
|
|
snprintf(line, CMDLEN, "Motor %s motor: %s", self->name, sw);
|
|
if (trace_switches)
|
|
ServerWriteGlobal(line, eStatus);
|
|
else
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
}
|
|
self->currFlags = iFlags;
|
|
self->currSteps = iSteps;
|
|
if (self->bias_bits > 0)
|
|
iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1);
|
|
self->currCounts = iCounts;
|
|
self->currPosition = motPosit(self);
|
|
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;
|
|
}
|
|
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(eStatus, &tv, "Motor: %s, state_trace_prn trace history listing start", self->name);
|
|
SICSLogWriteTime(line, eStatus, 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(eStatus, &tv, "Motor: %s, state_trace_prn trace history listing end (%d entries)", self->name, lines_printed);
|
|
#else
|
|
SICSLogPrintf(eStatus, "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->debug || 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->debug)
|
|
SICSLogWrite(lp, eStatus);
|
|
if (self->trace)
|
|
SCWrite(self->trace, lp, eStatus);
|
|
}
|
|
#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->debug || 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->debug || 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->debug || 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 */
|
|
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->debug || 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->debug)
|
|
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->debug) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n",
|
|
self->name,
|
|
self->currPosition,
|
|
self->fPreseek,
|
|
self->fTarget);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
self->creep_val = 0;
|
|
absolute = motCreep(self, target);
|
|
cmdPosition(self, absolute);
|
|
self->subState = 2;
|
|
/* save pre-motion values for logging */
|
|
self->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->debug) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
|
|
self->name, self->stepCount);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
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:
|
|
self->myNextState = NULL;
|
|
/* handle termination conditions (e.g. count) */
|
|
if (self->oscillate_count > 0 && self->oscillate_counter >= self->oscillate_count)
|
|
self->doOscillate = false;
|
|
if (!self->doOscillate) {
|
|
(void) report_motion(self, eReportFinal);
|
|
change_state(self, DMCState_OffTimer); /* TODO: check this is OK */
|
|
return;
|
|
}
|
|
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;
|
|
}
|
|
/* 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;
|
|
if (!self->oscillate_init)
|
|
self->oscillate_counter++;
|
|
self->oscillate_init = false;
|
|
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->debug*/ || 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->debug*/) {
|
|
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, eStatus);
|
|
}
|
|
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, eStatus);
|
|
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, eStatus);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
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->debug) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
|
|
self->name, self->stepCount);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
self->preseek = 0;
|
|
absolute = motCreep(self, target);
|
|
}
|
|
else if (self->preseek) {
|
|
absolute = motCreep(self, target);
|
|
if (self->debug) {
|
|
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, eStatus);
|
|
}
|
|
}
|
|
else {
|
|
/* change of direction, reset motion check */
|
|
set_lastMotion(self, self->currSteps, self->currCounts);
|
|
absolute = motCreep(self, target);
|
|
if (self->debug) {
|
|
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, eStatus);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
absolute = motCreep(self, target);
|
|
|
|
if (self->debug && self->preseek) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n",
|
|
self->name,
|
|
self->currPosition,
|
|
self->fPreseek,
|
|
self->fTarget);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
/*
|
|
* If we are still iterating, continue
|
|
*/
|
|
if (self->preseek) {
|
|
if (absolute == self->currSteps) {
|
|
if (self->debug) {
|
|
char line[CMDLEN];
|
|
snprintf(line, CMDLEN, "Motor=%s motion stopped, absolute = %d",
|
|
self->name, absolute);
|
|
SICSLogWrite(line, eStatus);
|
|
}
|
|
self->preseek = 0;
|
|
}
|
|
else {
|
|
if (self->settle
|
|
&& self->subState == 2
|
|
&& abs(absolute - self->currSteps) < 100) {
|
|
self->subState = 4;
|
|
DMC_SetTimer(self, self->settle);
|
|
return;
|
|
}
|
|
cmdPosition(self, absolute);
|
|
self->subState = 3;
|
|
return;
|
|
}
|
|
}
|
|
/*
|
|
* We are no longer iterating, so fall through
|
|
*/
|
|
}
|
|
/* handle settle time */
|
|
if (self->settle && self->time_settle_done.tv_sec == 0) {
|
|
gettimeofday(&self->time_settle_done, NULL);
|
|
self->time_settle_done.tv_sec += self->settle;
|
|
self->subState = 2;
|
|
DMC_SetTimer(self, self->settle);
|
|
return;
|
|
}
|
|
/* We get here when the motor stops normally
|
|
*/
|
|
if (true /*self->debug*/) {
|
|
double units = self->currPosition - self->origPosition;
|
|
long int steps = self->currSteps - self->origSteps;
|
|
long int counts = self->currCounts - self->origCounts;
|
|
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, eStatus);
|
|
}
|
|
change_state(self, DMCState_OffTimer);
|
|
return;
|
|
}
|
|
} while (0);
|
|
break;
|
|
case eCommandEvent:
|
|
switch (event->event.cmd.cmd_type) {
|
|
case CMD_RUN:
|
|
/* TODO: FIXME RUN command while running */
|
|
if (self->driver_status == HWIdle)
|
|
self->driver_status = HWBusy;
|
|
self->run_flag = 1;
|
|
if (self->waitResponse == false) {
|
|
change_state(self, DMCState_MotorHalt);
|
|
}
|
|
return;
|
|
case CMD_HALT:
|
|
/* handle halt command, send message */
|
|
self->run_flag = -1;
|
|
if (self->waitResponse == false) {
|
|
change_state(self, DMCState_MotorHalt);
|
|
}
|
|
return;
|
|
}
|
|
break;
|
|
case eTimeoutEvent:
|
|
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
|
|
self->errorCode = MOTCMDTMO;
|
|
self->driver_status = HWFault;
|
|
change_state(self, DMCState_MotorHalt);
|
|
return;
|
|
}
|
|
unhandled_event(self, event);
|
|
self->errorCode = STATEERROR;
|
|
change_state(self, DMCState_Error);
|
|
return;
|
|
}
|
|
|
|
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
|
|
switch (event->event_type) {
|
|
case eStateEvent:
|
|
DMC_ClearTimer(self);
|
|
cmdHalt(self);
|
|
/* Note fall through */
|
|
case eTimerEvent:
|
|
cmdStatus(self);
|
|
self->subState = 1;
|
|
return;
|
|
case eMessageEvent:
|
|
do {
|
|
pAsyncTxn pCmd = event->event.msg.cmd;
|
|
if (self->subState == 1) { /* Status Response */
|
|
int iRet;
|
|
iRet = rspStatus(self, pCmd->inp_buf);
|
|
if (iRet == 0)
|
|
break;
|
|
if (self->moving) {
|
|
DMC_SetTimer(self, 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,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,"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;
|
|
}
|
|
}
|
|
|
|
/* 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 (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
self->fLower = newValue;
|
|
return 1;
|
|
}
|
|
if (strcasecmp("hardupperlim", name) == 0) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
self->fUpper = newValue;
|
|
return 1;
|
|
}
|
|
if (strcasecmp("stepsPerX", name) == 0) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
self->stepsPerX = newValue;
|
|
return 1;
|
|
}
|
|
if (self->abs_encoder && strcasecmp("cntsPerX", name) == 0) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
self->cntsPerX = newValue;
|
|
return 1;
|
|
}
|
|
if (self->abs_encoder && strcasecmp("bias_bias", name) == 0) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
self->bias_bias = newValue;
|
|
return 1;
|
|
}
|
|
if (self->abs_encoder && strcasecmp("bias_bits", name) == 0) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
self->bias_bits = newValue;
|
|
return 1;
|
|
}
|
|
if (strcasecmp("motorPollFast", name) == 0) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
self->motorPollFast = newValue;
|
|
return 1;
|
|
}
|
|
if (strcasecmp("motorPollSlow", name) == 0) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
self->motorPollSlow = newValue;
|
|
return 1;
|
|
}
|
|
if (strcasecmp("airPollTimer", name) == 0) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
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, eStatus, "%s.part = %s\n", name, self->part);
|
|
SCPrintf(pCon, eStatus, "%s.long_name = %s\n", name, self->long_name);
|
|
SCPrintf(pCon, eStatus, "%s.axis = %c\n", name, self->axisLabel);
|
|
SCPrintf(pCon, eStatus, "%s.legacy_fsm = %s\n", name, self->legacy_fsm ? "ON" : "OFF");
|
|
if (self->encoderAxis) {
|
|
SCPrintf(pCon, eStatus, "%s.encoderAxis = %c\n", name, self->encoderAxis);
|
|
}
|
|
SCPrintf(pCon, eStatus, "%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, eStatus, "%s.address = %s:%d\n", self->name, addr, port);
|
|
}
|
|
}
|
|
if (self->errorCode) {
|
|
int iCode;
|
|
char error[CMDLEN];
|
|
DMC2280Error(self, &iCode, error, CMDLEN);
|
|
SCPrintf(pCon, eStatus, "%s.error = %d:%s", self->name, iCode, error);
|
|
}
|
|
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, eStatus, "%s.home = %f\n", name, self->fHome);
|
|
SCPrintf(pCon, eStatus, "%s.speed = %f\n", name, self->speed);
|
|
SCPrintf(pCon, eStatus, "%s.maxSpeed = %f\n", name, self->maxSpeed);
|
|
SCPrintf(pCon, eStatus, "%s.accel = %f\n", name, self->accel);
|
|
SCPrintf(pCon, eStatus, "%s.maxAccel = %f\n", name, self->maxAccel);
|
|
SCPrintf(pCon, eStatus, "%s.decel = %f\n", name, self->decel);
|
|
SCPrintf(pCon, eStatus, "%s.maxDecel = %f\n", name, self->maxDecel);
|
|
SCPrintf(pCon, eStatus, "%s.motOnDelay = %d\n", name, self->motOnDelay);
|
|
SCPrintf(pCon, eStatus, "%s.motOffDelay = %d\n", name, self->motOffDelay);
|
|
SCPrintf(pCon, eStatus, "%s.motorPollFast = %d\n", name, self->motorPollFast);
|
|
SCPrintf(pCon, eStatus, "%s.motorPollSlow = %d\n", name, self->motorPollSlow);
|
|
SCPrintf(pCon, eStatus, "%s.airPollTimer = %d\n", name, self->airPollTimer);
|
|
SCPrintf(pCon, eStatus, "%s.Debug = %d\n", name, self->debug);
|
|
SCPrintf(pCon, eStatus, "%s.Settle = %d\n", name, self->settle);
|
|
SCPrintf(pCon, eStatus, "%s.Blockage_Check_Interval = %f\n", name, self->blockage_ckInterval);
|
|
SCPrintf(pCon, eStatus, "%s.Blockage_Thresh = %f\n", name, self->blockage_thresh);
|
|
SCPrintf(pCon, eStatus, "%s.Blockage_Ratio = %f\n", name, self->blockage_ratio);
|
|
SCPrintf(pCon, eStatus, "%s.Blockage_Fail = %d\n", name, self->blockage_fail);
|
|
SCPrintf(pCon, eStatus, "%s.Backlash_offset = %f\n", name, self->backlash_offset);
|
|
SCPrintf(pCon, eStatus, "%s.Protocol = %d\n", name, self->protocol);
|
|
SCPrintf(pCon, eStatus, "%s.absEncoder = %d\n", name, self->abs_encoder);
|
|
if (self->abs_encoder) {
|
|
SCPrintf(pCon, eStatus, "%s.absEncHome = %d\n", name, self->absEncHome);
|
|
SCPrintf(pCon, eStatus, "%s.cntsPerX = %f\n", name, self->cntsPerX);
|
|
SCPrintf(pCon, eStatus, "%s.Creep_Offset = %f\n", name, self->creep_offset);
|
|
SCPrintf(pCon, eStatus, "%s.Creep_Precision = %f\n", name, self->creep_precision);
|
|
SCPrintf(pCon, eStatus, "%s.Creep_Factor = %f\n", name, self->creep_factor);
|
|
SCPrintf(pCon, eStatus, "%s.Creep_Substep = %f\n", name, self->creep_substep);
|
|
}
|
|
if (self->posit_count > 0) {
|
|
int i;
|
|
SCPrintf(pCon, eStatus, "%s.posit_count = %d\n", name,
|
|
self->posit_count);
|
|
for (i = 0; i < self->posit_count; ++i) {
|
|
SCPrintf(pCon, eStatus, "%s.posit_%d = %lld\n", name, i + 1,
|
|
posit2count(self, i + 1));
|
|
}
|
|
}
|
|
SCPrintf(pCon, eStatus, "%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, eStatus);
|
|
/* TODO: disconnect */
|
|
change_state(self, DMCState_Disconnected);
|
|
break;
|
|
case AQU_RECONNECT:
|
|
snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
|
|
SICSLogWrite(line, eStatus);
|
|
/* TODO: reconnect */
|
|
/* Reset the state machine */
|
|
DMC_ClearTimer(self);
|
|
change_state(self, DMCState_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 */
|
|
|
|
/* Debug: this motor driver logs exchanges */
|
|
if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL)
|
|
pNew->debug=0;
|
|
else {
|
|
sscanf(pPtr,"%d",&(pNew->debug));
|
|
}
|
|
|
|
pNew->ao_id[0] = '\0';
|
|
if ((pPtr=getParam(pCon, interp, params, "action", _OPTIONAL)) != NULL) {
|
|
strncpy(pNew->ao_id, pPtr, sizeof(pNew->ao_id));
|
|
pNew->ao_id[sizeof(pNew->ao_id) - 1] = '\0';
|
|
}
|
|
if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) {
|
|
strncpy(pNew->long_name, pPtr, sizeof(pNew->long_name));
|
|
pNew->long_name[sizeof(pNew->long_name) - 1] = '\0';
|
|
}
|
|
if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%f",&(pNew->fLower));
|
|
if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) {
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%f",&(pNew->fUpper));
|
|
if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) {
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%s",pNew->units);
|
|
if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL) {
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%f",&(pNew->speed));
|
|
pNew->maxSpeed = pNew->speed;
|
|
if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL) {
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%f",&(pNew->accel));
|
|
pNew->maxAccel = pNew->accel;
|
|
if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL) {
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%f",&(pNew->decel));
|
|
pNew->maxDecel = pNew->decel;
|
|
if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL) {
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%c",&(pNew->axisLabel));
|
|
if (islower(pNew->axisLabel))
|
|
pNew->axisLabel = toupper(pNew->axisLabel);
|
|
if (!(pNew->axisLabel >= 'A' && pNew->axisLabel <= 'H')) {
|
|
snprintf(pError, ERRLEN, "\tInvalid axis on motor '%s':%c", motor,
|
|
pNew->axisLabel);
|
|
SCWrite(pCon,pError,eError);
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
|
|
}
|
|
if ((pPtr=getParam(pCon, interp, params,"encoderaxis",_OPTIONAL)) == NULL)
|
|
pNew->encoderAxis=0;
|
|
else {
|
|
sscanf(pPtr,"%c",&(pNew->encoderAxis));
|
|
if (islower(pNew->encoderAxis))
|
|
pNew->encoderAxis = toupper(pNew->encoderAxis);
|
|
}
|
|
if ((pPtr=getParam(pCon, interp, params,"stepsperx",_REQUIRED)) == NULL) {
|
|
KillDMC2280(pNew);
|
|
free(pNew);
|
|
return NULL;
|
|
}
|
|
sscanf(pPtr,"%lg",&(pNew->stepsPerX));
|
|
if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL)
|
|
pNew->motorHome=0;
|
|
else
|
|
sscanf(pPtr,"%d",&(pNew->motorHome));
|
|
if ((pPtr=getParam(pCon, interp, params,"nopowersave",_OPTIONAL)) == NULL)
|
|
pNew->noPowerSave=_SAVEPOWER;
|
|
else
|
|
sscanf(pPtr,"%d",&(pNew->noPowerSave));
|
|
if ((pPtr=getParam(pCon, interp, params,"motondelay",_OPTIONAL)) == NULL)
|
|
pNew->motOnDelay=DEFAULT_DELAY_MOTOR_ON;
|
|
else
|
|
sscanf(pPtr,"%d",&(pNew->motOnDelay));
|
|
if ((pPtr=getParam(pCon, interp, params,"motoffdelay",_OPTIONAL)) == NULL)
|
|
pNew->motOffDelay=DEFAULT_DELAY_MOTOR_OFF;
|
|
else
|
|
sscanf(pPtr,"%d",&(pNew->motOffDelay));
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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, eStatus);
|
|
}
|
|
}
|
|
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, eStatus, "%s.error = %d:%s", self->name, iCode, error);
|
|
} else {
|
|
SCPrintf(pCon, eStatus, "%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);
|
|
}
|
|
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;
|
|
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;
|
|
}
|
|
if (fabs(self->oscillate_high - motPosit(self)) < fabs(self->oscillate_low - motPosit(self)))
|
|
self->oscillate_up = true;
|
|
else
|
|
self->oscillate_up = false;
|
|
self->doOscillate = true;
|
|
self->doReportMotion = true;
|
|
self->oscillate_init = true;
|
|
self->oscillate_counter = 0;
|
|
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, eStatus, "%s.oscillation = %s", self->name, self->doOscillate ? "on" : "off");
|
|
return 1;
|
|
}
|
|
}
|
|
else if(strcasecmp("driver", argv[1]) == 0) {
|
|
if(strcasecmp("run", argv[2]) == 0) {
|
|
if (argc > 2) {
|
|
float fLower, fUpper, fZero, fSign, fFixed, fLim;
|
|
double fSoft;
|
|
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, "softlowerlim", &fLower);
|
|
MotorGetPar(self->pMot, "softupperlim", &fUpper);
|
|
MotorGetPar(self->pMot, "softzero", &fZero);
|
|
MotorGetPar(self->pMot, "sign", &fSign);
|
|
MotorGetPar(self->pMot, "fixed", &fFixed);
|
|
if (fFixed >= 0) {
|
|
SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name);
|
|
} else if (fSoft > fUpper) {
|
|
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
|
|
fSoft, fUpper, self->name);
|
|
} else if (fSoft < fLower) {
|
|
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
|
|
fSoft, fLower, self->name);
|
|
} else {
|
|
double fHard = fSoft;
|
|
fHard += fZero;
|
|
fHard *= fSign;
|
|
if (fHard > self->fUpper) {
|
|
SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s",
|
|
fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name);
|
|
} else if (fHard < self->fLower) {
|
|
SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s",
|
|
fSoft, self->fLower * fSign + fZero, self->fLower, self->name);
|
|
} else {
|
|
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, eStatus, "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(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->posit_count);
|
|
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 (self->posit_count > 0) {
|
|
self->posit_count = 0;
|
|
}
|
|
if (self->positions) {
|
|
free(self->positions);
|
|
self->positions = NULL;
|
|
self->posit_count = 0;
|
|
}
|
|
if (argc > 3 && strcasecmp("erase", argv[2]) == 0) {
|
|
char line[132];
|
|
snprintf(line, 132, "%s.posit_count = %d", self->name, self->posit_count);
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
self->posit_count = argc - 2;
|
|
self->positions = malloc(self->posit_count * sizeof(*self->positions));
|
|
int i;
|
|
for (i = 0; i < self->posit_count; ++i) {
|
|
self->positions[i] = strtod(argv[i + 2], NULL);
|
|
}
|
|
return 1;
|
|
}
|
|
if (self->abs_encoder && strcasecmp("absEncHome", argv[1]) == 0) {
|
|
if (argc > 2) {
|
|
/* Debug Managers only */
|
|
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
|
return 0;
|
|
long lValue = strtol(argv[2], NULL, 10);
|
|
self->absEncHome = lValue;
|
|
return 1;
|
|
}
|
|
else {
|
|
char line[132];
|
|
snprintf(line, 132, "%s.absEncHome = %d.000000",
|
|
self->name,
|
|
self->absEncHome);
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
}
|
|
if (self->abs_encoder && strcasecmp("absenc", argv[1]) == 0) {
|
|
long lValue;
|
|
if (readAbsEnc(self, &lValue) != SUCCESS) {
|
|
lValue = -1;
|
|
}
|
|
char line[132];
|
|
snprintf(line, 132, "%s.absEnc = %ld.000000",
|
|
self->name,
|
|
lValue);
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
|
|
if (self->posit_count > 0) {
|
|
if(strcasecmp("posit2count", argv[1]) == 0) {
|
|
char line[132];
|
|
if (argc > 2) {
|
|
double target;
|
|
target = strtod(argv[2], NULL);
|
|
snprintf(line, 132, "%s.posit2count = %lld",
|
|
self->name,
|
|
posit2count(self, target));
|
|
}
|
|
else
|
|
strcpy(line, "missing value");
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
else if(strcasecmp("count2posit", argv[1]) == 0) {
|
|
char line[132];
|
|
if (argc > 2) {
|
|
long long target;
|
|
target = strtoll(argv[2], NULL, 10);
|
|
snprintf(line, 132, "%s.count2posit = %f",
|
|
self->name,
|
|
count2posit(self, target));
|
|
}
|
|
else
|
|
strcpy(line, "missing value");
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
else if(strcasecmp("unit2posit", argv[1]) == 0) {
|
|
char line[132];
|
|
if (argc > 2) {
|
|
double target;
|
|
target = strtod(argv[2], NULL);
|
|
snprintf(line, 132, "%s.unit2posit = %f",
|
|
self->name,
|
|
unit2posit(self, target));
|
|
}
|
|
else
|
|
strcpy(line, "missing value");
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
else if(strcasecmp("posit2unit", argv[1]) == 0) {
|
|
char line[132];
|
|
if (argc > 2) {
|
|
double target;
|
|
target = strtod(argv[2], NULL);
|
|
snprintf(line, 132, "%s.posit2unit = %f",
|
|
self->name,
|
|
posit2unit(self, target));
|
|
}
|
|
else
|
|
strcpy(line, "missing value");
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
else if(strcasecmp("posit", argv[1]) == 0) {
|
|
char line[132];
|
|
int target = self->currCounts;
|
|
if (argc > 2)
|
|
target = strtol(argv[2], NULL, 10);
|
|
snprintf(line, 132, "%s.posit = %f",
|
|
self->name,
|
|
count2posit(self, target));
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
}
|
|
}
|
|
return MotorAction(pCon, pSics, pData, argc, argv);
|
|
}
|
|
|
|
void DMC2280InitProtocol(SicsInterp *pSics) {
|
|
if (DMC2280_Protocol == NULL) {
|
|
DMC2280_Protocol = AsyncProtocolCreate(pSics, "DMC2280", NULL, NULL);
|
|
DMC2280_Protocol->sendCommand = DMC_Tx;
|
|
DMC2280_Protocol->handleInput = DMC_Rx;
|
|
DMC2280_Protocol->handleEvent = DMC_Ev;
|
|
DMC2280_Protocol->prepareTxn = NULL;
|
|
DMC2280_Protocol->killPrivate = NULL;
|
|
}
|
|
}
|