Add motor tracking on/off/clear/stats/save commands

site_ansto/motor_dmc2280.c

@@ -11,6 +11,7 @@
  * - check for motors enabled on plc
  * - Check error bit, see Dan's email
  */
+#include <limits.h>
 #include <stdlib.h>
 /* ISO C Standard:  7.16  Boolean type and values  <stdbool.h> */
 #include <stdbool.h>
@@ -103,6 +104,19 @@ struct EvtEvent_s {
   } event;
 };
 
+typedef struct TrackEntry_s {
+  double Timestamp;
+  double Position;
+  int     Steps;
+  int     Counts;
+} TrackEntry;
+#define TrackEntryCount 100
+typedef struct TrackEntryBuffer_s {
+  struct TrackEntryBuffer_s* nextBuff;
+  int index;
+  TrackEntry buff[TrackEntryCount];
+} TrackEntryBuffer;
+
 #define VAR_REQ       (1<<0)
 #define VAR_RSP       (1<<1)
 #define VAR_RUN       (1<<2)
@@ -255,6 +269,8 @@ struct __MoDriv {
   double S_m;
   double S_b;
   double S_r;
+  bool doTracking;
+  TrackEntryBuffer *trackHead, *trackTail;
 };
 
 int DMC2280MotionControl = 1; /* defaults to enabled */
@@ -345,6 +361,48 @@ static double getMotorParam(pDMC2280Driv self, char *param) {
   return fParam;
 }
 
+static void TrackingRelease(pDMC2280Driv self) {
+  while (self->trackHead) {
+    TrackEntryBuffer *victim;
+    victim = self->trackHead;
+    self->trackHead = self->trackHead->nextBuff;
+    free(victim);
+  }
+  self->trackTail = NULL;
+}
+
+static TrackEntryBuffer* TrackingBufferAllocate(pDMC2280Driv self) {
+  /* give the last one if still space available */
+  if (self->trackTail)
+    if (self->trackTail->index < TrackEntryCount)
+      return self->trackTail;
+  /* allocate and chain in a new one */
+  if (NULL == self->trackHead) {
+    self->trackHead = (TrackEntryBuffer *) malloc(sizeof(TrackEntryBuffer));
+    self->trackTail = self->trackHead;
+    if (NULL == self->trackTail) {
+      SICSLogPrintf(eLogError, "Out of memory in TrackingAllocate");
+      return NULL;
+    }
+  } else {
+    self->trackTail->nextBuff = (TrackEntryBuffer *) malloc(sizeof(TrackEntryBuffer));
+    if (NULL == self->trackTail->nextBuff) {
+      SICSLogPrintf(eLogError, "Out of memory in TrackingAllocate");
+      return NULL;
+    }
+    self->trackTail = self->trackTail->nextBuff;
+  }
+  memset(self->trackTail, 0, sizeof(TrackEntryBuffer));
+  return self->trackTail;
+}
+
+static TrackEntry *TrackEntryAllocate(pDMC2280Driv self) {
+  TrackEntryBuffer *bp = TrackingBufferAllocate(self);
+  if (NULL == bp)
+    return NULL;
+  return &bp->buff[bp->index++];
+}
+
 static bool check_positions(pDMC2280Driv self) {
   char line[CMDLEN];
   int missing = 0;
@@ -2450,6 +2508,8 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
           self->stepCount = 0;
           self->moveStartTime = DoubleTime();
           if (!self->legacy_fsm) {
+            if (self->trackHead)
+              TrackingRelease(self);
             self->myNextState = NULL;
             if (self->backlash_offset != 0) {
               change_state(self, DMCState_BacklashStart);
@@ -3020,6 +3080,18 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
       self->origSteps = self->currSteps;
       self->minRatio = 0.0;
       self->maxRatio = 0.0;
+      /*
+       * Record how the motor is tracking
+       */
+      if (self->doTracking) {
+        TrackEntry *ent = TrackEntryAllocate(self);
+        if (ent) {
+          ent->Timestamp = DoubleTime();
+          ent->Position = self->currPosition;
+          ent->Steps = self->currSteps;
+          ent->Counts = self->currCounts;
+        }
+      }
       /* begin moving */
       cmdBegin(self);
       self->stepCount++;
@@ -3053,6 +3125,7 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
           return;
         } else if (self->subState == 2) { /* Status */
           int iRet;
+
           /* update the stop time now in case of error exit */
           self->moveStopTime = DoubleTime();
           /* parse the status response and set error codes */
@@ -3060,6 +3133,18 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
           /* if the parse failed break out of here */
           if (iRet == 0)
             break;
+          /*
+           * Record how the motor is tracking
+           */
+          if (self->doTracking) {
+            TrackEntry *ent = TrackEntryAllocate(self);
+            if (ent) {
+              ent->Timestamp = DoubleTime();
+              ent->Position = self->currPosition;
+              ent->Steps = self->currSteps;
+              ent->Counts = self->currCounts;
+            }
+          }
           /* if the status response can be handled here, return */
           if (motorHandleStatus(self))
             return;
@@ -5262,8 +5347,92 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
     else if(strcasecmp("halt", argv[1]) == 0) {
       state_cmd_execute(self, CMD_HALT);
       return 1;
-    }
+    } else if(self->abs_encoder && strcasecmp("tracking", argv[1]) == 0) {
-    else if(self->abs_encoder && strcasecmp("stats", argv[1]) == 0) {
+      if (argc < 3) {
+        SCPrintf(pCon, eValue, "%s.tracking %s", self->name, self->doTracking ? "ON" : "OFF");
+        return 1;
+      }
+      if (strcasecmp(argv[2], "on") == 0) {
+        self->doTracking = true;
+        SCPrintf(pCon, eValue, "%s.tracking %s", self->name, self->doTracking ? "ON" : "OFF");
+      } else if (strcasecmp(argv[2], "off") == 0) {
+        self->doTracking = false;
+        SCPrintf(pCon, eValue, "%s.tracking %s", self->name, self->doTracking ? "ON" : "OFF");
+      } else if (strcasecmp(argv[2], "clear") == 0) {
+        TrackingRelease(self);
+        SCSendOK(pCon);
+      } else if (strcasecmp(argv[2], "stats") == 0) {
+        TrackEntryBuffer *bp;
+        TrackEntry *ep;
+        int i;
+        double S_x = 0.0;
+        double S_y = 0.0;
+        double S_xx = 0.0;
+        double S_yy = 0.0;
+        double S_xy = 0.0;
+        double S_n = 0.0;
+        double S_m = 0.0;
+        double S_b = 0.0;
+        double S_r = 0.0;
+        for (bp = self->trackHead; bp; bp = bp->nextBuff) {
+          for (i = 0; i < bp->index; i++) {
+            ep = &bp->buff[i];
+            /* TODO */
+            S_x += (double) ep->Counts;
+            S_y += (double) ep->Steps;
+            S_xy += (double) ep->Counts * (double) ep->Steps;
+            S_xx += (double) ep->Counts * (double) ep->Counts;
+            S_yy += (double) ep->Steps * (double) ep->Steps;
+            S_n += (double) 1;
+          }
+        }
+        S_m = S_n * S_xy - S_x * S_y;
+        S_m /= S_n * S_xx - S_x * S_x;
+        S_b = S_y / S_n - S_m * S_x / S_n;
+        S_r = S_n * S_xy - S_x * S_y;
+        S_r /= sqrt((S_n * S_xx - S_x * S_x)
+            * (S_n * S_yy - S_y * S_y));
+        SCPrintf(pCon, eValue, "Motor=%s stats: n=%.0f, m=%f, b=%f, r=%f, stepsPerX=%f",
+            self->name, S_n, S_m, S_b, S_r, self->cntsPerX * S_m);
+      } else if (strcasecmp(argv[2], "save") == 0) {
+        TrackEntryBuffer *bp;
+        TrackEntry *ep;
+        int i;
+        FILE *fp;
+        char *rp;
+        if (argc < 4) {
+          SCPrintf(pCon, eError, "Motor %s, %s subcommand %s needs filename",
+              argv[0], argv[1], argv[2]);
+          return 1;
+        }
+        rp = realpath(argv[3], NULL);
+        fp = fopen(argv[3], "w");
+        if (NULL == fp) {
+          SCPrintf(pCon, eError, "Motor %s, %s subcommand %s failed to open %s",
+              argv[0], argv[1], argv[2], rp ? rp : argv[3]);
+          if (rp)
+            free(rp);
+          return 1;
+        }
+        fprintf(fp, "Timestamp,MotorPosition,MotorSteps,EncoderCounts\n");
+        for (bp = self->trackHead; bp; bp = bp->nextBuff) {
+          for (i = 0; i < bp->index; i++) {
+            ep = &bp->buff[i];
+            /* TODO timestamp in HH:MM:SS.cc, softPosition maybe ??*/
+            fprintf(fp, "%.6f,%.6f,%d,%d\n", ep->Timestamp, ep->Position, ep->Steps, ep->Counts);
+          }
+        }
+        fclose(fp);
+        SCPrintf(pCon, eStatus, "Motor %s, tracking data saved to %s",
+            argv[0], rp ? rp : argv[3]);
+        if (rp)
+          free(rp);
+      } else {
+        SCPrintf(pCon, eError, "Motor %s, %s subcommand %s not recognized",
+            argv[0], argv[1], argv[2]);
+      }
+      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,