diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index e40d6037..92e1bad0 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -1,3 +1,4 @@ +#define DEPRECATED /** \file motor_dmc2280.c * \brief Driver for Galil DMC2280 motor controller. * @@ -17,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -168,6 +170,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ #define MOTOR_POLL_SLOW 1000 #define ON_SETTLE_TIMER 1000 #define MAX_CREEP_STEPS 100 +#define MAX_RESTARTS 0 #define AIRPADS_DOWN 0 #define AIRPADS_RAISE 1 @@ -619,20 +622,7 @@ static void set_lastMotion(pDMC2280Driv self, float steps, float counts) { gettimeofday(&(self->time_lastPos_set), NULL); } -static int set_currMotion(pDMC2280Driv self, const char* text) { - 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; -} - +#ifndef DEPRECATED /** \brief Reads motion. * * \param *steps motor steps @@ -653,6 +643,7 @@ static int readMotion(pDMC2280Driv self, float *steps, float *counts) { return FAILURE; return SUCCESS; } +#endif /** \brief Reads absolute encoder. * @@ -739,6 +730,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { return SUCCESS; } +#ifndef DEPRECATED /** * \brief calculate and issue the motion commands * @@ -845,15 +837,6 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ s_stps, s_cnts, 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 } else { newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5); @@ -989,6 +972,7 @@ static int motoff_timeout(void* context, int mode) { DMC2280Send(self, cmd); return 0; } +#endif /** \brief Check if the axis has moved significantly since * the last check. @@ -1018,10 +1002,12 @@ static int checkMotion(void *pData) { steps = self->currSteps; counts = self->currCounts; } +#ifndef DEPRECATED else { if (FAILURE == readMotion(self, &steps, &counts)) return 0; } +#endif set_lastMotion(pData, steps, counts); return 1; } @@ -1036,10 +1022,12 @@ static int checkMotion(void *pData) { steps = self->currSteps; counts = self->currCounts; } +#ifndef DEPRECATED else { if (FAILURE == readMotion(self, &steps, &counts)) return 0; } +#endif /* If not stepping, then not blocked */ if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ @@ -1088,6 +1076,7 @@ static int checkMotion(void *pData) { return 1; } +#ifndef DEPRECATED /** * \brief handle the run command for a motor with airpads * @@ -1099,33 +1088,100 @@ static int DMC2280RunAir(pDMC2280Driv self, float fValue) { return HWFault; 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); -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); +/* State Callbacks */ 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); +/** + * \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) { if (func == DMCState_Unknown) return "DMCState_Unknown"; 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_Moving) return "DMCState_Moving"; if (func == DMCState_MotorHalt) return "DMCState_MotorHalt"; 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_WaitStatus) return "DMCState_WaitStatus"; return ""; @@ -1295,18 +1351,14 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value); if (FAILURE == DMC2280Send(self, cmd)) break; - snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", - self->axisLabel, - self->axisLabel, - self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); + request_status(self); return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; if (pCmd->out_buf[0] == 'M') { /* MG */ int iRet; - iRet = set_currMotion(self, pCmd->inp_buf); + iRet = process_status(self, pCmd->inp_buf); if (iRet == 0) break; set_lastMotion(self, self->currSteps, self->currCounts); @@ -1314,13 +1366,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { change_state(self, DMCState_Idle); 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"); -#endif DMC_SendCmd(self, cmd, state_msg_callback); return; } @@ -1329,14 +1375,15 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { return; } if (pCmd->out_buf[0] == 'L') { /* LV */ - char req[12]; - char rdy[12]; - 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; - } + if (has_var_x(self, pCmd->inp_buf, "REQ")) + if (has_var_x(self, pCmd->inp_buf, "RSP")) { + 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); return; } @@ -1355,11 +1402,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { case eTimerEvent: - snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", - self->axisLabel, - self->axisLabel, - self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); + request_status(self); change_state(self, DMCState_WaitStatus); return; case eMessageEvent: @@ -1373,12 +1416,13 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { if (iRet != 2) break; if (fReply < 0) { - if (self->subState > 0) { + if (self->subState >= MAX_RESTARTS) { + /* TODO: handle fault logging */ self->driver_status = HWFault; self->errorCode = THREADZERO; return; } - DMC_SendCmd(self, "XQ #AUTO,0", state_msg_callback); + DMC_SendCmd(self, "XQ #THREAD0,0", state_msg_callback); return; } /* Handle limit switches */ @@ -1397,25 +1441,21 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { } if (self->has_airpads == 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); - DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_AirOn); } else { 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; } - 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); DMC_SendCmd(self, cmd, state_msg_callback); - self->subState = 1; + self->subState += 1; } } while (0); break; @@ -1447,11 +1487,11 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { self->errorCode = STATEERROR; } -static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) { +static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { case eTimerEvent: - if (self->has_airpads == 2) + if (self->has_airpads == 2 || self->has_airpads == 3) snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); else snprintf(cmd, CMDLEN, "MG APDONE"); @@ -1460,7 +1500,13 @@ static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) { case eMessageEvent: do { 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= */ NetWatchRegisterTimer(&self->state_timer, AIR_POLL_TIMER, @@ -1498,25 +1544,15 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { case eTimerEvent: - snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", - self->axisLabel, - self->axisLabel, - self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); + request_status(self); return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; - 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] == 'M') { /* MG */ + if (pCmd->out_buf[0] == 'M') { /* MG */ int iRet, absolute; float target; - iRet = set_currMotion(self, pCmd->inp_buf); + iRet = process_status(self, pCmd->inp_buf); if (iRet == 0) break; set_lastMotion(self, self->currSteps, self->currCounts); @@ -1553,12 +1589,14 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { } self->creep_val = 0; absolute = motCreep(self, target); + /* TODO set DSTx for has_airpads == 3 */ snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute); DMC_SendCmd(self, cmd, state_msg_callback); return; } else if (pCmd->out_buf[0] == 'P') { /* PA */ self->stepCount = 1; + /* TODO set RUNx for has_airpads == 3 */ snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); DMC_SendCmd(self, cmd, state_msg_callback); change_state(self, DMCState_Moving); @@ -1579,11 +1617,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { case eTimerEvent: - snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", - self->axisLabel, - self->axisLabel, - self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); + request_status(self); return; case eMessageEvent: do { @@ -1603,7 +1637,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { else if (pCmd->out_buf[0] == 'M') { /* MG */ int iRet, iFlags; 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) break; iFlags = self->currFlags; @@ -1779,11 +1813,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) char cmd[CMDLEN]; switch (event->event_type) { case eTimerEvent: - snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c", - self->axisLabel, - self->axisLabel, - self->axisLabel); - DMC_SendCmd(self, cmd, state_msg_callback); + request_status(self); return; case eMessageEvent: do { @@ -1796,7 +1826,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) } else if (pCmd->out_buf[0] == 'M') { /* MG */ int iRet, iFlags; - iRet = set_currMotion(self, pCmd->inp_buf); + iRet = process_status(self, pCmd->inp_buf); if (iRet == 0) break; iFlags = self->currFlags; @@ -1809,19 +1839,19 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) if (self->has_airpads == 1) { snprintf(cmd, CMDLEN, "FTUBE=0"); DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_AirOff); + change_state(self, DMCState_MotorStop); 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); DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_AirOff); + change_state(self, DMCState_MotorStop); return; } else { snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorOff); + change_state(self, DMCState_MotorStop); return; } } @@ -1842,18 +1872,18 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { if (self->has_airpads == 1) { snprintf(cmd, CMDLEN, "FTUBE=0"); 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); DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_AirOff); + change_state(self, DMCState_MotorStop); return; } else { snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); DMC_SendCmd(self, cmd, state_msg_callback); - change_state(self, DMCState_MotorOff); + change_state(self, DMCState_MotorStop); } return; case eCommandEvent: @@ -1881,11 +1911,11 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { self->errorCode = STATEERROR; } -static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) { +static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { char cmd[CMDLEN]; switch (event->event_type) { case eTimerEvent: - if (self->has_airpads == 2) + if (self->has_airpads == 2 || self->has_airpads == 3) snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); else snprintf(cmd, CMDLEN, "MG APDONE"); @@ -1897,9 +1927,12 @@ static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) { if (pCmd->out_buf[0] == 'F' || /* FTUBE */ 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; - fReply = (float) atof(pCmd->inp_buf); + if (pCmd->out_buf[1] == 'O') /* MO */ + fReply = 1; + else + fReply = (float) atof(pCmd->inp_buf); if (fReply == 0) { NetWatchRegisterTimer(&self->state_timer, ON_SETTLE_TIMER, @@ -1964,9 +1997,9 @@ static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event) { case eMessageEvent: do { 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; - iRet = set_currMotion(self, pCmd->inp_buf); + iRet = process_status(self, pCmd->inp_buf); if (iRet == 0) break; change_state(self, DMCState_Idle); @@ -2029,26 +2062,24 @@ static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event) { * */ static int DMC2280GetPos(void *pData, float *fPos){ pDMC2280Driv self = NULL; +#ifndef DEPRECATED char reply[CMDLEN]; char cmd[CMDLEN]; float absEncPos, motorPos; reply[0]='\0'; +#endif self = (pDMC2280Driv)pData; assert(self != NULL); -#if 1 if (self->has_fsm) { if (self->myState == DMCState_Unknown) SicsWait(1); - if (self->abs_encoder) - *fPos = (self->currCounts - self->absEncHome)/self->cntsPerX + self->home; - else - *fPos = (self->currSteps - self->motorHome)/self->stepsPerX + self->home; + *fPos = motPosit(self); return OKOK; } -#endif +#ifndef DEPRECATED if (1 == self->abs_encoder) { if (readAbsEnc(self, &absEncPos) == FAILURE) return HWFault; @@ -2060,6 +2091,7 @@ static int DMC2280GetPos(void *pData, float *fPos){ motorPos =(float)atof(reply); *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home; } +#endif return OKOK; } @@ -2108,7 +2140,7 @@ static int DMC2280Run(void *pData,float fValue){ if (self->has_fsm) { /* TODO: FIXME and handle RUN command everywhere */ - while (self->myState == DMCState_AirOff + while (self->myState == DMCState_MotorStop || self->myState == DMCState_MotorOff) { SicsWait(1); } @@ -2116,6 +2148,7 @@ static int DMC2280Run(void *pData,float fValue){ return 1; } +#ifndef DEPRECATED /* * Note: this passes control to a timer routine */ @@ -2124,6 +2157,10 @@ static int DMC2280Run(void *pData,float fValue){ return DMC2280RunAir(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){ pDMC2280Driv self = NULL; +#ifndef DEPRECATED char cmd[CMDLEN]; int switches; 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; +#endif self = (pDMC2280Driv)pData; assert(self != NULL); @@ -2156,6 +2190,7 @@ static int DMC2280Status(void *pData){ if (self->has_fsm) { return self->driver_status; } +#ifndef DEPRECATED /* * If we are waiting for the motor or airpads then we * are busy @@ -2217,31 +2252,6 @@ static int DMC2280Status(void *pData){ self->errorCode = ERRORLIM; 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 */ @@ -2303,6 +2313,10 @@ static int DMC2280Status(void *pData){ } return HWIdle; } +#else + assert(self->has_fsm); + return 0; +#endif } /** \brief DMC2280 implementation of the GetError * method in the MotorDriver interface. @@ -2472,7 +2486,9 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ */ static int DMC2280Halt(void *pData){ pDMC2280Driv self = NULL; +#ifndef DEPRECATED char cmd[CMDLEN]; +#endif self = (pDMC2280Driv)pData; assert(self != NULL); @@ -2481,6 +2497,7 @@ static int DMC2280Halt(void *pData){ state_cmd_execute(self, CMD_HALT); return 1; } +#ifndef DEPRECATED /* Stop motor */ snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); if (FAILURE == DMC2280Send(self, cmd)) @@ -2491,6 +2508,10 @@ static int DMC2280Halt(void *pData){ return HWFault; else return 1; +#else + assert(self->has_fsm); + return 0; +#endif } /** \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) { snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome); SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX); + SCWrite(pCon, buffer, eStatus); if (self->has_fsm) { - snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX); - SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset); SCWrite(pCon, buffer, eStatus); 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; else { sscanf(pPtr,"%d",&(pNew->has_fsm)); + assert(pNew->has_fsm); /* DEPRECATED */ } if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) { @@ -3206,6 +3228,17 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { 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,"stepsperx",_REQUIRED)) == NULL) { KillDMC2280(pNew); free(pNew); @@ -3286,6 +3319,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { 0, state_tmr_callback, pNew); } +#ifndef DEPRECATED else { char cmd[CMDLEN]; /* Set speed */ @@ -3304,6 +3338,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { * to get a sensible initial list output */ } +#endif return (MotorDriver *)pNew; } @@ -3348,6 +3383,27 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, } 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) { if (argc > 2) { strncpy(self->units, argv[2], sizeof(self->units));