From 4069e7174861577fddb33dd6902f5424bd24fdf5 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Wed, 7 Aug 2013 12:22:12 +1000 Subject: [PATCH] Add motor tracking on/off/clear/stats/save commands --- site_ansto/motor_dmc2280.c | 173 ++++++++++++++++++++++++++++++++++++- 1 file changed, 171 insertions(+), 2 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 31a16287..20628b9f 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -11,6 +11,7 @@ * - check for motors enabled on plc * - Check error bit, see Dan's email */ +#include #include /* ISO C Standard: 7.16 Boolean type and values */ #include @@ -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("stats", argv[1]) == 0) { + } else if(self->abs_encoder && strcasecmp("tracking", 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,