linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Before removal of deprecated code

Browse Source 
r2102 | dcl | 2007-08-02 08:06:01 +1000 (Thu, 02 Aug 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-08-02 08:06:01 +10:00
parent d0ac56e112
commit 18ab7a86b8
1 changed files with 209 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

354
site_ansto/motor_dmc2280.c
View File

@@ -1,3 +1,4 @@
#define DEPRECATED
/** \file motor_dmc2280.c /** \file motor_dmc2280.c
* \brief Driver for Galil DMC2280 motor controller. * \brief Driver for Galil DMC2280 motor controller.
* *
@@ -17,6 +18,7 @@
#include <math.h> #include <math.h>
#include <float.h> #include <float.h>
#include <assert.h> #include <assert.h>
#include <ctype.h>
#include <string.h> #include <string.h>
#include <stdarg.h> #include <stdarg.h>
#include <sys/time.h> #include <sys/time.h>
@@ -168,6 +170,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
#define MOTOR_POLL_SLOW 1000 #define MOTOR_POLL_SLOW 1000
#define ON_SETTLE_TIMER 1000 #define ON_SETTLE_TIMER 1000
#define MAX_CREEP_STEPS 100 #define MAX_CREEP_STEPS 100
#define MAX_RESTARTS 0
#define AIRPADS_DOWN 0 #define AIRPADS_DOWN 0
#define AIRPADS_RAISE 1 #define AIRPADS_RAISE 1
@@ -619,20 +622,7 @@ static void set_lastMotion(pDMC2280Driv self, float steps, float counts) {
gettimeofday(&(self->time_lastPos_set), NULL); gettimeofday(&(self->time_lastPos_set), NULL);
} }
static int set_currMotion(pDMC2280Driv self, const char* text) { #ifndef DEPRECATED
int iRet, iFlags;
double steps, counts, flags;
iRet = sscanf(text, "%lf %lf %lf", &steps, &counts, &flags);
if (iRet != 3)
return 0;
iFlags = (int)(flags + 0.1);
self->currFlags = iFlags;
self->currSteps = steps;
self->currCounts = counts;
self->currPosition = motPosit(self);
return 1;
}
/** \brief Reads motion. /** \brief Reads motion.
* *
* \param *steps motor steps * \param *steps motor steps
@@ -653,6 +643,7 @@ static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
return FAILURE; return FAILURE;
return SUCCESS; return SUCCESS;
} }
#endif
/** \brief Reads absolute encoder. /** \brief Reads absolute encoder.
* *
@@ -739,6 +730,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
return SUCCESS; return SUCCESS;
} }
#ifndef DEPRECATED
/** /**
* \brief calculate and issue the motion commands * \brief calculate and issue the motion commands
* *
@@ -845,15 +837,6 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
s_stps, s_stps,
s_cnts, s_cnts,
axis); axis);
#endif
#ifdef BACKLASHFIX
do {
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis,
(int) (target * cntsPerX + absEncHome + 0.5));
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
} while (0);
#endif #endif
} else { } else {
newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5); newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5);
@@ -989,6 +972,7 @@ static int motoff_timeout(void* context, int mode) {
DMC2280Send(self, cmd); DMC2280Send(self, cmd);
return 0; return 0;
} }
#endif
/** \brief Check if the axis has moved significantly since /** \brief Check if the axis has moved significantly since
* the last check. * the last check.
@@ -1018,10 +1002,12 @@ static int checkMotion(void *pData) {
steps = self->currSteps; steps = self->currSteps;
counts = self->currCounts; counts = self->currCounts;
} }
#ifndef DEPRECATED
else { else {
if (FAILURE == readMotion(self, &steps, &counts)) if (FAILURE == readMotion(self, &steps, &counts))
return 0; return 0;
} }
#endif
set_lastMotion(pData, steps, counts); set_lastMotion(pData, steps, counts);
return 1; return 1;
} }
@@ -1036,10 +1022,12 @@ static int checkMotion(void *pData) {
steps = self->currSteps; steps = self->currSteps;
counts = self->currCounts; counts = self->currCounts;
} }
#ifndef DEPRECATED
else { else {
if (FAILURE == readMotion(self, &steps, &counts)) if (FAILURE == readMotion(self, &steps, &counts))
return 0; return 0;
} }
#endif
/* If not stepping, then not blocked */ /* If not stepping, then not blocked */
if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
/* just update the timestamp */ /* just update the timestamp */
@@ -1088,6 +1076,7 @@ static int checkMotion(void *pData) {
return 1; return 1;
} }
#ifndef DEPRECATED
/** /**
* \brief handle the run command for a motor with airpads * \brief handle the run command for a motor with airpads
* *
@@ -1099,33 +1088,100 @@ static int DMC2280RunAir(pDMC2280Driv self, float fValue) {
return HWFault; return HWFault;
return OKOK; return OKOK;
} }
#endif
/* State Functions */ /**
* \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_x(pDMC2280Driv self, const char* vars, const char* name) {
char tmp[20];
char* p;
snprintf(tmp, 20, "%s%c=", name, self->axisLabel);
p = strstr(vars, tmp);
if (p == NULL) /* not found at all */
return false;
if (p == vars) /* found at start of first line */
return false;
if (p[-1] == '\n') /* found at start of another line */
return true;
return false; /* found partial (ZVVVx=) only */
}
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event); /* State Callbacks */
static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event);
static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event);
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event);
static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event);
static int state_msg_callback(pAsyncTxn pCmd); static int state_msg_callback(pAsyncTxn pCmd);
static int state_tmr_callback(void* ctx, int mode); static int state_tmr_callback(void* ctx, int mode);
static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd); static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd);
/**
* \brief request standard state information for the motor and controller
*
* \param self access to the drive data
*/
static void request_status(pDMC2280Driv self) {
char cmd[CMDLEN];
/* TODO: add _RUNx for has_airpads == 3 */
snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,_TI0,_TI1,_XQ0",
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return;
}
static int process_status(pDMC2280Driv self, const char* text) {
int iRet, iFlags;
int iSteps, iCounts;
int iTI0, iTI1, iXQ0;
/* TODO: add _RUNx for has_airpads == 3 */
iRet = sscanf(text, "%d %d %d %d %d %d",
&iSteps, &iCounts, &iFlags,
&iTI0, &iTI1, &iXQ0);
if (iRet != 6)
return 0;
self->currFlags = iFlags;
self->currSteps = iSteps;
self->currCounts = iCounts;
self->currPosition = motPosit(self);
/* TODO Amplifier Error and Thread0 */
return 1;
}
/* State Functions */
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event);
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event);
static char* state_name(StateFunc func) { static char* state_name(StateFunc func) {
if (func == DMCState_Unknown) return "DMCState_Unknown"; if (func == DMCState_Unknown) return "DMCState_Unknown";
if (func == DMCState_Idle) return "DMCState_Idle"; if (func == DMCState_Idle) return "DMCState_Idle";
if (func == DMCState_AirOn) return "DMCState_AirOn"; if (func == DMCState_MotorStart) return "DMCState_MotorStart";
if (func == DMCState_MotorOn) return "DMCState_MotorOn"; if (func == DMCState_MotorOn) return "DMCState_MotorOn";
if (func == DMCState_Moving) return "DMCState_Moving"; if (func == DMCState_Moving) return "DMCState_Moving";
if (func == DMCState_MotorHalt) return "DMCState_MotorHalt"; if (func == DMCState_MotorHalt) return "DMCState_MotorHalt";
if (func == DMCState_OffTimer) return "DMCState_OffTimer"; if (func == DMCState_OffTimer) return "DMCState_OffTimer";
if (func == DMCState_AirOff) return "DMCState_AirOff"; if (func == DMCState_MotorStop) return "DMCState_MotorStop";
if (func == DMCState_MotorOff) return "DMCState_MotorOff"; if (func == DMCState_MotorOff) return "DMCState_MotorOff";
if (func == DMCState_WaitStatus) return "DMCState_WaitStatus"; if (func == DMCState_WaitStatus) return "DMCState_WaitStatus";
return "<unknown_state>"; return "<unknown_state>";
@@ -1295,18 +1351,14 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value); snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280Send(self, cmd))
break; break;
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", request_status(self);
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return; return;
case eMessageEvent: case eMessageEvent:
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'M') { /* MG */ if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet; int iRet;
iRet = set_currMotion(self, pCmd->inp_buf); iRet = process_status(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
set_lastMotion(self, self->currSteps, self->currCounts); set_lastMotion(self, self->currSteps, self->currCounts);
@@ -1314,13 +1366,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
change_state(self, DMCState_Idle); change_state(self, DMCState_Idle);
return; return;
} }
#if 0
value = ((self->currCounts - self->absEncHome) / self->cntsPerX) * self->stepsPerX;
self->currSteps = value;
snprintf(cmd, CMDLEN, "DP%c=%d", self->axisLabel, value);
#else
snprintf(cmd, CMDLEN, "LV"); snprintf(cmd, CMDLEN, "LV");
#endif
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
return; return;
} }
@@ -1329,13 +1375,14 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
return; return;
} }
if (pCmd->out_buf[0] == 'L') { /* LV */ if (pCmd->out_buf[0] == 'L') { /* LV */
char req[12]; if (has_var_x(self, pCmd->inp_buf, "REQ"))
char rdy[12]; if (has_var_x(self, pCmd->inp_buf, "RSP")) {
snprintf(req, 12, "REQ%c=", self->axisLabel);
snprintf(rdy, 12, "RSP%c=", self->axisLabel);
if (strstr(pCmd->inp_buf, req) &&
strstr(pCmd->inp_buf, rdy)) {
self->has_airpads = 2; self->has_airpads = 2;
if (has_var_x(self, pCmd->inp_buf, "RUN"))
if (has_var_x(self, pCmd->inp_buf, "DST"))
if (has_var_x(self, pCmd->inp_buf, "POS")) {
self->has_airpads = 3;
}
} }
change_state(self, DMCState_Idle); change_state(self, DMCState_Idle);
return; return;
@@ -1355,11 +1402,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN]; char cmd[CMDLEN];
switch (event->event_type) { switch (event->event_type) {
case eTimerEvent: case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", request_status(self);
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_WaitStatus); change_state(self, DMCState_WaitStatus);
return; return;
case eMessageEvent: case eMessageEvent:
@@ -1373,12 +1416,13 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
if (iRet != 2) if (iRet != 2)
break; break;
if (fReply < 0) { if (fReply < 0) {
if (self->subState > 0) { if (self->subState >= MAX_RESTARTS) {
/* TODO: handle fault logging */
self->driver_status = HWFault; self->driver_status = HWFault;
self->errorCode = THREADZERO; self->errorCode = THREADZERO;
return; return;
} }
DMC_SendCmd(self, "XQ #AUTO,0", state_msg_callback); DMC_SendCmd(self, "XQ #THREAD0,0", state_msg_callback);
return; return;
} }
/* Handle limit switches */ /* Handle limit switches */
@@ -1397,25 +1441,21 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
} }
if (self->has_airpads == 1) { if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=1"); snprintf(cmd, CMDLEN, "FTUBE=1");
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOn);
} }
else if (self->has_airpads == 2) { else if (self->has_airpads == 2 || self->has_airpads == 3) {
snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel); snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOn);
} }
else { else {
snprintf(cmd, CMDLEN, "SH%c", self->axisLabel); snprintf(cmd, CMDLEN, "SH%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_MotorOn);
} }
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_MotorStart);
return; return;
} }
if (pCmd->out_buf[0] == 'X') { /* XQ #AUTO,0 */ if (pCmd->out_buf[0] == 'X') { /* XQ #THREAD0,0 */
snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel); snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
self->subState = 1; self->subState += 1;
} }
} while (0); } while (0);
break; break;
@@ -1447,11 +1487,11 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
self->errorCode = STATEERROR; self->errorCode = STATEERROR;
} }
static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) { static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN]; char cmd[CMDLEN];
switch (event->event_type) { switch (event->event_type) {
case eTimerEvent: case eTimerEvent:
if (self->has_airpads == 2) if (self->has_airpads == 2 || self->has_airpads == 3)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else else
snprintf(cmd, CMDLEN, "MG APDONE"); snprintf(cmd, CMDLEN, "MG APDONE");
@@ -1460,7 +1500,13 @@ static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) {
case eMessageEvent: case eMessageEvent:
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'F' || /* FTUBE */ if (pCmd->out_buf[0] == 'S') { /* SH */
NetWatchRegisterTimer(&self->state_timer,
ON_SETTLE_TIMER,
state_tmr_callback, self);
return;
}
else if (pCmd->out_buf[0] == 'F' || /* FTUBE */
pCmd->out_buf[0] == 'R') { /* REQx= */ pCmd->out_buf[0] == 'R') { /* REQx= */
NetWatchRegisterTimer(&self->state_timer, NetWatchRegisterTimer(&self->state_timer,
AIR_POLL_TIMER, AIR_POLL_TIMER,
@@ -1498,25 +1544,15 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN]; char cmd[CMDLEN];
switch (event->event_type) { switch (event->event_type) {
case eTimerEvent: case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", request_status(self);
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return; return;
case eMessageEvent: case eMessageEvent:
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'S') { /* SH */ if (pCmd->out_buf[0] == 'M') { /* MG */
NetWatchRegisterTimer(&self->state_timer,
ON_SETTLE_TIMER,
state_tmr_callback, self);
return;
}
else if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet, absolute; int iRet, absolute;
float target; float target;
iRet = set_currMotion(self, pCmd->inp_buf); iRet = process_status(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
set_lastMotion(self, self->currSteps, self->currCounts); set_lastMotion(self, self->currSteps, self->currCounts);
@@ -1553,12 +1589,14 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
} }
self->creep_val = 0; self->creep_val = 0;
absolute = motCreep(self, target); absolute = motCreep(self, target);
/* TODO set DSTx for has_airpads == 3 */
snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute); snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute);
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
return; return;
} }
else if (pCmd->out_buf[0] == 'P') { /* PA */ else if (pCmd->out_buf[0] == 'P') { /* PA */
self->stepCount = 1; self->stepCount = 1;
/* TODO set RUNx for has_airpads == 3 */
snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); snprintf(cmd, CMDLEN, "BG%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_Moving); change_state(self, DMCState_Moving);
@@ -1579,11 +1617,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN]; char cmd[CMDLEN];
switch (event->event_type) { switch (event->event_type) {
case eTimerEvent: case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", request_status(self);
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return; return;
case eMessageEvent: case eMessageEvent:
do { do {
@@ -1603,7 +1637,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
else if (pCmd->out_buf[0] == 'M') { /* MG */ else if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet, iFlags; int iRet, iFlags;
bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
iRet = set_currMotion(self, pCmd->inp_buf); iRet = process_status(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
iFlags = self->currFlags; iFlags = self->currFlags;
@@ -1779,11 +1813,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
char cmd[CMDLEN]; char cmd[CMDLEN];
switch (event->event_type) { switch (event->event_type) {
case eTimerEvent: case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", request_status(self);
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
return; return;
case eMessageEvent: case eMessageEvent:
do { do {
@@ -1796,7 +1826,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
} }
else if (pCmd->out_buf[0] == 'M') { /* MG */ else if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet, iFlags; int iRet, iFlags;
iRet = set_currMotion(self, pCmd->inp_buf); iRet = process_status(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
iFlags = self->currFlags; iFlags = self->currFlags;
@@ -1809,19 +1839,19 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
if (self->has_airpads == 1) { if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=0"); snprintf(cmd, CMDLEN, "FTUBE=0");
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff); change_state(self, DMCState_MotorStop);
return; return;
} }
else if (self->has_airpads == 2) { else if (self->has_airpads == 2 || self->has_airpads == 3) {
snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel); snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff); change_state(self, DMCState_MotorStop);
return; return;
} }
else { else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_MotorOff); change_state(self, DMCState_MotorStop);
return; return;
} }
} }
@@ -1842,18 +1872,18 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
if (self->has_airpads == 1) { if (self->has_airpads == 1) {
snprintf(cmd, CMDLEN, "FTUBE=0"); snprintf(cmd, CMDLEN, "FTUBE=0");
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff); change_state(self, DMCState_MotorStop);
} }
else if (self->has_airpads == 2) { else if (self->has_airpads == 2 || self->has_airpads == 3) {
snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel); snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_AirOff); change_state(self, DMCState_MotorStop);
return; return;
} }
else { else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback); DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_MotorOff); change_state(self, DMCState_MotorStop);
} }
return; return;
case eCommandEvent: case eCommandEvent:
@@ -1881,11 +1911,11 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
self->errorCode = STATEERROR; self->errorCode = STATEERROR;
} }
static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) { static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN]; char cmd[CMDLEN];
switch (event->event_type) { switch (event->event_type) {
case eTimerEvent: case eTimerEvent:
if (self->has_airpads == 2) if (self->has_airpads == 2 || self->has_airpads == 3)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else else
snprintf(cmd, CMDLEN, "MG APDONE"); snprintf(cmd, CMDLEN, "MG APDONE");
@@ -1897,8 +1927,11 @@ static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) {
if (pCmd->out_buf[0] == 'F' || /* FTUBE */ if (pCmd->out_buf[0] == 'F' || /* FTUBE */
pCmd->out_buf[0] == 'R') { /* REQx= */ pCmd->out_buf[0] == 'R') { /* REQx= */
} }
else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx */ else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx or MO */
float fReply; float fReply;
if (pCmd->out_buf[1] == 'O') /* MO */
fReply = 1;
else
fReply = (float) atof(pCmd->inp_buf); fReply = (float) atof(pCmd->inp_buf);
if (fReply == 0) { if (fReply == 0) {
NetWatchRegisterTimer(&self->state_timer, NetWatchRegisterTimer(&self->state_timer,
@@ -1964,9 +1997,9 @@ static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event) {
case eMessageEvent: case eMessageEvent:
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'M') { /* MG _TD%c,_TP%c,_TS%c */ if (pCmd->out_buf[0] == 'M') { /* MG */
int iRet; int iRet;
iRet = set_currMotion(self, pCmd->inp_buf); iRet = process_status(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
change_state(self, DMCState_Idle); change_state(self, DMCState_Idle);
@@ -2029,26 +2062,24 @@ static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event) {
* */ * */
static int DMC2280GetPos(void *pData, float *fPos){ static int DMC2280GetPos(void *pData, float *fPos){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
#ifndef DEPRECATED
char reply[CMDLEN]; char reply[CMDLEN];
char cmd[CMDLEN]; char cmd[CMDLEN];
float absEncPos, motorPos; float absEncPos, motorPos;
reply[0]='\0'; reply[0]='\0';
#endif
self = (pDMC2280Driv)pData; self = (pDMC2280Driv)pData;
assert(self != NULL); assert(self != NULL);
#if 1
if (self->has_fsm) { if (self->has_fsm) {
if (self->myState == DMCState_Unknown) if (self->myState == DMCState_Unknown)
SicsWait(1); SicsWait(1);
if (self->abs_encoder) *fPos = motPosit(self);
*fPos = (self->currCounts - self->absEncHome)/self->cntsPerX + self->home;
else
*fPos = (self->currSteps - self->motorHome)/self->stepsPerX + self->home;
return OKOK; return OKOK;
} }
#endif
#ifndef DEPRECATED
if (1 == self->abs_encoder) { if (1 == self->abs_encoder) {
if (readAbsEnc(self, &absEncPos) == FAILURE) if (readAbsEnc(self, &absEncPos) == FAILURE)
return HWFault; return HWFault;
@@ -2060,6 +2091,7 @@ static int DMC2280GetPos(void *pData, float *fPos){
motorPos =(float)atof(reply); motorPos =(float)atof(reply);
*fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home; *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home;
} }
#endif
return OKOK; return OKOK;
} }
@@ -2108,7 +2140,7 @@ static int DMC2280Run(void *pData,float fValue){
if (self->has_fsm) { if (self->has_fsm) {
/* TODO: FIXME and handle RUN command everywhere */ /* TODO: FIXME and handle RUN command everywhere */
while (self->myState == DMCState_AirOff while (self->myState == DMCState_MotorStop
|| self->myState == DMCState_MotorOff) { || self->myState == DMCState_MotorOff) {
SicsWait(1); SicsWait(1);
} }
@@ -2116,6 +2148,7 @@ static int DMC2280Run(void *pData,float fValue){
return 1; return 1;
} }
#ifndef DEPRECATED
/* /*
* Note: this passes control to a timer routine * Note: this passes control to a timer routine
*/ */
@@ -2124,6 +2157,10 @@ static int DMC2280Run(void *pData,float fValue){
return DMC2280RunAir(self, fValue); return DMC2280RunAir(self, fValue);
return DMC2280RunCommon(self, fValue); return DMC2280RunCommon(self, fValue);
#else
assert(self->has_fsm);
return 0;
#endif
} }
@@ -2140,15 +2177,12 @@ static int DMC2280Run(void *pData,float fValue){
*/ */
static int DMC2280Status(void *pData){ static int DMC2280Status(void *pData){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
#ifndef DEPRECATED
char cmd[CMDLEN]; char cmd[CMDLEN];
int switches; int switches;
char switchesAscii[CMDLEN]; char switchesAscii[CMDLEN];
#ifdef BACKLASHFIX
char reply[CMDLEN];
int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus;
int SHOULD_FIXPOS=1, should_fixpos;
#endif
bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
#endif
self = (pDMC2280Driv)pData; self = (pDMC2280Driv)pData;
assert(self != NULL); assert(self != NULL);
@@ -2156,6 +2190,7 @@ static int DMC2280Status(void *pData){
if (self->has_fsm) { if (self->has_fsm) {
return self->driver_status; return self->driver_status;
} }
#ifndef DEPRECATED
/* /*
* If we are waiting for the motor or airpads then we * If we are waiting for the motor or airpads then we
* are busy * are busy
@@ -2217,31 +2252,6 @@ static int DMC2280Status(void *pData){
self->errorCode = ERRORLIM; self->errorCode = ERRORLIM;
return HWFault; return HWFault;
} }
#ifdef BACKLASHFIX
if (self->abs_encoder == 1) {
/* Make sure that the servo loop is closed by checking if
* the CLSLOOP thread is running on the controller.*/
if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply))
return HWFault;
sscanf(reply, "%d", &servoLoopStatus);
if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) {
/* Start subroutine on controller to close the servo loop */
if (FAILURE == DMC2280Send(self, "XQ#CLSLOOP"))
return HWFault;
}
snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel);
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return HWFault;
sscanf(reply, "%d", &should_fixpos);
if (should_fixpos == SHOULD_FIXPOS) {
snprintf(cmd, CMDLEN, "%cFIXPOS=1", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
self->errorCode=BADBSY;
return HWBusy;
}
}
#endif
/* /*
* When we get here, the motion has completed * When we get here, the motion has completed
*/ */
@@ -2303,6 +2313,10 @@ static int DMC2280Status(void *pData){
} }
return HWIdle; return HWIdle;
} }
#else
assert(self->has_fsm);
return 0;
#endif
} }
/** \brief DMC2280 implementation of the GetError /** \brief DMC2280 implementation of the GetError
* method in the MotorDriver interface. * method in the MotorDriver interface.
@@ -2472,7 +2486,9 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
*/ */
static int DMC2280Halt(void *pData){ static int DMC2280Halt(void *pData){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
#ifndef DEPRECATED
char cmd[CMDLEN]; char cmd[CMDLEN];
#endif
self = (pDMC2280Driv)pData; self = (pDMC2280Driv)pData;
assert(self != NULL); assert(self != NULL);
@@ -2481,6 +2497,7 @@ static int DMC2280Halt(void *pData){
state_cmd_execute(self, CMD_HALT); state_cmd_execute(self, CMD_HALT);
return 1; return 1;
} }
#ifndef DEPRECATED
/* Stop motor */ /* Stop motor */
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280Send(self, cmd))
@@ -2491,6 +2508,10 @@ static int DMC2280Halt(void *pData){
return HWFault; return HWFault;
else else
return 1; return 1;
#else
assert(self->has_fsm);
return 0;
#endif
} }
/** \brief Fetches the value of the named parameter, /** \brief Fetches the value of the named parameter,
@@ -2952,9 +2973,9 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
if (self->abs_encoder) { if (self->abs_encoder) {
snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome); snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome);
SCWrite(pCon, buffer, eStatus); SCWrite(pCon, buffer, eStatus);
if (self->has_fsm) {
snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX); snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX);
SCWrite(pCon, buffer, eStatus); SCWrite(pCon, buffer, eStatus);
if (self->has_fsm) {
snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset); snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset);
SCWrite(pCon, buffer, eStatus); SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Creep_Precision = %f\n", name, self->creep_precision); snprintf(buffer, BUFFLEN, "%s.Creep_Precision = %f\n", name, self->creep_precision);
@@ -3155,6 +3176,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
pNew->has_fsm=1; pNew->has_fsm=1;
else { else {
sscanf(pPtr,"%d",&(pNew->has_fsm)); sscanf(pPtr,"%d",&(pNew->has_fsm));
assert(pNew->has_fsm); /* DEPRECATED */
} }
if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) { if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) {
@@ -3206,6 +3228,17 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
return NULL; return NULL;
} }
sscanf(pPtr,"%c",&(pNew->axisLabel)); 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,"stepsperx",_REQUIRED)) == NULL) { if ((pPtr=getParam(pCon, interp, params,"stepsperx",_REQUIRED)) == NULL) {
KillDMC2280(pNew); KillDMC2280(pNew);
free(pNew); free(pNew);
@@ -3286,6 +3319,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
0, 0,
state_tmr_callback, pNew); state_tmr_callback, pNew);
} }
#ifndef DEPRECATED
else { else {
char cmd[CMDLEN]; char cmd[CMDLEN];
/* Set speed */ /* Set speed */
@@ -3304,6 +3338,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
* to get a sensible initial list output * to get a sensible initial list output
*/ */
} }
#endif
return (MotorDriver *)pNew; return (MotorDriver *)pNew;
} }
@@ -3348,6 +3383,27 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
} }
return 1; 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,
self->axisLabel);
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("units", argv[1]) == 0) { else if (strcasecmp("units", argv[1]) == 0) {
if (argc > 2) { if (argc > 2) {
strncpy(self->units, argv[2], sizeof(self->units)); strncpy(self->units, argv[2], sizeof(self->units));