diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index a9b24453..887e9b36 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -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;