From eeb74517a8c7dab94d1246ba9e5b96713f04080c Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Thu, 11 Jul 2013 16:55:48 +1000 Subject: [PATCH] Add some braces, add and improve comments to make code clearer, no functional changes --- site_ansto/motor_dmc2280.c | 111 +++++++++++++++++++++++++++++-------- 1 file changed, 88 insertions(+), 23 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index cc67b4a9..c9379044 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -205,7 +205,7 @@ struct __MoDriv { float backlash_offset; /**< signed offset to drive from */ double fTarget; /**< target passed from SICS to timer callback */ double fPreseek; /**< preseek target when preseek is active */ - int settle; /**< motor settling time in seconds */ + int settle; /**< motor settling time in milliseconds */ struct timeval time_settle_done; /**< time when settling will be over */ int debug; int stepCount; /**< number of step operations for this move cycle */ @@ -534,7 +534,7 @@ static double motPosit(pDMC2280Driv self) { /** \brief Convert motor target in physical units to - * motor absolute position in steps + * motor target in absolute steps * * \param self (r) provides access to the motor's data structure * \param target in physical units, eg mm, degrees @@ -571,7 +571,7 @@ static int motAbsol(pDMC2280Driv self, double target) { } /** \brief Convert motor target in physical units to - * motor absolute position in steps + * motor target in absolute steps * * \param self provides access to the motor's data structure * \param target in physical units, eg mm, degrees @@ -579,23 +579,41 @@ static int motAbsol(pDMC2280Driv self, double target) { */ static int motCreep(pDMC2280Driv self, double target) { int target_steps = motAbsol(self, target); - if (!self->abs_encoder || /* no absolute encoder */ - self->preseek || /* backlash preseek active */ - self->creep_offset == 0) /* not creeping anyway */ + if (!self->abs_encoder || /* no absolute encoder */ + self->preseek || /* backlash preseek active */ + self->creep_offset == 0) { /* not creeping anyway */ + /* + * Legacy case for non-creep scenarios + */ return target_steps; + } else { + /* + * This is the creep scenario - and self->preseek is zero + * + * calculate the offset of the target from the current position in motor steps + */ int offset = target_steps - self->currSteps; - if (self->creep_val == 0) { /* initial creep step */ + /* + * If this is our first time, initialize creep_val depending on direction + */ + if (self->creep_val == 0) { 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 */ + } else { + /* + * not first, bump the direction sensitive counters + */ + if (self->creep_val > 0) { /* moving down */ ++self->creep_val; - else + } else { --self->creep_val; + } + /* + * In all repetitive things, there just has to be a limit + */ if (abs(self->creep_val) > MAX_CREEP_STEPS && abs(self->creep_val) > 2.0 * fabs(self->stepsPerX / self->cntsPerX)) { if (self->debug) { @@ -604,32 +622,50 @@ static int motCreep(pDMC2280Driv self, double target) { self->name, self->stepCount); SICSLogWrite(line, eStatus); } + /* + * self->preseek remains zero to say we are finished + */ return target_steps; } } + /* + * We may still move, calculate if we will and how far + */ 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); } - if (self->creep_val > 0) /* moving down handle as positive */ + /* + * We don't want to handle each case separately, so fold negative into positive case + */ + if (self->creep_val > 0) /* moving down, handle as positive */ offset = -offset; + /* + * Only move if offset is outside of the creep_precision in steps + */ if (offset > fabs(self->stepsPerX * self->creep_precision)) { #ifdef DECADIC_CREEP - /* if half offset is more than creep_offset warp to creep_offset */ - if (offset > (int) (2.0 * fabs(self->stepsPerX * self->creep_offset))) + /* + * if the offset is more than double the creep_offset the we just + * warp to the target minus the creep_offset + */ + if (offset > (int) (2.0 * fabs(self->stepsPerX * self->creep_offset))) { offset = offset - fabs(self->stepsPerX * self->creep_offset); - else { - /* if closer than one count, "single step" else go half way */ + } else { + /* + * if the offset is less than or equal to the steps for one encoder count + * we have to "single step" otherwise we want to go half way + */ if (offset <= fabs(self->stepsPerX / self->cntsPerX)) { offset = offset / DECADIC_CREEP; if (offset < 1) offset = 1; - } - else + } else { offset = offset / 2; + } } #else if (offset - (int) fabs(self->stepsPerX * self->creep_offset) > (int) fabs(self->stepsPerX / self->cntsPerX)) @@ -641,9 +677,15 @@ static int motCreep(pDMC2280Driv self, double target) { offset = offset / 2; } #endif + /* + * Since we want to move, set preseek to flag it to the caller + */ self->preseek = 1; } - if (self->creep_val > 0) /* moving down restore to negative */ + /* + * If we folded the negative case, then unfold it + */ + if (self->creep_val > 0) /* moving down, restore to negative */ offset = - offset; if (self->debug) { @@ -652,6 +694,10 @@ static int motCreep(pDMC2280Driv self, double target) { self->currSteps, target_steps, offset, self->currSteps + offset); SICSLogWrite(text, eStatus); } + /* + * The new absolute step target is the current step position plus + * the possibly modified offset + */ target_steps = self->currSteps + offset; } return target_steps; @@ -2342,14 +2388,28 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { } static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { + /* + * Substates: + * 0 On entry , immediately changed to 1 + * 1 BG sent, waiting for reply + * 2 poll_timer/status poll sent + * 3 PA sent waiting for reply + * 4 settle_timer/status poll sent + */ switch (event->event_type) { case eStateEvent: + /* + * The PA has already been confirmed by the caller, now do the move + */ cmdBegin(self); self->stepCount = 1; self->subState = 1; return; case eTimerEvent: - /* Note that the substate has already been set */ + /* + * Find out how the motor is travelling, + * Note that the substate has already been set + */ cmdStatus(self); return; case eMessageEvent: @@ -2381,7 +2441,9 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { else if (self->subState == 2 || self->subState == 4) { /* Status */ int iRet, iFlags; bool fwd_limit_active, rvrs_limit_active, errorlimit; + /* 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; iFlags = self->currFlags; @@ -2493,9 +2555,13 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { return; } /* - * If this was a pre-seek then compute the next iteration + * If we get here, the motor has stopped and everything seems OK. + * We must decide on the next course of action. */ if (self->preseek) { + /* + * this was a pre-seek so compute the next iteration + */ double target; int absolute; self->preseek = 0; @@ -2579,8 +2645,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { else { if (self->settle && self->subState == 2 - && abs(absolute - self->currSteps) < 100) - { + && abs(absolute - self->currSteps) < 100) { self->subState = 4; DMC_SetTimer(self, self->settle); return;