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

Fixing blocked motor detection

Browse Source 
r1710 | dcl | 2007-03-23 18:43:43 +1100 (Fri, 23 Mar 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-03-23 18:43:43 +11:00
parent 7d36b64f68
commit 96b8a1aee6
1 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
site_ansto/motor_dmc2280.c
View File

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