From 84f883f5d737ebe51b6b5907ae0158503c628832 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Fri, 4 May 2007 14:01:19 +1000 Subject: [PATCH] Implement finite state machine model. Improved backlash (unidirectional motor driving). Precision motor creeping. Ensure thread zero is operating. Floating point steps and counts per unit. "send", "reset", "state" and "trace" commands. r1939 | dcl | 2007-05-04 14:01:19 +1000 (Fri, 04 May 2007) | 2 lines --- site_ansto/motor_dmc2280.c | 1149 +++++++++++++++++++++++++++++++++++- 1 file changed, 1132 insertions(+), 17 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index b57a7cef..008c610e 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -38,11 +38,41 @@ * \see getDMCSetting */ enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration}; +enum commandtype {CMD_RUN=1, CMD_HALT=2}; + + typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv; typedef struct __command Command, *pCommand; typedef int (*CommandCallback)(pCommand pCmd); +enum eventtype {eTimerEvent, eMessageEvent, eCommandEvent, eTimeoutEvent}; +typedef struct EvtEvent_s EvtEvent, *pEvtEvent; + +typedef void (*StateFunc)(pDMC2280Driv self, pEvtEvent event); + +typedef struct EvtTimer_s { } EvtTimer; + +typedef struct EvtMessage_s { + pCommand cmd; +} EvtMessage; + +typedef struct EvtCommand_s { + enum commandtype cmd_type; +} EvtCommand; + +typedef struct EvtTimeout_s { } EvtTimeout; + +struct EvtEvent_s { + enum eventtype event_type; + union { + EvtTimer tmr; + EvtMessage msg; + EvtCommand cmd; + EvtTimeout tmo; + } event; +}; + /*----------------------------------------------------------------------- The motor driver structure. Please note that the first set of fields has be identical with the fields of AbstractModriv in ../modriv.h @@ -96,6 +126,10 @@ struct __MoDriv { int absEncHome; /**< Home position in counts for abs enc */ float cntsPerX; /**< absolute encoder counts per physical unit */ int motOffDelay; /**< number of 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 */ float lastSteps; float lastCounts; @@ -114,12 +148,26 @@ struct __MoDriv { pNWTimer airpad_timer; /**< timer waiting for airpad action to complete */ pNWTimer motoff_timer; /**< motor off timer */ int debug; + int stepCount; /**< number of step operations for this move cycle */ + float creep_offset; /**< last little bit to creep in units */ + float creep_precision; + int creep_val; int preseek; /**< Flag = 1 if current move is a backlash preseek */ + int has_fsm; /**< Flag = 1 if motor has finite state machine driver model */ + int driver_status; + StateFunc myState; + int subState; + pNWTimer state_timer; /**< motor state timer */ + SConnection *trace; }; int DMC2280MotionControl = 1; /* defaults to enabled */ #define AIR_POLL_TIMER 1000 +#define MOTOR_POLL_TIMER 50 +#define ON_SETTLE_TIMER 1000 +#define MAX_CREEP_STEPS 100 + #define AIRPADS_DOWN 0 #define AIRPADS_RAISE 1 #define AIRPADS_UP 2 @@ -143,6 +191,8 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ #define MOTIONCONTROLOFF -17 #define MOTIONCONTROLUNK -18 #define MOTCMDTMO -19 /* Motor Command Timeout */ +#define STATEERROR -20 +#define THREADZERO -21 /* Thread zero not running */ /*--------------------------------------------------------------------*/ #define STATUSMOVING 128 /* Motor is moving */ #define STATUSERRORLIMIT 64 /* Number of errors exceed limit */ @@ -226,6 +276,127 @@ static int motDecel(pDMC2280Driv self, float axisDecel) { return decel; } +/** \brief Convert motor position to physical units + * using the current motor steps or encoder counts + * \param self (r) provides access to the motor's data structure + * \return the motor position in physical units + */ +static float motPosit(pDMC2280Driv self) { + float fPos; + if (self->abs_encoder) + fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->home; + else + fPos = (self->currSteps - self->motorHome) / self->stepsPerX + self->home; + return fPos; +} + +/** \brief Convert motor target in physical units to + * motor absolute position in steps + * \param self (r) provides access to the motor's data structure + * \param target in physical units, eg mm, degrees + * \return the absolute position in motor steps + */ +static int motAbsol(pDMC2280Driv self, float target) { + double absolute; + int result; + if (self->abs_encoder) { +#if 1 + /* distance of target from home in units */ + absolute = target - self->home; + /* subtract current encoder position in units */ + absolute -= (self->currCounts - self->absEncHome) / self->cntsPerX; + /* convert to motor steps */ + absolute *= self->stepsPerX; + /* add current position in steps */ + absolute += (double) self->currSteps; +#else + absolute = ((self->absEncHome - self->currCounts) + + (self->cntsPerX * (target - self->home))) * + (self->stepsPerX / self->cntsPerX) + + self->currSteps; +#endif + } + else { + absolute = (target - self->home) * self->stepsPerX + self->motorHome; + } + if (absolute > 0) + result = (int) (absolute + 0.5); + else if (absolute < 0) + result = (int) (absolute - 0.5); + else + result = (int) absolute; + if (self->debug) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "Rounding %f to %d", absolute, result); + SICSLogWrite(line, eStatus); + } + return result; +} + +/** \brief Convert motor target in physical units to + * motor absolute position in steps + * \param self (r) provides access to the motor's data structure + * \param target in physical units, eg mm, degrees + * \return the absolute position in motor steps + */ +static int motCreep(pDMC2280Driv self, float target) { + int target_steps = motAbsol(self, target); + if (!self->abs_encoder || /* no absolute encoder */ + self->preseek || /* backlash preseek active */ + self->creep_offset == 0) /* not creeping anyway */ + return target_steps; + else { + int offset = target_steps - self->currSteps; + if (self->creep_val == 0) { /* initial creep step */ + if (offset > 0) /* moving up */ + self->creep_val = -1; + else if (offset < 0) /* moving down */ + self->creep_val = +1; + } + else { + if (self->creep_val > 0) /* moving down */ + ++self->creep_val; + else + --self->creep_val; + if (abs(self->creep_val) > MAX_CREEP_STEPS && + abs(self->creep_val) > 2.0 * fabs(self->stepsPerX / self->cntsPerX)) { + if (self->debug) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s creep stopped, stepcount = %d", + self->name, self->stepCount); + SICSLogWrite(line, eStatus); + } + return target_steps; + } + } + + if (self->creep_val > 0) /* moving down handle as positive */ + offset = -offset; + if (offset > fabs(self->stepsPerX * self->creep_precision)) { + if (offset - (int) fabs(self->stepsPerX * self->creep_offset) > (int) fabs(self->stepsPerX / self->cntsPerX)) + offset = offset - fabs(self->stepsPerX * self->creep_offset); + else { + if (offset <= fabs(self->stepsPerX / self->cntsPerX)) + offset = 1; + else + offset = offset / 2; + } + self->preseek = 1; + } + if (self->creep_val > 0) /* moving down restore to negative */ + offset = - offset; + + if (self->debug) { + char text[CMDLEN]; + snprintf(text, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d", + self->currSteps, target_steps, offset, self->currSteps + offset); + SICSLogWrite(text, eStatus); + } + target_steps = self->currSteps + offset; + } + return target_steps; +} + static int DMC_Tx(void* ctx) { int iRet = 1; @@ -561,6 +732,19 @@ 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; +} /** \brief Reads motion. * @@ -939,8 +1123,14 @@ static int checkMotion(void *pData) { return 1; if (self->time_lastPos_set.tv_sec == 0) { /* first time - initialise the data */ - if (FAILURE == readMotion(self, &steps, &counts)) + if (self->has_fsm) { + steps = self->currSteps; + counts = self->currCounts; + } + else { + if (FAILURE == readMotion(self, &steps, &counts)) return 0; + } set_lastMotion(pData, steps, counts); return 1; } @@ -951,8 +1141,14 @@ static int checkMotion(void *pData) { usec_TimeDiff -= self->time_lastPos_set.tv_usec; if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval)) return 1; - if (FAILURE == readMotion(self, &steps, &counts)) - return 0; + if (self->has_fsm) { + steps = self->currSteps; + counts = self->currCounts; + } + else { + if (FAILURE == readMotion(self, &steps, &counts)) + return 0; + } /* If not stepping, then not blocked */ if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ @@ -1007,6 +1203,753 @@ static int DMC2280RunAir(pDMC2280Driv self, float fValue) { return OKOK; } +/* State Functions */ + +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 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_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_MotorOff) return "DMCState_MotorOff"; + return ""; +} + +static char* event_name(pEvtEvent event, char* text, int length) { + switch (event->event_type) { + case eTimerEvent: + snprintf(text, length, "eTimerEvent"); + return text; + case eMessageEvent: + snprintf(text, length, "eMessageEvent:%s:%s", + event->event.msg.cmd->out_buf, + event->event.msg.cmd->inp_buf); + return text; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + snprintf(text, length, "eCommandEvent:RUN"); + return text; + case CMD_HALT: + snprintf(text, length, "eCommandEvent:HALT"); + return text; + } + snprintf(text, length, "eCommandEvent:unknown"); + return text; + case eTimeoutEvent: + snprintf(text, length, "eTimeoutEvent"); + return text; + default: + snprintf(text, length, ""); + return text; + } +} + +static void report_event(pDMC2280Driv self, pEvtEvent event) { + if (self->debug || self->trace) { + char line[CMDLEN]; + char text[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s, State=%s, event=%s", self->name, + state_name(self->myState), event_name(event, text, CMDLEN)); + if (self->debug) + SICSLogWrite(line, eStatus); + if (self->trace) + SCWrite(self->trace, line, eStatus); + } +} + +static void change_state(pDMC2280Driv self, StateFunc func) { + if (self->debug || self->trace) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s, OldState=%s, NewState=%s", self->name, + state_name(self->myState), state_name(func)); + if (self->debug) + SICSLogWrite(line, eStatus); + if (self->trace) + SCWrite(self->trace, line, eStatus); + } + self->myState = func; + self->subState = 0; +} + +static int state_msg_callback(pCommand pCmd) +{ + pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; + EvtEvent event; + if (pCmd->inp_idx > 0) { + if (self->debug) { + SICSLogWrite(pCmd->out_buf, eStatus); + SICSLogWrite(pCmd->inp_buf, eStatus); + } + event.event_type = eMessageEvent; + event.event.msg.cmd = pCmd; + } + else { + if (self->debug) { + SICSLogWrite(pCmd->out_buf, eStatus); + SICSLogWrite("", eStatus); + } + event.event_type = eTimeoutEvent; + event.event.msg.cmd = pCmd; + } + if (self->debug || self->trace) + report_event(self, &event); + self->myState(self, &event); + return 0; +} + +static int state_tmr_callback(void* ctx, int mode) +{ + pDMC2280Driv self = (pDMC2280Driv) ctx; + EvtEvent event; + self->state_timer = 0; + event.event_type = eTimerEvent; + if (self->debug || self->trace) + report_event(self, &event); + self->myState(self, &event); + return 0; +} + +static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd) { + EvtEvent event; + event.event_type = eCommandEvent; + event.event.cmd.cmd_type = cmd; + if (self->debug || self->trace) + report_event(self, &event); + self->myState(self, &event); + return 0; +} + +static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { + char cmd[CMDLEN]; + int value; + float steps, counts; + + switch (event->event_type) { + case eTimerEvent: + /* Set speed */ + value = motSpeed(self, self->speed); + snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value); + if (FAILURE == DMC2280Send(self, cmd)) + break; + /* Set acceleration */ + value = motAccel(self, self->accel); + snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value); + if (FAILURE == DMC2280Send(self, cmd)) + break; + /* Set deceleration */ + value = motDecel(self, self->decel); + 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); + return; + case eMessageEvent: + do { + pCommand pCmd = event->event.msg.cmd; + if (pCmd->out_buf[0] == 'M') { /* MG */ + int iRet; + iRet = set_currMotion(self, pCmd->inp_buf); + if (iRet == 0) + break; + set_lastMotion(self, self->currSteps, self->currCounts); + if (self->abs_encoder == 0) { + change_state(self, DMCState_Idle); + return; + } + value = ((counts - self->absEncHome)/ self->cntsPerX) * self->stepsPerX; + self->currSteps = value; + snprintf(cmd, CMDLEN, "DP%c=%d", self->axisLabel, value); + DMC_SendCmd(self, cmd, state_msg_callback); + return; + } + if (pCmd->out_buf[0] == 'D') { /* DP */ + change_state(self, DMCState_Idle); + return; + } + } + while (0); + break; + case eTimeoutEvent: + /* TODO: handle timeout */ + break; + } + self->errorCode = STATEERROR; +} + +static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { + char cmd[CMDLEN]; + switch (event->event_type) { + case eMessageEvent: + do { + pCommand pCmd = event->event.msg.cmd; + if (pCmd->out_buf[0] == 'M') { /* MG _XQ0,_TSx */ + float fReply; + int iRet, iFlags; + int fwd_limit_active, rvrs_limit_active, errorlimit; + iRet = sscanf(pCmd->inp_buf, "%f %d", &fReply, &iFlags); + if (iRet != 2) + break; + if (fReply < 0) { + if (self->subState > 0) { + self->driver_status = HWFault; + self->errorCode = THREADZERO; + return; + } + DMC_SendCmd(self, "XQ #AUTO,0", state_msg_callback); + return; + } + /* Handle limit switches */ + fwd_limit_active = !(iFlags & STATUSFWDLIMIT); + rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT); + errorlimit = (iFlags & STATUSERRORLIMIT); + if (fwd_limit_active && rvrs_limit_active) { + self->errorCode = IMPOSSIBLE_LIM_SW; + self->driver_status = HWFault; +; + } else if (errorlimit) { + self->errorCode = ERRORLIM; + self->driver_status = HWFault; + } + if (self->driver_status == HWFault) { + return; + } + if (self->has_airpads) { + snprintf(cmd, CMDLEN, "FTUBE=1"); + 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); + } + return; + } + if (pCmd->out_buf[0] == 'X') { /* XQ #AUTO,0 */ + snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + self->subState = 1; + } + } while (0); + break; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + if (self->pMot == NULL) + self->pMot = FindMotor(pServ->pSics, self->name); + self->driver_status = HWBusy; + snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + self->subState = 0; + return; + case CMD_HALT: + /* TODO: handle halt command */ + return; + } + break; + case eTimeoutEvent: + /* TODO handle message timeout */ + break; + } + self->errorCode = STATEERROR; +} + +static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) { + switch (event->event_type) { + case eTimerEvent: + DMC_SendCmd(self, "MG APDONE", state_msg_callback); + return; + case eMessageEvent: + do { + pCommand pCmd = event->event.msg.cmd; + if (pCmd->out_buf[0] == 'F') { /* FTUBE */ + NetWatchRegisterTimer(&self->state_timer, + AIR_POLL_TIMER, + state_tmr_callback, self); + return; + } + else if (pCmd->out_buf[0] == 'M') { /* MG APDONE */ + float fReply; + fReply = (float) atof(pCmd->inp_buf); + if (fReply > 0) { + NetWatchRegisterTimer(&self->state_timer, + ON_SETTLE_TIMER, + state_tmr_callback, self); + change_state(self, DMCState_MotorOn); + return; + } + /* TODO handle airpad timeout */ + NetWatchRegisterTimer(&self->state_timer, + AIR_POLL_TIMER, + state_tmr_callback, self); + return; + } + } while (0); + break; + case eTimeoutEvent: + /* TODO handle message timeout */ + break; + } + self->errorCode = STATEERROR; +} + +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); + return; + case eMessageEvent: + do { + pCommand 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 */ + int iRet, absolute; + float steps, counts, target; + iRet = set_currMotion(self, pCmd->inp_buf); + if (iRet == 0) + break; + set_lastMotion(self, self->currSteps, self->currCounts); + /* compute position for PA command */ + target = self->fTarget; + self->preseek = 0; + if (self->backlash_offset) { + if (self->backlash_offset > 0) { + /* + * We want to be moving from high to low, + * if the target is higher than current + * we must pre-seek to the higher side + */ + if (target > self->currPosition) { + self->preseek = 1; + target += self->backlash_offset; + if (target > self->fUpper) + target = self->fUpper; + } + } + else if (self->backlash_offset < 0) { + /* + * We want to be moving from to low high , + * if the target is lower than current + * we must pre-seek to the lower side + */ + if (target < self->currPosition) { + self->preseek = 1; + target += self->backlash_offset; + if (target < self->fLower) + target = self->fLower; + } + } + } + self->creep_val = 0; + absolute = motCreep(self, target); + 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; + snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + change_state(self, DMCState_Moving); + return; + } + } while (0); + break; + case eTimeoutEvent: + /* TODO handle message timeout */ + break; + } + self->errorCode = STATEERROR; +} + +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); + return; + case eMessageEvent: + do { + pCommand pCmd = event->event.msg.cmd; + if (pCmd->out_buf[0] == 'B') { /* BG */ + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_TIMER, + state_tmr_callback, self); + return; + } + else if (pCmd->out_buf[0] == 'P') { /* PA */ + snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + self->stepCount++; + return; + } + 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); + if (iRet == 0) + break; + iFlags = self->currFlags; + moving = (iFlags & STATUSMOVING); + if (moving) { + /* If Motion Control is off, report HWFault */ + if (DMC2280MotionControl != 1) { + state_cmd_execute(self, CMD_HALT); + if (DMC2280MotionControl == 0) + self->errorCode = MOTIONCONTROLOFF; + else + self->errorCode = MOTIONCONTROLUNK; + self->driver_status = HWFault; + return; + } + if (self->abs_encoder && checkMotion(self) == 0) + { + /* handle blocked */ + state_cmd_execute(self, CMD_HALT); + self->errorCode = BLOCKED; + self->driver_status = HWFault; + return; + } + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_TIMER, + state_tmr_callback, self); + return; + } + /* Motor has stopped */ + /* Handle limit switches */ + fwd_limit_active = !(iFlags & STATUSFWDLIMIT); + rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT); + errorlimit = (iFlags & STATUSERRORLIMIT); + if (fwd_limit_active && rvrs_limit_active) { + self->errorCode = IMPOSSIBLE_LIM_SW; + self->driver_status = HWFault; + } else if (fwd_limit_active) { + self->errorCode = FWDLIM; + self->driver_status = HWFault; + } else if (rvrs_limit_active) { + self->errorCode = RVRSLIM; + self->driver_status = HWFault; + } else if (errorlimit) { + self->errorCode = ERRORLIM; + self->driver_status = HWFault; + } + if (self->driver_status == HWFault) { + state_cmd_execute(self, CMD_HALT); + return; + } + /* + * If this was a pre-seek then compute the next iteration + */ + if (self->preseek) { + float target; + int absolute; + self->preseek = 0; + target = self->fTarget; + /* if we are not creeping it is backlash correction */ + /* TODO: ensure minimum progress so it doesn't stall */ + /* TODO: handle edge conditions like limits */ + if (self->creep_val == 0) { + float precision; + MotorGetPar(self->pMot, "precision", &precision); + /* TODO: take precision into account */ + if (self->backlash_offset > 0) { + if (target + self->backlash_offset > self->currPosition + precision) { + self->preseek = 1; + target += self->backlash_offset + precision; + if (target > self->fUpper) + target = self->fUpper; + } + } + else if (self->backlash_offset < 0) { + if (target + self->backlash_offset < self->currPosition - precision) { + self->preseek = 1; + target += self->backlash_offset - precision; + if (target < self->fLower) + target = self->fLower; + } + } + + /* limit the maximum number of tries */ + if (self->preseek && self->stepCount > 10) { + if (self->debug) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s preseek stopped stepcount = %d", + self->name, self->stepCount); + SICSLogWrite(line, eStatus); + } + self->preseek = 0; + } + else if (self->preseek) { + absolute = motCreep(self, target); + } + else { + /* change of direction, reset motion check */ + set_lastMotion(self, self->currSteps, self->currCounts); + absolute = motCreep(self, target); + } + } + else + absolute = motCreep(self, target); + /* + * If we are still iterating, continue + */ + if (self->preseek) { + if (absolute == self->currSteps) { + if (self->debug) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s motion stopped absolute = %d", + self->name, absolute); + SICSLogWrite(line, eStatus); + } + self->preseek = 0; + } + else { + snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute); + DMC_SendCmd(self, cmd, state_msg_callback); + return; + } + } + /* + * We are no longer iterating, so fall through + */ + } + /* handle settle time */ + if (self->settle && self->time_settle_done.tv_sec == 0) { + gettimeofday(&self->time_settle_done, NULL); + self->time_settle_done.tv_sec += self->settle; + NetWatchRegisterTimer(&self->state_timer, + self->settle, + state_tmr_callback, self); + return; + } + if (self->motOffDelay) + NetWatchRegisterTimer(&self->state_timer, + self->motOffDelay, + state_tmr_callback, self); + else + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_TIMER, + state_tmr_callback, self); + self->driver_status = HWIdle; + change_state(self, DMCState_OffTimer); + return; + } + } while (0); + break; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_HALT: + /* handle halt command, send message */ + snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + change_state(self, DMCState_MotorHalt); + return; + } + break; + case eTimeoutEvent: + self->errorCode = MOTCMDTMO; + self->driver_status = HWFault; + state_cmd_execute(self, CMD_HALT); + return; + } + self->errorCode = STATEERROR; +} + +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); + return; + case eMessageEvent: + do { + pCommand pCmd = event->event.msg.cmd; + if (pCmd->out_buf[0] == 'S') { /* ST */ + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_TIMER, + state_tmr_callback, self); + return; + } + else if (pCmd->out_buf[0] == 'M') { /* MG */ + int iRet, iFlags; + iRet = set_currMotion(self, pCmd->inp_buf); + if (iRet == 0) + break; + iFlags = self->currFlags; + if (iFlags & STATUSMOVING) { + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_TIMER, + state_tmr_callback, self); + return; + } + if (self->has_airpads) { + snprintf(cmd, CMDLEN, "FTUBE=0"); + DMC_SendCmd(self, cmd, state_msg_callback); + change_state(self, DMCState_AirOff); + return; + } + else { + snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + change_state(self, DMCState_MotorOff); + return; + } + } + } while (0); + break; + case eTimeoutEvent: + /* TODO handle message timeout */ + break; + } + self->errorCode = STATEERROR; +} + +static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { + char cmd[CMDLEN]; + switch (event->event_type) { + case eTimerEvent: + if (self->has_airpads) { + snprintf(cmd, CMDLEN, "FTUBE=0"); + DMC_SendCmd(self, cmd, state_msg_callback); + change_state(self, DMCState_AirOff); + } + else { + snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); + DMC_SendCmd(self, cmd, state_msg_callback); + change_state(self, DMCState_MotorOff); + } + return; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + /* handle run command, convert to motor on timer expired */ + NetWatchRemoveTimer(self->state_timer); + self->state_timer = 0; + self->driver_status = HWBusy; + change_state(self, DMCState_MotorOn); + event->event_type = eTimerEvent; + self->myState(self, event); + return; + case CMD_HALT: + /* handle halt command, convert to motor off timer expired */ + NetWatchRemoveTimer(self->state_timer); + self->state_timer = 0; + event->event_type = eTimerEvent; + self->myState(self, event); + return; + } + return; + } + self->errorCode = STATEERROR; +} + +static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) { + char cmd[CMDLEN]; + switch (event->event_type) { + case eTimerEvent: + DMC_SendCmd(self, "MG APDONE", state_msg_callback); + return; + case eMessageEvent: + do { + pCommand pCmd = event->event.msg.cmd; + if (pCmd->out_buf[0] == 'F') { /* FTUBE */ + } + else if (pCmd->out_buf[0] == 'M') { /* MG APDONE */ + float fReply; + fReply = (float) atof(pCmd->inp_buf); + if (fReply == 0) { + NetWatchRegisterTimer(&self->state_timer, + ON_SETTLE_TIMER, + state_tmr_callback, self); + change_state(self, DMCState_MotorOff); + return; + } + } + NetWatchRegisterTimer(&self->state_timer, + AIR_POLL_TIMER, + state_tmr_callback, self); + } while (0); + return; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + /* TODO: handle run command */ + return; + case CMD_HALT: + /* TODO: handle halt command */ + return; + } + break; + case eTimeoutEvent: + /* TODO handle message timeout */ + break; + } + self->errorCode = STATEERROR; +} + +static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event) { + switch (event->event_type) { + case eTimerEvent: + case eMessageEvent: + /* progress to IDLE */ + change_state(self, DMCState_Idle); + if (self->driver_status == HWBusy) + self->driver_status = HWIdle; + return; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + /* TODO: handle run command */ + return; + case CMD_HALT: + /* TODO: handle halt command */ + return; + } + break; + case eTimeoutEvent: + /* TODO handle message timeout */ + break; + } + self->errorCode = STATEERROR; +} + /** \brief Reads motor position, implements the GetPosition * method in the MotorDriver interface. * @@ -1025,6 +1968,19 @@ static int DMC2280GetPos(void *pData, float *fPos){ reply[0]='\0'; self = (pDMC2280Driv)pData; assert(self != NULL); + +#if 0 + 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; + return OKOK; + } +#endif + if (1 == self->abs_encoder) { if (readAbsEnc(self, &absEncPos) == FAILURE) return HWFault; @@ -1082,6 +2038,15 @@ static int DMC2280Run(void *pData,float fValue){ self->fTarget = fValue; + if (self->has_fsm) { + while (self->myState == DMCState_AirOff + || self->myState == DMCState_MotorOff) { + SicsWait(1); + } + state_cmd_execute(self, CMD_RUN); + return 1; + } + /* * Note: this passes control to a timer routine */ @@ -1119,6 +2084,9 @@ static int DMC2280Status(void *pData){ self = (pDMC2280Driv)pData; assert(self != NULL); + if (self->has_fsm) { + return self->driver_status; + } /* * If we are waiting for the motor or airpads then we * are busy @@ -1353,6 +2321,12 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ case MOTIONCONTROLUNK: strncpy(error, "MOTION CONTROL SEEMS TO BE UNKNOWN", (size_t)errLen); break; + case STATEERROR: + strncpy(error, "ERROR IN DMC2280 FINITE STATE MACHINE", (size_t)errLen); + break; + case THREADZERO: + strncpy(error, "THREAD ZERO NOT RUNNING ON CONTROLLER", (size_t)errLen); + break; default: /* FIXME What's the default */ snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode); @@ -1386,6 +2360,7 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ case MOTIONCONTROLOFF: case MOTIONCONTROLUNK: case MOTCMDTMO: + case THREADZERO: return MOTFAIL; case POSFAULT: #if HAS_RS232 @@ -1398,6 +2373,19 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ #endif return MOTREDO; break; + case STATEERROR: + /* TODO: recover state error */ + if (self->has_fsm) { + if (self->state_timer) + NetWatchRemoveTimer(self->state_timer); + self->state_timer = 0; + change_state(self, DMCState_Unknown); + /* Schedule a timer event as soon as possible */ + NetWatchRegisterTimer(&self->state_timer, + 0, + state_tmr_callback, self); + } + return MOTFAIL; default: return MOTFAIL; break; @@ -1419,6 +2407,11 @@ static int DMC2280Halt(void *pData){ self = (pDMC2280Driv)pData; assert(self != NULL); + + if (self->has_fsm) { + state_cmd_execute(self, CMD_HALT); + return 1; + } /* Stop motor */ snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); if (FAILURE == DMC2280Send(self, cmd)) @@ -1541,6 +2534,14 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->absEncHome; return 1; } + if(strcasecmp(name,"creep_offset") == 0) { + *fValue = self->creep_offset; + return 1; + } + if(strcasecmp(name,"creep_precision") == 0) { + *fValue = self->creep_precision; + return 1; + } } else { if (strcasecmp(name,"homerun") == 0) { @@ -1716,6 +2717,28 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, } } + /* Set creep offset, + * managers only */ + if(self->abs_encoder && strcasecmp(name,"creep_offset") == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->creep_offset = fabs(newValue); + return 1; + } + } + + /* Set creep_precision, + * managers only */ + if(self->abs_encoder && strcasecmp(name,"creep_precision") == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->creep_precision = fabs(newValue); + return 1; + } + } + /* Invoke Home Run routine in controller, * managers only */ if(self->abs_encoder == 0 && strcasecmp(name,"homerun") == 0) { @@ -1856,6 +2879,10 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); 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); + SCWrite(pCon, buffer, eStatus); } snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX); SCWrite(pCon, buffer, eStatus); @@ -1913,7 +2940,6 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { char *pPtr = NULL; char buffer[132]; char pError[ERRLEN]; - char cmd[CMDLEN]; Tcl_Interp *interp; buffer[0]='\0'; @@ -1997,6 +3023,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { pNew->blockage_ratio = 2.0; pNew->blockage_fail = 0; pNew->backlash_offset = 0.0; + pNew->myState = DMCState_Unknown; /* PARAMETERS: Fetch parameter values */ @@ -2007,6 +3034,13 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { sscanf(pPtr,"%d",&(pNew->debug)); } + /* FSM: this driver uses the finite state machine model */ + if ((pPtr=getParam(pCon, interp, params,"fsm",_OPTIONAL)) == NULL) + pNew->has_fsm=0; + else { + sscanf(pPtr,"%d",&(pNew->has_fsm)); + } + if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) { strncpy(pNew->long_name, pPtr, sizeof(pNew->long_name)); pNew->long_name[sizeof(pNew->long_name) - 1] = '\0'; @@ -2101,21 +3135,51 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { pNew->cntsPerX = 1.0; else sscanf(pPtr,"%f",&(pNew->cntsPerX)); + /* CREEP_OFFSET: this controls unidirectional driving */ + if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL) + pNew->creep_offset = 0.0; + else { + sscanf(pPtr, "%f", &(pNew->creep_offset)); + if (pNew->creep_offset < 0) + pNew->creep_offset = -pNew->creep_offset; + } + + /* CREEP_PRECISION: this controls unidirectional driving */ + if ((pPtr=getParam(pCon, interp, params,"creep_precision",_OPTIONAL)) == NULL) + pNew->creep_precision = 0.0; + else { + sscanf(pPtr, "%f", &(pNew->creep_precision)); + if (pNew->creep_precision < 0) + pNew->creep_precision = -pNew->creep_precision; + } + + } + + if (pNew->has_fsm) { + /* Schedule a timer event as soon as possible */ + NetWatchRegisterTimer(&pNew->state_timer, + 0, + state_tmr_callback, pNew); + } + else { + char cmd[CMDLEN]; + /* Set speed */ + snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed)); + if (FAILURE == DMC2280Send(pNew, cmd)) + return NULL; + /* Set acceleration */ + snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel)); + if (FAILURE == DMC2280Send(pNew, cmd)) + return NULL; + /* Set deceleration */ + snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel)); + if (FAILURE == DMC2280Send(pNew, cmd)) + return NULL; + /* TODO Initialise current position and target + * to get a sensible initial list output + */ } - /* Set speed */ - snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed)); - if (FAILURE == DMC2280Send(pNew, cmd)) - return NULL; - /* Set acceleration */ - snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel)); - if (FAILURE == DMC2280Send(pNew, cmd)) - return NULL; - /* Set deceleration */ - snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel)); - if (FAILURE == DMC2280Send(pNew, cmd)) - return NULL; - /* TODO Initialise current position and target to get a sensible initial list output */ return (MotorDriver *)pNew; } @@ -2126,6 +3190,24 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, pDMC2280Driv self = (pDMC2280Driv) pm->pDriver; if (argc > 1) { + if (strcasecmp("send", argv[1]) == 0) { + char cmd[CMDLEN]; + char rsp[CMDLEN]; + int idx = 0; + int i, j; + cmd[0] = '\0'; + for (i = 2; i < argc; ++i) { + j = snprintf(&cmd[idx], CMDLEN - j, "%s%s", + (i > 2) ? " " : "", + argv[i]); + if (j < 0) + break; + idx += j; + } + DMC2280SendReceive(self, cmd, rsp); + SCWrite(pCon, rsp, eValue); + return 1; + } if (strcasecmp("units", argv[1]) == 0) { if (argc > 2) { strncpy(self->units, argv[2], sizeof(self->units)); @@ -2192,6 +3274,39 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, newZero = (currPos - newValue); return MotorSetPar(self->pMot, pCon, "softzero", newZero); } + else if(strcasecmp("reset", argv[1]) == 0) { + if (self->has_fsm) { + if (self->state_timer) + NetWatchRemoveTimer(self->state_timer); + self->state_timer = 0; + change_state(self, DMCState_Unknown); + /* Schedule a timer event as soon as possible */ + NetWatchRegisterTimer(&self->state_timer, + 0, + state_tmr_callback, self); + } + /* Handle further in generic motor driver */ + return MotorAction(pCon, pSics, pData, argc, argv); + } + else if(strcasecmp("state", argv[1]) == 0) { + char line[132]; + snprintf(line, 132, "%s.state = %s (timer=%s)", + self->name, state_name(self->myState), + self->state_timer ? "active" : "inactive"); + SCWrite(pCon, line, eValue); + return 1; + } + else if(strcasecmp("trace", argv[1]) == 0) { + if (strcasecmp("on", argv[2]) == 0) { + self->trace = pCon; + SCWrite(pCon, "TRACE ON", eValue); + } + else { + self->trace = 0; + SCWrite(pCon, "TRACE OFF", eValue); + } + return 1; + } } return MotorAction(pCon, pSics, pData, argc, argv); }