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

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

Browse Source
This commit is contained in:
Douglas Clowes
2013-08-07 12:22:12 +10:00
parent 537fad7df4
commit 4069e71748
1 changed files with 171 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

171
site_ansto/motor_dmc2280.c
View File

@@ -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) {
if (argc < 3) {
SCPrintf(pCon, eValue, "%s.tracking %s", self->name, self->doTracking ? "ON" : "OFF");
return 1;
}
else if(self->abs_encoder && strcasecmp("stats", argv[1]) == 0) {
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,