Fixing blocked motor detection
r1710 | dcl | 2007-03-23 18:43:43 +1100 (Fri, 23 Mar 2007) | 2 lines
This commit is contained in:
@ -774,11 +774,16 @@ static int checkMotion(void *pData) {
|
||||
return 1;
|
||||
}
|
||||
/* calculate observed and expected steps per count ratios */
|
||||
ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts);
|
||||
if (counts == self->lastCounts) /* prevent divide by zero */
|
||||
ratio_obs = (steps - self->lastSteps);
|
||||
else
|
||||
ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts);
|
||||
ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX;
|
||||
ratio_cmp = ratio_obs / ratio_exp;
|
||||
/* less than half or more than double is trouble */
|
||||
if (ratio_cmp > self->blockage_ratio || (1.0 / ratio_cmp) > self->blockage_ratio) {
|
||||
/* wrong signs, less than half, or more than double is trouble */
|
||||
if (ratio_cmp < 0.0 ||
|
||||
ratio_cmp > self->blockage_ratio ||
|
||||
(1.0 / ratio_cmp) > self->blockage_ratio) {
|
||||
char msg[132];
|
||||
snprintf(msg, sizeof(msg), "Motion check fail: obs=%f, exp=%f",
|
||||
ratio_obs, ratio_exp);
|
||||
|
Reference in New Issue
Block a user