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

Change floats to doubles, controller virtual motor variables, a little refactoring

Browse Source 
r2264 | dcl | 2007-12-07 10:38:34 +1100 (Fri, 07 Dec 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-12-07 10:38:34 +11:00
parent ef525d1f58
commit 3926e47ab5
1 changed files with 110 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

190
site_ansto/motor_dmc2280.c
View File

@@ -78,13 +78,15 @@ struct EvtEvent_s {
} event; } event;
}; };
#define VAR_REQ (1<<0) #define VAR_REQ (1<<0)
#define VAR_RSP (1<<1) #define VAR_RSP (1<<1)
#define VAR_RUN (1<<2) #define VAR_RUN (1<<2)
#define VAR_DST (1<<3) #define VAR_DST (1<<3)
#define VAR_POS (1<<4) #define VAR_POS (1<<4)
#define VAR_HLT (1<<5) #define VAR_HLT (1<<5)
#define VAR_ENC (1<<6) #define VAR_ENC (1<<6)
#define VAR_SWI (1<<7)
#define VAR_HOM (1<<8)
static pAsyncProtocol DMC2280_Protocol = NULL; static pAsyncProtocol DMC2280_Protocol = NULL;
@@ -133,19 +135,19 @@ struct __MoDriv {
char axisLabel; char axisLabel;
char lastCmd[CMDLEN]; char lastCmd[CMDLEN];
char dmc2280Error[CMDLEN]; char dmc2280Error[CMDLEN];
float fHome; /**< home position for axis, default=0 */ double fHome; /**< home position for axis, default=0 */
int motorHome; /**< motor home position in steps */ int motorHome; /**< motor home position in steps */
int noPowerSave; /**< Flag = 1 to leave motors on after a move */ int noPowerSave; /**< Flag = 1 to leave motors on after a move */
float stepsPerX; /**< steps per physical unit */ double stepsPerX; /**< steps per physical unit */
int abs_encoder; /**< Flag = 1 if there is an abs enc */ int abs_encoder; /**< Flag = 1 if there is an abs enc */
int absEncHome; /**< Home position in counts for abs enc */ int absEncHome; /**< Home position in counts for abs enc */
float cntsPerX; /**< absolute encoder counts per physical unit */ double cntsPerX; /**< absolute encoder counts per physical unit */
int motOffDelay; /**< msec to wait before switching motor off, default=0 */ int motOffDelay; /**< msec to wait before switching motor off, default=0 */
int currFlags; int currFlags;
int currSteps; int currSteps;
int currCounts; int currCounts;
float currPosition; /**< Position at last position check */ double currPosition; /**< Position at last position check */
float lastPosition; /**< Position at last position check */ double lastPosition; /**< Position at last position check */
int lastSteps; int lastSteps;
int lastCounts; int lastCounts;
int thread0; /**< last read of _XQ0 */ int thread0; /**< last read of _XQ0 */
@@ -162,8 +164,8 @@ struct __MoDriv {
int blockage_fail; /**< flag =1 if we should fail the motor */ int blockage_fail; /**< flag =1 if we should fail the motor */
int has_airpads; /**< Flag = 1 if there is are airpads for this motor */ int has_airpads; /**< Flag = 1 if there is are airpads for this motor */
float backlash_offset; /**< signed offset to drive from */ float backlash_offset; /**< signed offset to drive from */
float fTarget; /**< target passed from SICS to timer callback */ double fTarget; /**< target passed from SICS to timer callback */
float fPreseek; /**< preseek target when preseek is active */ double fPreseek; /**< preseek target when preseek is active */
int settle; /**< motor settling time in seconds */ int settle; /**< motor settling time in seconds */
struct timeval time_settle_done; /**< time when settling will be over */ struct timeval time_settle_done; /**< time when settling will be over */
int airpad_state; /**< state of the airpads finite state machine */ int airpad_state; /**< state of the airpads finite state machine */
@@ -383,7 +385,7 @@ static bool check_positions(pDMC2280Driv self) {
return false; return false;
} }
static long long unit2count(pDMC2280Driv self, float target) { static long long unit2count(pDMC2280Driv self, double target) {
double absolute; double absolute;
long long result; long long result;
if (self->abs_encoder == 0) { if (self->abs_encoder == 0) {
@@ -413,8 +415,8 @@ static long long unit2count(pDMC2280Driv self, float target) {
return result; return result;
} }
static float count2unit(pDMC2280Driv self, long long counts) { static double count2unit(pDMC2280Driv self, long long counts) {
float fPos; double fPos;
if (self->abs_encoder == 0) { if (self->abs_encoder == 0) {
char line[CMDLEN]; char line[CMDLEN];
snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder", snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder",
@@ -427,7 +429,7 @@ static float count2unit(pDMC2280Driv self, long long counts) {
} }
#ifdef POSIT_COUNTS #ifdef POSIT_COUNTS
static long long posit2count(pDMC2280Driv self, float target) { static long long posit2count(pDMC2280Driv self, double target) {
double absolute; double absolute;
long long result; long long result;
if (self->abs_encoder == 0) { if (self->abs_encoder == 0) {
@@ -463,9 +465,9 @@ static long long posit2count(pDMC2280Driv self, float target) {
return result; return result;
} }
static float count2posit(pDMC2280Driv self, long long counts) { static double count2posit(pDMC2280Driv self, long long counts) {
int i, j; int i, j;
float fPos; double fPos;
if (self->abs_encoder == 0) { if (self->abs_encoder == 0) {
char line[CMDLEN]; char line[CMDLEN];
snprintf(line, CMDLEN, "count2posit motor %s has no absolute encoder", snprintf(line, CMDLEN, "count2posit motor %s has no absolute encoder",
@@ -498,19 +500,19 @@ static float count2posit(pDMC2280Driv self, long long counts) {
return fPos; return fPos;
} }
static float unit2posit(pDMC2280Driv self, float target) { static double unit2posit(pDMC2280Driv self, double target) {
return count2posit(self, unit2count(self, target)); return count2posit(self, unit2count(self, target));
} }
static float posit2unit(pDMC2280Driv self, float target) { static double posit2unit(pDMC2280Driv self, double target) {
return count2unit(self, posit2count(self, target)); return count2unit(self, posit2count(self, target));
} }
#else #else
static float unit2posit(pDMC2280Driv self, float target) { static double unit2posit(pDMC2280Driv self, double target) {
int i, j; int i, j;
float fPos; double fPos;
if (!check_positions(self)) if (!check_positions(self))
return 0; return 0;
@@ -536,7 +538,7 @@ static float unit2posit(pDMC2280Driv self, float target) {
return fPos; return fPos;
} }
static float posit2unit(pDMC2280Driv self, float target) { static double posit2unit(pDMC2280Driv self, double target) {
double result; double result;
int i = ((int) target) - 1; int i = ((int) target) - 1;
if (i < 0) { if (i < 0) {
@@ -550,11 +552,11 @@ static float posit2unit(pDMC2280Driv self, float target) {
return result; return result;
} }
static long long posit2count(pDMC2280Driv self, float target) { static long long posit2count(pDMC2280Driv self, double target) {
return unit2count(self, posit2unit(self, target)); return unit2count(self, posit2unit(self, target));
} }
static float count2posit(pDMC2280Driv self, long long counts) { static double count2posit(pDMC2280Driv self, long long counts) {
return unit2posit(self, count2unit(self, counts)); return unit2posit(self, count2unit(self, counts));
} }
@@ -567,7 +569,7 @@ static float count2posit(pDMC2280Driv self, long long counts) {
* \param speed in physical units, eg mm/sec degrees/sec * \param speed in physical units, eg mm/sec degrees/sec
* \return the speed in motor steps/sec * \return the speed in motor steps/sec
*/ */
static int motSpeed(pDMC2280Driv self, float axisSpeed) { static int motSpeed(pDMC2280Driv self, double axisSpeed) {
int speed; int speed;
speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5); speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5);
return speed; return speed;
@@ -580,7 +582,7 @@ static int motSpeed(pDMC2280Driv self, float axisSpeed) {
* \param acceleration in physical units, eg mm/sec^2 degrees/sec^2 * \param acceleration in physical units, eg mm/sec^2 degrees/sec^2
* \return the acceleration in motor steps/sec^2 * \return the acceleration in motor steps/sec^2
*/ */
static int motAccel(pDMC2280Driv self, float axisAccel) { static int motAccel(pDMC2280Driv self, double axisAccel) {
int accel; int accel;
accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5); accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5);
return accel; return accel;
@@ -593,7 +595,7 @@ static int motAccel(pDMC2280Driv self, float axisAccel) {
* \param deceleration in physical units, eg mm/sec^2 degrees/sec^2 * \param deceleration in physical units, eg mm/sec^2 degrees/sec^2
* \return the deceleration in motor steps/sec^2 * \return the deceleration in motor steps/sec^2
*/ */
static int motDecel(pDMC2280Driv self, float axisDecel) { static int motDecel(pDMC2280Driv self, double axisDecel) {
int decel; int decel;
decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5); decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5);
return decel; return decel;
@@ -605,8 +607,8 @@ static int motDecel(pDMC2280Driv self, float axisDecel) {
* \param self (r) provides access to the motor's data structure * \param self (r) provides access to the motor's data structure
* \return the motor position in physical units * \return the motor position in physical units
*/ */
static float motPosit(pDMC2280Driv self) { static double motPosit(pDMC2280Driv self) {
float fPos; double fPos;
if (self->abs_encoder) if (self->abs_encoder)
fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->fHome; fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->fHome;
else else
@@ -622,7 +624,7 @@ static float motPosit(pDMC2280Driv self) {
* \param target in physical units, eg mm, degrees * \param target in physical units, eg mm, degrees
* \return the absolute position in motor steps * \return the absolute position in motor steps
*/ */
static int motAbsol(pDMC2280Driv self, float target) { static int motAbsol(pDMC2280Driv self, double target) {
double absolute; double absolute;
int result; int result;
if (self->abs_encoder) { if (self->abs_encoder) {
@@ -659,7 +661,7 @@ static int motAbsol(pDMC2280Driv self, float target) {
* \param target in physical units, eg mm, degrees * \param target in physical units, eg mm, degrees
* \return the absolute position in motor steps * \return the absolute position in motor steps
*/ */
static int motCreep(pDMC2280Driv self, float target) { static int motCreep(pDMC2280Driv self, double target) {
int target_steps = motAbsol(self, target); int target_steps = motAbsol(self, target);
if (!self->abs_encoder || /* no absolute encoder */ if (!self->abs_encoder || /* no absolute encoder */
self->preseek || /* backlash preseek active */ self->preseek || /* backlash preseek active */
@@ -1000,7 +1002,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) {
if (FAILURE == DMC2280SendReceive(self, cmd, reply)) if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return FAILURE; return FAILURE;
*pos = (float) atoi(reply); *pos = (double) atol(reply);
return SUCCESS; return SUCCESS;
} }
@@ -1063,7 +1065,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
return FAILURE; return FAILURE;
return SUCCESS; return SUCCESS;
} }
snprintf(cmd, CMDLEN, "XQ #HOME,1"); snprintf(cmd, CMDLEN, "XQ #HOME,7");
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280Send(self, cmd))
return FAILURE; return FAILURE;
@@ -1082,7 +1084,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
* - 0 MOTOR BLOCKED, no significant change in position detected. * - 0 MOTOR BLOCKED, no significant change in position detected.
*/ */
static int checkMotion(void *pData) { static int checkMotion(void *pData) {
float ratio_obs, ratio_exp, ratio_cmp; double ratio_obs, ratio_exp, ratio_cmp;
long int usec_TimeDiff; long int usec_TimeDiff;
struct timeval now; struct timeval now;
@@ -1114,17 +1116,17 @@ static int checkMotion(void *pData) {
if (self->currCounts == self->lastCounts) /* prevent divide by zero */ if (self->currCounts == self->lastCounts) /* prevent divide by zero */
ratio_obs = (self->currSteps - self->lastSteps); ratio_obs = (self->currSteps - self->lastSteps);
else { else {
ratio_obs = (float) (self->currSteps - self->lastSteps); ratio_obs = (double) (self->currSteps - self->lastSteps);
ratio_obs /= (float) (self->currCounts - self->lastCounts); ratio_obs /= (double) (self->currCounts - self->lastCounts);
} }
ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX; ratio_exp = (double) self->stepsPerX / (double) self->cntsPerX;
ratio_cmp = ratio_obs / ratio_exp; ratio_cmp = ratio_obs / ratio_exp;
/* wrong signs, less than half, or more than double is trouble */ /* wrong signs, less than half, or more than double is trouble */
if (ratio_cmp < 0.0 || if (ratio_cmp < 0.0 ||
ratio_cmp > self->blockage_ratio || ratio_cmp > self->blockage_ratio ||
(1.0 / ratio_cmp) > self->blockage_ratio) { (1.0 / ratio_cmp) > self->blockage_ratio) {
char msg[132]; char msg[132];
float cmp = ratio_cmp; double cmp = ratio_cmp;
if (fabs(cmp) > 0 && fabs(cmp) < 1) if (fabs(cmp) > 0 && fabs(cmp) < 1)
cmp = 1.0 / cmp; cmp = 1.0 / cmp;
snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f", snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f",
@@ -1142,7 +1144,7 @@ static int checkMotion(void *pData) {
} else { } else {
if (self->debug) { if (self->debug) {
char msg[132]; char msg[132];
float cmp = ratio_cmp; double cmp = ratio_cmp;
if (fabs(cmp) > 0 && fabs(cmp) < 1) if (fabs(cmp) > 0 && fabs(cmp) < 1)
cmp = 1.0 / cmp; cmp = 1.0 / cmp;
snprintf(msg, sizeof(msg), "Motor %s pass: obs=%f, exp=%f, cmp=%f", snprintf(msg, sizeof(msg), "Motor %s pass: obs=%f, exp=%f, cmp=%f",
@@ -1170,10 +1172,10 @@ static int checkMotion(void *pData) {
* *
* \return true if found else false * \return true if found else false
*/ */
static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) { static bool has_var(pDMC2280Driv self, const char* vars, const char* name) {
char tmp[20];
char* p; char* p;
snprintf(tmp, 20, "%s%c=", name, self->axisLabel); char tmp[20];
snprintf(tmp, 20, "%s=", name);
p = strstr(vars, tmp); p = strstr(vars, tmp);
if (p == NULL) /* not found at all */ if (p == NULL) /* not found at all */
return false; return false;
@@ -1184,6 +1186,13 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
return false; /* found partial (ZVVVx=) only */ 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);
}
/* State Machine Events */ /* State Machine Events */
static int state_msg_callback(pAsyncTxn pCmd); static int state_msg_callback(pAsyncTxn pCmd);
@@ -1195,31 +1204,17 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd);
* *
* \param self access to the drive data * \param self access to the drive data
*/ */
#if 1
static int cmdStatus(pDMC2280Driv self) { static int cmdStatus(pDMC2280Driv self) {
char cmd[CMDLEN]; char cmd[CMDLEN];
/* TODO: Use POSx, ENCx, RUNx if it has these variables */ /* TODO: Use POSx, ENCx, RUNx, SWIx if it has these variables */
snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,_TS%c,%s%c,_TI0,_TI1,_XQ0", snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,_TI0,_TI1,_XQ0",
(self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel, (self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel,
(self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel, (self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel,
self->axisLabel, (self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel,
(self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel); (self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel);
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendCmd(self, cmd, state_msg_callback);
} }
#else
static int cmdStatus(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->has_airpads == 3)
snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,RUN%c,_TI0,_TI1,_XQ0",
self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel);
else
snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,_BG%c,_TI0,_TI1,_XQ0",
self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel);
return DMC_SendCmd(self, cmd, state_msg_callback);
}
#endif
static int cmdVars(pDMC2280Driv self) { static int cmdVars(pDMC2280Driv self) {
char cmd[CMDLEN]; char cmd[CMDLEN];
@@ -1330,6 +1325,8 @@ static int rspStatus(pDMC2280Driv self, const char* text) {
static int rspVars(pDMC2280Driv self, const char* text) { static int rspVars(pDMC2280Driv self, const char* text) {
self->variables = 0; self->variables = 0;
if (has_var(self, text, "HOMERUN"))
self->variables |= VAR_HOM;
if (has_var_x(self, text, "REQ")) if (has_var_x(self, text, "REQ"))
self->variables |= VAR_REQ; self->variables |= VAR_REQ;
if (has_var_x(self, text, "RSP")) if (has_var_x(self, text, "RSP"))
@@ -1344,6 +1341,8 @@ static int rspVars(pDMC2280Driv self, const char* text) {
self->variables |= VAR_HLT; self->variables |= VAR_HLT;
if (has_var_x(self, text, "ENC")) if (has_var_x(self, text, "ENC"))
self->variables |= VAR_ENC; self->variables |= VAR_ENC;
if (has_var_x(self, text, "SWI"))
self->variables |= VAR_SWI;
if ((self->variables & VAR_REQ) && if ((self->variables & VAR_REQ) &&
(self->variables & VAR_RSP)) { (self->variables & VAR_RSP)) {
self->has_airpads = 2; self->has_airpads = 2;
@@ -1356,6 +1355,14 @@ static int rspVars(pDMC2280Driv self, const char* text) {
return 1; return 1;
} }
static int rspPoll(pDMC2280Driv self, const char* text) {
int iReply = atoi(text);
if (iReply < 0)
snprintf(self->dmc2280Error, CMDLEN,
"MOTOR CONTROLLER REQ ERROR: %d", iReply);
return iReply;
}
/* State Functions */ /* State Functions */
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event); static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event);
@@ -1482,7 +1489,7 @@ static void unhandled_event(pDMC2280Driv self, pEvtEvent event) {
state_name(self->myState), state_name(self->myState),
self->subState, self->subState,
event_name(event, text, CMDLEN)); event_name(event, text, CMDLEN));
SICSLogWrite(line, eStatus); SICSLogWrite(line, eError);
} }
static void handle_event(pDMC2280Driv self, pEvtEvent event) { static void handle_event(pDMC2280Driv self, pEvtEvent event) {
@@ -1624,7 +1631,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* Status Response */ if (self->subState == 1) { /* Status Response */
int iRet; int iRet;
float fDelta; long lDelta;
iRet = rspStatus(self, pCmd->inp_buf); iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
@@ -1636,10 +1643,10 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
} }
/* if the motor moved, update any observers */ /* if the motor moved, update any observers */
if (self->abs_encoder) if (self->abs_encoder)
fDelta = fabs(self->currCounts - self->lastCounts); lDelta = fabs(self->currCounts - self->lastCounts);
else else
fDelta = fabs(self->currSteps - self->lastSteps); lDelta = fabs(self->currSteps - self->lastSteps);
if (fDelta > 10) { if (lDelta > 10) {
set_lastMotion(self, self->currSteps, self->currCounts); set_lastMotion(self, self->currSteps, self->currCounts);
report_motion(self); report_motion(self);
} }
@@ -1759,12 +1766,19 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
} }
} }
else if (self->subState == 2) { /* Poll */ else if (self->subState == 2) { /* Poll */
float fReply; int iRet;
fReply = (float) atof(pCmd->inp_buf); iRet = rspPoll(self, pCmd->inp_buf);
if (fReply > 0) { if (iRet > 0) { /* state has changed to ON */
change_state(self, DMCState_MotorOn); change_state(self, DMCState_MotorOn);
return; return;
} }
else if (iRet < 0) {
/* Handle error return */
self->errorCode = RUNERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorOff);
return;
}
/* TODO handle airpad timeout */ /* TODO handle airpad timeout */
NetWatchRegisterTimer(&self->state_timer, NetWatchRegisterTimer(&self->state_timer,
AIR_POLL_TIMER, AIR_POLL_TIMER,
@@ -1801,7 +1815,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* Status Response */ if (self->subState == 1) { /* Status Response */
int iRet, absolute; int iRet, absolute;
float target; double target;
iRet = rspStatus(self, pCmd->inp_buf); iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
@@ -2004,7 +2018,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
* If this was a pre-seek then compute the next iteration * If this was a pre-seek then compute the next iteration
*/ */
if (self->preseek) { if (self->preseek) {
float target; double target;
int absolute; int absolute;
self->preseek = 0; self->preseek = 0;
target = self->fTarget; target = self->fTarget;
@@ -2239,12 +2253,20 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
} }
} }
else if (self->subState == 1) { /* Poll */ else if (self->subState == 1) { /* Poll */
float fReply; int iRet;
fReply = (float) atof(pCmd->inp_buf); iRet = rspPoll(self, pCmd->inp_buf);
if (fReply == 0) { if (iRet == 0) { /* state has changed to OFF */
change_state(self, DMCState_MotorOff); change_state(self, DMCState_MotorOff);
return; return;
} }
else if (iRet < 0) {
/* Handle error return */
self->errorCode = RUNERROR;
self->driver_status = HWFault;
change_state(self, DMCState_MotorOff);
return;
}
/* TODO handle airpad timeout */
} }
NetWatchRegisterTimer(&self->state_timer, NetWatchRegisterTimer(&self->state_timer,
AIR_POLL_TIMER, AIR_POLL_TIMER,
@@ -2647,6 +2669,10 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->fHome; *fValue = self->fHome;
return 1; return 1;
} }
if(strcasecmp(name, "stepsPerX") == 0) {
*fValue = self->stepsPerX;
return 1;
}
if(strcasecmp(name,HARDLOWERLIM) == 0) { if(strcasecmp(name,HARDLOWERLIM) == 0) {
*fValue = self->fLower; *fValue = self->fLower;
return 1; return 1;
@@ -2737,6 +2763,10 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->absEncHome; *fValue = self->absEncHome;
return 1; return 1;
} }
if (strcasecmp(name,"cntsperx") == 0) {
*fValue = self->cntsPerX;
return 1;
}
if (self->has_fsm) { if (self->has_fsm) {
if(strcasecmp(name,"creep_offset") == 0) { if(strcasecmp(name,"creep_offset") == 0) {
*fValue = self->creep_offset; *fValue = self->creep_offset;
@@ -3395,7 +3425,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
free(pNew); free(pNew);
return NULL; return NULL;
} }
sscanf(pPtr,"%f",&(pNew->stepsPerX)); sscanf(pPtr,"%lg",&(pNew->stepsPerX));
if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL) if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL)
pNew->motorHome=0; pNew->motorHome=0;
else else
@@ -3442,7 +3472,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL) if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL)
pNew->cntsPerX = 1.0; pNew->cntsPerX = 1.0;
else else
sscanf(pPtr,"%f",&(pNew->cntsPerX)); sscanf(pPtr,"%lg",&(pNew->cntsPerX));
if (pNew->has_fsm) { if (pNew->has_fsm) {
/* CREEP_OFFSET: this controls unidirectional driving */ /* CREEP_OFFSET: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL) if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL)
@@ -3538,14 +3568,14 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
} }
else if(strcasecmp("data", argv[1]) == 0) { else if(strcasecmp("data", argv[1]) == 0) {
char line[132]; char line[132];
snprintf(line, 132, "%s.motor_step = %f %s", self->name, snprintf(line, 132, "%s.motor_step = %10g %s", self->name,
1.0 / self->stepsPerX, self->units); 1.0 / self->stepsPerX, self->units);
SCWrite(pCon, line, eValue); SCWrite(pCon, line, eValue);
if (self->abs_encoder) { if (self->abs_encoder) {
snprintf(line, 132, "%s.encoder_count = %f %s", self->name, snprintf(line, 132, "%s.encoder_count = %10g %s", self->name,
1.0 / self->cntsPerX, self->units); 1.0 / self->cntsPerX, self->units);
SCWrite(pCon, line, eValue); SCWrite(pCon, line, eValue);
snprintf(line, 132, "%s.steps_per_count = %f steps", self->name, snprintf(line, 132, "%s.steps_per_count = %10g steps", self->name,
self->stepsPerX / self->cntsPerX); self->stepsPerX / self->cntsPerX);
SCWrite(pCon, line, eValue); SCWrite(pCon, line, eValue);
} }