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

Add some motor statistics and regression to calculate steps per unit

Browse Source
This commit is contained in:
Douglas Clowes
2013-07-12 11:52:30 +10:00
parent 1d18fbf646
commit e9a884fa30
1 changed files with 91 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
site_ansto/motor_dmc2280.c
View File

@@ -240,6 +240,16 @@ struct __MoDriv {
int bias_bias; /**< bias to add to encoder value */
char ao_id[256];
bool testing; /**< flag for testing new code */
bool doStats; /**< flag to request stats collection */
double S_x;
double S_y;
double S_xx;
double S_yy;
double S_xy;
double S_n;
double S_m;
double S_b;
double S_r;
};
int DMC2280MotionControl = 1; /* defaults to enabled */
@@ -2914,24 +2924,61 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
DMC_SetTimer(self, self->settle);
return;
}
if (true /*self->debug*/) {
if (true /*self->debug*/ || true /*self->doStats*/) {
double units = self->currPosition - self->origPosition;
long int steps = self->currSteps - self->origSteps;
long int counts = self->currCounts - self->origCounts;
double time = DoubleTime() - self->origTime;
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f,"
" steps=%ld, counts=%ld, stepsPerX=%.6f,"
" ratio=(%.6f-%.6f), time=%.3f",
self->name,
units,
steps,
counts,
(double) steps / units,
self->minRatio,
self->maxRatio,
time);
SICSLogWrite(line, eStatus);
if (true /*self->debug*/) {
snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f,"
" steps=%ld, counts=%ld, stepsPerX=%.6f,"
" ratio=(%.6f-%.6f), time=%.3f",
self->name,
units,
steps,
counts,
(double) steps / units,
self->minRatio,
self->maxRatio,
time);
SICSLogWrite(line, eStatus);
}
if (self->abs_encoder && true /*self->doStats*/) {
if (labs(steps) > fabs(self->stepsPerX) && labs(counts) > fabs(self->cntsPerX)) {
self->S_x += (double) counts;
self->S_y += (double) steps;
self->S_xy += (double) counts * (double) steps;
self->S_xx += (double) counts * (double) counts;
self->S_yy += (double) steps * (double) steps;
self->S_n += (double) 1;
self->S_m = self->S_n * self->S_xy - self->S_x * self->S_y;
self->S_m /= self->S_n * self->S_xx - self->S_x * self->S_x;
self->S_b = self->S_y / self->S_n - self->S_m * self->S_x / self->S_n;
self->S_r = self->S_n * self->S_xy - self->S_x * self->S_y;
self->S_r /= sqrt((self->S_n * self->S_xx - self->S_x * self->S_x)
* (self->S_n * self->S_yy - self->S_y * self->S_y));
if (self->S_n > 2) {
snprintf(line, CMDLEN, "Motor=%s stats: n=%.0f, S_x=%.0f, S_y=%.0f, S_xx=%.0f, S_yy=%.0f, S_xy=%.0f",
self->name,
self->S_n,
self->S_x,
self->S_y,
self->S_xx,
self->S_yy,
self->S_xy);
SICSLogWrite(line, eStatus);
snprintf(line, CMDLEN, "Motor=%s stats: n=%.0f, m=%f, b=%f, r=%f, stepsPerX=%f",
self->name,
self->S_n,
self->S_m,
self->S_b,
self->S_r,
self->cntsPerX * self->S_m);
SICSLogWrite(line, eStatus);
}
}
}
}
change_state(self, self->myMoveCallerReturn);
return;
@@ -4928,6 +4975,37 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
SCWrite(pCon, line, eValue);
return 1;
}
else if(self->abs_encoder && strcasecmp("stats", argv[1]) == 0) {
char line[132];
snprintf(line, CMDLEN, "%s.stats=%s samples=%.0f, slope=%f, intercept=%f, r=%f, stepsPerX=%f",
self->name,
self->doStats ? "ON" : "OFF",
self->S_n,
self->S_m,
self->S_b,
self->S_r,
self->cntsPerX * self->S_m);
SCWrite(pCon, line, eValue);
if (argc > 2) {
if (strcasecmp(argv[2], "reset") == 0) {
/* reset the stats */
self->S_x = 0.0;
self->S_y = 0.0;
self->S_xx = 0.0;
self->S_yy = 0.0;
self->S_xy = 0.0;
self->S_n = 0.0;
self->S_m = 0.0;
self->S_b = 0.0;
self->S_r = 0.0;
} else if (strcasecmp(argv[2], "on") == 0) {
self->doStats = true;
} else if (strcasecmp(argv[2], "off") == 0) {
self->doStats = false;
}
}
return 1;
}
else if(strcasecmp("testing", argv[1]) == 0) {
if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
self->testing = true;