From 3926e47ab50df3ea4a740d95ce7ed1fc644f8e52 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Fri, 7 Dec 2007 10:38:34 +1100 Subject: [PATCH] Change floats to doubles, controller virtual motor variables, a little refactoring r2264 | dcl | 2007-12-07 10:38:34 +1100 (Fri, 07 Dec 2007) | 2 lines --- site_ansto/motor_dmc2280.c | 190 +++++++++++++++++++++---------------- 1 file changed, 110 insertions(+), 80 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 30fc6d9f..8990e787 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -78,13 +78,15 @@ struct EvtEvent_s { } event; }; -#define VAR_REQ (1<<0) -#define VAR_RSP (1<<1) -#define VAR_RUN (1<<2) -#define VAR_DST (1<<3) -#define VAR_POS (1<<4) -#define VAR_HLT (1<<5) -#define VAR_ENC (1<<6) +#define VAR_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) static pAsyncProtocol DMC2280_Protocol = NULL; @@ -133,19 +135,19 @@ struct __MoDriv { char axisLabel; char lastCmd[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 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 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 currFlags; int currSteps; int currCounts; - float currPosition; /**< Position at last position check */ - float lastPosition; /**< Position at last position check */ + double currPosition; /**< Position at last position check */ + double lastPosition; /**< Position at last position check */ int lastSteps; int lastCounts; int thread0; /**< last read of _XQ0 */ @@ -162,8 +164,8 @@ struct __MoDriv { int blockage_fail; /**< flag =1 if we should fail the motor */ int has_airpads; /**< Flag = 1 if there is are airpads for this motor */ float backlash_offset; /**< signed offset to drive from */ - float fTarget; /**< target passed from SICS to timer callback */ - float fPreseek; /**< preseek target when preseek is active */ + double fTarget; /**< target passed from SICS to timer callback */ + double fPreseek; /**< preseek target when preseek is active */ int settle; /**< motor settling time in seconds */ struct timeval time_settle_done; /**< time when settling will be over */ int airpad_state; /**< state of the airpads finite state machine */ @@ -383,7 +385,7 @@ static bool check_positions(pDMC2280Driv self) { return false; } -static long long unit2count(pDMC2280Driv self, float target) { +static long long unit2count(pDMC2280Driv self, double target) { double absolute; long long result; if (self->abs_encoder == 0) { @@ -413,8 +415,8 @@ static long long unit2count(pDMC2280Driv self, float target) { return result; } -static float count2unit(pDMC2280Driv self, long long counts) { - float fPos; +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", @@ -427,7 +429,7 @@ static float count2unit(pDMC2280Driv self, long long counts) { } #ifdef POSIT_COUNTS -static long long posit2count(pDMC2280Driv self, float target) { +static long long posit2count(pDMC2280Driv self, double target) { double absolute; long long result; if (self->abs_encoder == 0) { @@ -463,9 +465,9 @@ static long long posit2count(pDMC2280Driv self, float target) { return result; } -static float count2posit(pDMC2280Driv self, long long counts) { +static double count2posit(pDMC2280Driv self, long long counts) { int i, j; - float fPos; + double fPos; if (self->abs_encoder == 0) { char line[CMDLEN]; snprintf(line, CMDLEN, "count2posit motor %s has no absolute encoder", @@ -498,19 +500,19 @@ static float count2posit(pDMC2280Driv self, long long counts) { return fPos; } -static float unit2posit(pDMC2280Driv self, float target) { +static double unit2posit(pDMC2280Driv self, double 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)); } #else -static float unit2posit(pDMC2280Driv self, float target) { +static double unit2posit(pDMC2280Driv self, double target) { int i, j; - float fPos; + double fPos; if (!check_positions(self)) return 0; @@ -536,7 +538,7 @@ static float unit2posit(pDMC2280Driv self, float target) { return fPos; } -static float posit2unit(pDMC2280Driv self, float target) { +static double posit2unit(pDMC2280Driv self, double target) { double result; int i = ((int) target) - 1; if (i < 0) { @@ -550,11 +552,11 @@ static float posit2unit(pDMC2280Driv self, float target) { return result; } -static long long posit2count(pDMC2280Driv self, float target) { +static long long posit2count(pDMC2280Driv self, double 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)); } @@ -567,7 +569,7 @@ static float count2posit(pDMC2280Driv self, long long counts) { * \param speed in physical units, eg mm/sec degrees/sec * \return the speed in motor steps/sec */ -static int motSpeed(pDMC2280Driv self, float axisSpeed) { +static int motSpeed(pDMC2280Driv self, double axisSpeed) { int speed; speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5); 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 * \return the acceleration in motor steps/sec^2 */ -static int motAccel(pDMC2280Driv self, float axisAccel) { +static int motAccel(pDMC2280Driv self, double axisAccel) { int accel; accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5); 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 * \return the deceleration in motor steps/sec^2 */ -static int motDecel(pDMC2280Driv self, float axisDecel) { +static int motDecel(pDMC2280Driv self, double axisDecel) { int decel; decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5); return decel; @@ -605,8 +607,8 @@ static int motDecel(pDMC2280Driv self, float axisDecel) { * \param self (r) provides access to the motor's data structure * \return the motor position in physical units */ -static float motPosit(pDMC2280Driv self) { - float fPos; +static double motPosit(pDMC2280Driv self) { + double fPos; if (self->abs_encoder) fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->fHome; else @@ -622,7 +624,7 @@ static float motPosit(pDMC2280Driv self) { * \param target in physical units, eg mm, degrees * \return the absolute position in motor steps */ -static int motAbsol(pDMC2280Driv self, float target) { +static int motAbsol(pDMC2280Driv self, double target) { double absolute; int result; if (self->abs_encoder) { @@ -659,7 +661,7 @@ static int motAbsol(pDMC2280Driv self, float target) { * \param target in physical units, eg mm, degrees * \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); if (!self->abs_encoder || /* no absolute encoder */ self->preseek || /* backlash preseek active */ @@ -1000,7 +1002,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) { if (FAILURE == DMC2280SendReceive(self, cmd, reply)) return FAILURE; - *pos = (float) atoi(reply); + *pos = (double) atol(reply); return SUCCESS; } @@ -1063,7 +1065,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { return FAILURE; return SUCCESS; } - snprintf(cmd, CMDLEN, "XQ #HOME,1"); + snprintf(cmd, CMDLEN, "XQ #HOME,7"); if (FAILURE == DMC2280Send(self, cmd)) return FAILURE; @@ -1082,7 +1084,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { * - 0 MOTOR BLOCKED, no significant change in position detected. */ static int checkMotion(void *pData) { - float ratio_obs, ratio_exp, ratio_cmp; + double ratio_obs, ratio_exp, ratio_cmp; long int usec_TimeDiff; struct timeval now; @@ -1114,17 +1116,17 @@ static int checkMotion(void *pData) { if (self->currCounts == self->lastCounts) /* prevent divide by zero */ ratio_obs = (self->currSteps - self->lastSteps); else { - ratio_obs = (float) (self->currSteps - self->lastSteps); - ratio_obs /= (float) (self->currCounts - self->lastCounts); + ratio_obs = (double) (self->currSteps - self->lastSteps); + 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; /* 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]; - float cmp = ratio_cmp; + double cmp = ratio_cmp; if (fabs(cmp) > 0 && fabs(cmp) < 1) cmp = 1.0 / cmp; snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f", @@ -1142,7 +1144,7 @@ static int checkMotion(void *pData) { } else { if (self->debug) { char msg[132]; - float cmp = ratio_cmp; + 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", @@ -1170,10 +1172,10 @@ static int checkMotion(void *pData) { * * \return true if found else false */ -static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) { - char tmp[20]; +static bool has_var(pDMC2280Driv self, const char* vars, const char* name) { char* p; - snprintf(tmp, 20, "%s%c=", name, self->axisLabel); + char tmp[20]; + snprintf(tmp, 20, "%s=", name); p = strstr(vars, tmp); if (p == NULL) /* not found at all */ 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 */ } +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 */ 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 */ -#if 1 static int cmdStatus(pDMC2280Driv self) { char cmd[CMDLEN]; - /* TODO: Use POSx, ENCx, RUNx if it has these variables */ - snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,_TS%c,%s%c,_TI0,_TI1,_XQ0", + /* TODO: Use POSx, ENCx, RUNx, SWIx if it has these variables */ + 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_ENC) ? "ENC" : "_TP", self->axisLabel, - self->axisLabel, + (self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel, (self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel); 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) { char cmd[CMDLEN]; @@ -1330,6 +1325,8 @@ static int rspStatus(pDMC2280Driv self, const char* text) { static int rspVars(pDMC2280Driv self, const char* text) { self->variables = 0; + if (has_var(self, text, "HOMERUN")) + self->variables |= VAR_HOM; if (has_var_x(self, text, "REQ")) self->variables |= VAR_REQ; if (has_var_x(self, text, "RSP")) @@ -1344,6 +1341,8 @@ static int rspVars(pDMC2280Driv self, const char* text) { 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 ((self->variables & VAR_REQ) && (self->variables & VAR_RSP)) { self->has_airpads = 2; @@ -1356,6 +1355,14 @@ static int rspVars(pDMC2280Driv self, const char* text) { 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 */ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event); @@ -1482,7 +1489,7 @@ static void unhandled_event(pDMC2280Driv self, pEvtEvent event) { state_name(self->myState), self->subState, event_name(event, text, CMDLEN)); - SICSLogWrite(line, eStatus); + SICSLogWrite(line, eError); } 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; if (self->subState == 1) { /* Status Response */ int iRet; - float fDelta; + long lDelta; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; @@ -1636,10 +1643,10 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { } /* if the motor moved, update any observers */ if (self->abs_encoder) - fDelta = fabs(self->currCounts - self->lastCounts); + lDelta = fabs(self->currCounts - self->lastCounts); else - fDelta = fabs(self->currSteps - self->lastSteps); - if (fDelta > 10) { + lDelta = fabs(self->currSteps - self->lastSteps); + if (lDelta > 10) { set_lastMotion(self, self->currSteps, self->currCounts); report_motion(self); } @@ -1759,12 +1766,19 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { } } else if (self->subState == 2) { /* Poll */ - float fReply; - fReply = (float) atof(pCmd->inp_buf); - if (fReply > 0) { + 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_MotorOff); + return; + } /* TODO handle airpad timeout */ NetWatchRegisterTimer(&self->state_timer, AIR_POLL_TIMER, @@ -1801,7 +1815,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* Status Response */ int iRet, absolute; - float target; + double target; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) 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 (self->preseek) { - float target; + double target; int absolute; self->preseek = 0; target = self->fTarget; @@ -2239,12 +2253,20 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { } } else if (self->subState == 1) { /* Poll */ - float fReply; - fReply = (float) atof(pCmd->inp_buf); - if (fReply == 0) { + int iRet; + iRet = rspPoll(self, pCmd->inp_buf); + if (iRet == 0) { /* state has changed to OFF */ change_state(self, DMCState_MotorOff); 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, AIR_POLL_TIMER, @@ -2647,6 +2669,10 @@ static int DMC2280GetPar(void *pData, char *name, *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; @@ -2737,6 +2763,10 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->absEncHome; return 1; } + if (strcasecmp(name,"cntsperx") == 0) { + *fValue = self->cntsPerX; + return 1; + } if (self->has_fsm) { if(strcasecmp(name,"creep_offset") == 0) { *fValue = self->creep_offset; @@ -3395,7 +3425,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { free(pNew); return NULL; } - sscanf(pPtr,"%f",&(pNew->stepsPerX)); + sscanf(pPtr,"%lg",&(pNew->stepsPerX)); if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL) pNew->motorHome=0; else @@ -3442,7 +3472,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL) pNew->cntsPerX = 1.0; else - sscanf(pPtr,"%f",&(pNew->cntsPerX)); + sscanf(pPtr,"%lg",&(pNew->cntsPerX)); if (pNew->has_fsm) { /* CREEP_OFFSET: this controls unidirectional driving */ 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) { 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); SCWrite(pCon, line, eValue); 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); 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); SCWrite(pCon, line, eValue); }