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

Break down the motor states further to make them even simpler

Browse Source
This commit is contained in:
Douglas Clowes
2013-07-15 13:36:20 +10:00
parent 4d933a6399
commit 23f53b8a4c
1 changed files with 262 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

376
site_ansto/motor_dmc2280.c
View File

@@ -221,7 +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 */
StateFunc myNextState; /**< 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 */
@@ -1656,8 +1656,10 @@ 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_BacklashStart(pDMC2280Driv self, pEvtEvent event);
static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event);
static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event);
static void DMCState_CreepCont(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);
@@ -1672,8 +1674,10 @@ static char* state_name(StateFunc func) {
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_BacklashStart) return "DMCState_BacklashStart";
if (func == DMCState_BacklashCont) return "DMCState_BacklashCont";
if (func == DMCState_CreepStart) return "DMCState_CreepStart";
if (func == DMCState_CreepCont) return "DMCState_CreepCont";
if (func == DMCState_MotorHalt) return "DMCState_MotorHalt";
if (func == DMCState_OffTimer) return "DMCState_OffTimer";
if (func == DMCState_MotorStop) return "DMCState_MotorStop";
@@ -2443,12 +2447,12 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
cmdDecel(self); /* No Response */
#endif
if (self->testing) {
self->myMoveCallerReturn = NULL;
self->myNextState = NULL;
if (self->backlash_offset != 0) {
change_state(self, DMCState_Backlash);
change_state(self, DMCState_BacklashStart);
return;
} else if (self->creep_offset != 0) {
change_state(self, DMCState_Creeping);
change_state(self, DMCState_CreepStart);
return;
}
change_state(self, DMCState_SimpleMove);
@@ -2565,15 +2569,7 @@ static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) {
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;
}
self->myNextState = NULL;
target = self->fTarget;
absolute = motAbsol(self, target);
self->doSettle = self->settle > 0;
@@ -2588,7 +2584,110 @@ static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) {
change_state(self, DMCState_MotorHalt);
return;
}
self->myMoveCallerReturn = DMCState_SimpleMove;
/* set the next state after this move */
self->myNextState = DMCState_OffTimer;
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 sets up to moves the motor from its current position to fTarget + backlash_offset.
* If there is creep, it happens later.
* If there is a fault it transfers out
*/
static void DMCState_BacklashStart(pDMC2280Driv self, pEvtEvent event) {
double target;
int absolute;
bool doPreseek;
switch (event->event_type) {
case eStateEvent:
self->myNextState = NULL;
target = self->fTarget;
doPreseek = false;
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 the current position we must pre-seek to the higher side
*/
if (target > self->currPosition) {
doPreseek = true;
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
* the current position we must pre-seek to the lower side
*/
if (target < self->currPosition) {
doPreseek = true;
target += self->backlash_offset;
if (target < self->fLower)
target = self->fLower;
}
}
}
if (doPreseek == false) {
/* preseek is not required, handle as a simple move */
if (self->creep_offset != 0) {
change_state(self, DMCState_CreepStart);
} else {
change_state(self, DMCState_SimpleMove);
}
return;
}
self->preseek = 1;
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;
}
/* set the next state after this move */
self->myNextState = DMCState_BacklashCont;
change_state(self, DMCState_StepMove);
return;
case eCommandEvent:
@@ -2626,96 +2725,60 @@ static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) {
/*
* This state moves the motor from its current position to fTarget + backlash_offset.
* There is no creep.
* If there is creep, it happens after this.
* If there is a fault it transfers out
*/
static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event) {
static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event) {
double target;
float precision;
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;
}
self->myNextState = 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;
}
}
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;
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);
}
} 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);
change_state(self, DMCState_CreepStart);
} else {
change_state(self, DMCState_SimpleMove);
}
return;
}
self->preseek = 1;
self->fPreseek = target;
absolute = motAbsol(self, target);
/* decide if we should be letting the motor settle */
@@ -2730,7 +2793,94 @@ static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event) {
change_state(self, DMCState_MotorHalt);
return;
}
self->myMoveCallerReturn = DMCState_Backlash;
/* set the next state after this move */
self->myNextState = DMCState_BacklashCont;
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 sets up to move the motor from its current position to fTarget by creeping.
* If there was a backlash correction, it has already happened.
* If there is a fault it transfers out
*/
static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event) {
double target;
int absolute;
switch (event->event_type) {
case eStateEvent:
self->myNextState = NULL;
/* initialize the creep control variable */
self->creep_val = 0;
/*
* calculate the absolute step position of the adjusted target
*/
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;
}
/* set the next state after this move */
self->myNextState = DMCState_CreepCont;
change_state(self, DMCState_StepMove);
return;
case eCommandEvent:
@@ -2771,25 +2921,17 @@ static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event) {
* There is no backlash.
* If there is a fault it transfers out
*/
static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event) {
static void DMCState_CreepCont(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;
self->myNextState = NULL;
if (self->preseek == 0) {
/* it is finished */
change_state(self, DMCState_OffTimer);
return;
}
target = self->fTarget;
self->preseek = 0;
@@ -2819,7 +2961,7 @@ static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event) {
change_state(self, DMCState_MotorHalt);
return;
}
self->myMoveCallerReturn = DMCState_Creeping;
self->myNextState = DMCState_CreepCont;
change_state(self, DMCState_StepMove);
return;
case eCommandEvent:
@@ -2856,9 +2998,9 @@ static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event) {
}
/*
* 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
* This state moves the motor from its current position to the current destination
* set by PA (or PR) by the caller in myNextState.
* If there is a fault it transfers out, otherwise it moves to the myNextState
* state when the move is successfully completed.
*/
static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
@@ -2980,7 +3122,7 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
}
}
}
change_state(self, self->myMoveCallerReturn);
change_state(self, self->myNextState);
return;
}
} while (0);
@@ -5007,13 +5149,19 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
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);
if (argc > 2) {
if (strcasecmp("on", argv[2]) == 0) {
self->testing = true;
SCWrite(pCon, "TESTING ON", eValue);
}
else {
self->testing = false;
SCWrite(pCon, "TESTING OFF", eValue);
}
} else {
SCPrintf(pCon, eValue, "%s.testing = %s",
self->name,
self->testing ? "ON" : "OFF");
}
return 1;
}