diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 26a5eec4..41e8d88e 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -207,6 +207,7 @@ struct __MoDriv { double fPreseek; /**< preseek target when preseek is active */ int settle; /**< motor settling time in milliseconds */ struct timeval time_settle_done; /**< time when settling will be over */ + bool doSettle; /**< flag to request settle after move, autoreset */ int debug; int stepCount; /**< number of step operations for this move cycle */ float creep_offset; /**< last little bit to creep in units */ @@ -220,6 +221,7 @@ struct __MoDriv { int timerValue; /**< save for debug printing */ StateFunc myState; /**< pointer to state action method */ StateFunc myPrevState; /**< save for debug printing */ + StateFunc myMoveCallerReturn; /**< state to return to */ int subState; /**< tracks substate within state method */ bool waitResponse; /**< true is a message sent and we wait for response */ pNWTimer state_timer; /**< motor state timer */ @@ -237,6 +239,7 @@ struct __MoDriv { int bias_bits; /**< number of bits to mask */ int bias_bias; /**< bias to add to encoder value */ char ao_id[256]; + bool testing; /**< flag for testing new code */ }; int DMC2280MotionControl = 1; /* defaults to enabled */ @@ -1632,6 +1635,10 @@ 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_StepMove(pDMC2280Driv self, pEvtEvent event); +static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event); +static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event); +static void DMCState_Creeping(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); @@ -1644,6 +1651,10 @@ static char* state_name(StateFunc func) { if (func == DMCState_MotorStart) return "DMCState_MotorStart"; if (func == DMCState_MotorOn) return "DMCState_MotorOn"; if (func == DMCState_Moving) return "DMCState_Moving"; + if (func == DMCState_StepMove) return "DMCState_StepMove"; + if (func == DMCState_SimpleMove) return "DMCState_SimpleMove"; + if (func == DMCState_Backlash) return "DMCState_Backlash"; + if (func == DMCState_Creeping) return "DMCState_Creeping"; if (func == DMCState_MotorHalt) return "DMCState_MotorHalt"; if (func == DMCState_OffTimer) return "DMCState_OffTimer"; if (func == DMCState_MotorStop) return "DMCState_MotorStop"; @@ -2412,6 +2423,18 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { cmdAccel(self); /* No Response */ cmdDecel(self); /* No Response */ #endif + if (self->testing) { + self->myMoveCallerReturn = NULL; + if (self->backlash_offset != 0) { + change_state(self, DMCState_Backlash); + return; + } else if (self->creep_offset != 0) { + change_state(self, DMCState_Creeping); + return; + } + change_state(self, DMCState_SimpleMove); + return; + } set_lastMotion(self, self->currSteps, self->currCounts); /* compute position for PA command */ target = self->fTarget; @@ -2512,6 +2535,433 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { return; } +/* + * This state moves the motor from its current position to fTarget. + * There is no backlash and no creep. + * If there is a fault it transfers out + */ +static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) { + double target; + int absolute; + + switch (event->event_type) { + case eStateEvent: + /* + * If we are the myMoveCallerReturn then continue + */ + if (DMCState_SimpleMove == self->myMoveCallerReturn) { + /* TODO: precision check and retry */ + self->myMoveCallerReturn = NULL; + change_state(self, DMCState_OffTimer); + return; + } + target = self->fTarget; + absolute = motAbsol(self, target); + self->doSettle = self->settle > 0; + /* decide if we should be letting the motor settle */ + cmdPosition(self, absolute); + return; + case eTimerEvent: + cmdStatus(self); + return; + case eMessageEvent: + if (self->run_flag != 0) { + change_state(self, DMCState_MotorHalt); + return; + } + self->myMoveCallerReturn = DMCState_SimpleMove; + change_state(self, DMCState_StepMove); + return; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + /* TODO: FIXME RUN command while running */ + if (self->driver_status == HWIdle) + self->driver_status = HWBusy; + self->run_flag = 1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + case CMD_HALT: + /* handle halt command, send message */ + self->run_flag = -1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + } + break; + case eTimeoutEvent: + strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); + self->errorCode = MOTCMDTMO; + self->driver_status = HWFault; + change_state(self, DMCState_MotorHalt); + return; + } + unhandled_event(self, event); + self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; +} + +/* + * This state moves the motor from its current position to fTarget + backlash_offset. + * There is no creep. + * If there is a fault it transfers out + */ +static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event) { + double target; + int absolute; + + switch (event->event_type) { + case eStateEvent: + /* + * If we are the myMoveCallerReturn then continue + */ + if (DMCState_Backlash == self->myMoveCallerReturn) { + float precision; + /* TODO: precision check and retry */ + self->myMoveCallerReturn = NULL; + target = self->fTarget; + self->preseek = 0; + /* take precision into account */ + MotorGetPar(self->pMot, "precision", &precision); + 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; + self->fPreseek = target; + } + } + 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; + self->fPreseek = target; + } + } + + if (self->preseek && self->stepCount > 10) { + /* limit the maximum number of tries */ + 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 { + target = self->fTarget; + self->preseek = 0; + if (self->backlash_offset != 0) { + 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 low to 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; + } + } + } + } + if (self->preseek == 0) { + /* preseek is not required, handle as a simple move */ + if (self->creep_offset != 0) { + change_state(self, DMCState_Creeping); + } else { + change_state(self, DMCState_SimpleMove); + } + return; + } + self->fPreseek = target; + absolute = motAbsol(self, target); + /* decide if we should be letting the motor settle */ + self->doSettle = self->settle > 0; + cmdPosition(self, absolute); + return; + case eTimerEvent: + cmdStatus(self); + return; + case eMessageEvent: + if (self->run_flag != 0) { + change_state(self, DMCState_MotorHalt); + return; + } + self->myMoveCallerReturn = DMCState_Backlash; + change_state(self, DMCState_StepMove); + return; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + /* TODO: FIXME RUN command while running */ + if (self->driver_status == HWIdle) + self->driver_status = HWBusy; + self->run_flag = 1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + case CMD_HALT: + /* handle halt command, send message */ + self->run_flag = -1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + } + break; + case eTimeoutEvent: + strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); + self->errorCode = MOTCMDTMO; + self->driver_status = HWFault; + change_state(self, DMCState_MotorHalt); + return; + } + unhandled_event(self, event); + self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; +} + +/* + * This state moves the motor from its current position to fTarget by creeping. + * There is no backlash. + * If there is a fault it transfers out + */ +static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event) { + double target; + int absolute; + + switch (event->event_type) { + case eStateEvent: + /* + * If we are the myMoveCallerReturn then continue + */ + if (DMCState_Creeping == self->myMoveCallerReturn) { + self->myMoveCallerReturn = NULL; + if (self->preseek == 0) { + /* it is finished */ + change_state(self, DMCState_OffTimer); + return; + } + } else { + /* first time, initialize */ + self->creep_val = 0; + } + target = self->fTarget; + self->preseek = 0; + absolute = motCreep(self, target); + if (self->preseek == 0) { + /* it is finished */ + change_state(self, DMCState_OffTimer); + return; + } + /* decide if we should be letting the motor settle */ + self->doSettle = self->settle > 0; + if (self->doSettle) { + if (abs(absolute - self->currSteps) > fabs(self->creep_offset * self->stepsPerX)) + self->doSettle = false; + if (self->creep_precision > 0.0) + if (abs(absolute - self->currSteps) > 10.0 * fabs(self->creep_precision * self->stepsPerX)) + self->doSettle = false; + } + self->fPreseek = target; + cmdPosition(self, absolute); + return; + case eTimerEvent: + cmdStatus(self); + return; + case eMessageEvent: + if (self->run_flag != 0) { + change_state(self, DMCState_MotorHalt); + return; + } + self->myMoveCallerReturn = DMCState_Creeping; + change_state(self, DMCState_StepMove); + return; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + /* TODO: FIXME RUN command while running */ + if (self->driver_status == HWIdle) + self->driver_status = HWBusy; + self->run_flag = 1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + case CMD_HALT: + /* handle halt command, send message */ + self->run_flag = -1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + } + break; + case eTimeoutEvent: + strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); + self->errorCode = MOTCMDTMO; + self->driver_status = HWFault; + change_state(self, DMCState_MotorHalt); + return; + } + unhandled_event(self, event); + self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; +} + +/* + * This state moves the motor one step from its current position to the current destination + * set by PA (or PR) by the caller in myMoveCallerReturn. + * If there is a fault it transfers out, otherwise it returns to the previous + * state when the move is successfully completed. + */ +static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) { + switch (event->event_type) { + /* + * The PA has already been confirmed by the caller, now do the move + */ + case eStateEvent: + /* save pre-motion values for logging */ + set_lastMotion(self, self->currSteps, self->currCounts); + self->origTime = DoubleTime(); + self->origPosition = self->currPosition; + self->origCounts = self->currCounts; + self->origSteps = self->currSteps; + self->minRatio = 0.0; + self->maxRatio = 0.0; + /* begin moving */ + cmdBegin(self); + self->stepCount = 1; + self->subState = 1; + return; + case eTimerEvent: + /* + * Find out how the motor is travelling, + * Note that the substate has already been set + */ + cmdStatus(self); + return; + case eMessageEvent: + if (self->run_flag != 0) { + change_state(self, DMCState_MotorHalt); + return; + } + do { + pAsyncTxn pCmd = event->event.msg.cmd; + if (self->subState == 1) { /* BG */ + /* Check if BG worked (reply != '?') */ + if (pCmd->inp_buf[0] == '?') { + /* TODO: what happens when BGx fails? */ + self->errorCode = BADCMD; + self->faultPending = true; /* defer fault */ + change_state(self, DMCState_MotorHalt); + return; + } + self->subState = 2; + DMC_SetTimer(self, self->motorPollFast); + return; + } else if (self->subState == 2) { /* Status */ + int iRet; + /* parse the status response and set error codes */ + iRet = rspStatus(self, pCmd->inp_buf); + /* if the parse failed break out of here */ + if (iRet == 0) + break; + /* if the status response can be handled here, return */ + if (motorHandleStatus(self)) + return; + /* + * We get here when the motor stops normally + */ + if (self->doSettle) { + self->doSettle = false; + DMC_SetTimer(self, self->settle); + return; + } + if (true /*self->debug*/) { + double units = self->currPosition - self->origPosition; + long int steps = self->currSteps - self->origSteps; + long int counts = self->currCounts - self->origCounts; + double time = DoubleTime() - self->origTime; + char line[CMDLEN]; + snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f," + " steps=%ld, counts=%ld, stepsPerX=%.6f," + " ratio=(%.6f-%.6f), time=%.3f", + self->name, + units, + steps, + counts, + (double) steps / units, + self->minRatio, + self->maxRatio, + time); + SICSLogWrite(line, eStatus); + } + change_state(self, self->myMoveCallerReturn); + return; + } + } while (0); + break; + case eCommandEvent: + switch (event->event.cmd.cmd_type) { + case CMD_RUN: + /* TODO: FIXME RUN command while running */ + if (self->driver_status == HWIdle) + self->driver_status = HWBusy; + self->run_flag = 1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + case CMD_HALT: + /* handle halt command, send message */ + self->run_flag = -1; + if (self->waitResponse == false) { + change_state(self, DMCState_MotorHalt); + } + return; + } + break; + case eTimeoutEvent: + strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); + self->errorCode = MOTCMDTMO; + self->driver_status = HWFault; + change_state(self, DMCState_MotorHalt); + return; + } + unhandled_event(self, event); + self->errorCode = STATEERROR; + change_state(self, DMCState_Error); + return; +} + static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { /* * Substates: @@ -4469,6 +4919,17 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, SCWrite(pCon, line, eValue); return 1; } + else if(strcasecmp("testing", argv[1]) == 0) { + if (argc > 2 && strcasecmp("on", argv[2]) == 0) { + self->testing = true; + SCWrite(pCon, "TESTING ON", eValue); + } + else { + self->testing = false; + SCWrite(pCon, "TESTING OFF", eValue); + } + return 1; + } else if(strcasecmp("trace", argv[1]) == 0) { if (argc > 2 && strcasecmp("on", argv[2]) == 0) { self->trace = SCCopyConnection(pCon);