Add some motor statistics and regression to calculate steps per unit

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,"
+            if (true /*self->debug*/) {
-                " steps=%ld, counts=%ld, stepsPerX=%.6f,"
+              snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f,"
-                " ratio=(%.6f-%.6f), time=%.3f",
+                  " steps=%ld, counts=%ld, stepsPerX=%.6f,"
-                self->name,
+                  " ratio=(%.6f-%.6f), time=%.3f",
-                units,
+                  self->name,
-                steps,
+                  units,
-                counts,
+                  steps,
-                (double) steps / units,
+                  counts,
-                self->minRatio,
+                  (double) steps / units,
-                self->maxRatio,
+                  self->minRatio,
-                time);
+                  self->maxRatio,
-            SICSLogWrite(line, eStatus);
+                  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;