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

Add some braces, add and improve comments to make code clearer, no functional changes

Browse Source
This commit is contained in:
Douglas Clowes
2013-07-11 16:55:48 +10:00
parent ca779ddc36
commit eeb74517a8
1 changed files with 88 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
site_ansto/motor_dmc2280.c
View File

@@ -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;