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

Refinements to motion reporting for motors

Browse Source
This commit is contained in:
Douglas Clowes
2014-03-13 16:00:24 +11:00
parent 0536c16843
commit ef236ecb1b
1 changed files with 50 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
site_ansto/motor_dmc2280.c
View File

@ -46,7 +46,7 @@ extern double DoubleTime(void);
* with valid values. * with valid values.
* \see getDMCSetting * \see getDMCSetting
*/ */
enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration}; typedef enum report_type {eReportMotion, eReportIdle, eReportFinal} ReportType;
enum commandtype {CMD_RUN=1, CMD_HALT=2}; enum commandtype {CMD_RUN=1, CMD_HALT=2};
typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv; typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv;
@ -1153,28 +1153,52 @@ static void set_lastMotion(pDMC2280Driv self, int steps, int counts) {
gettimeofday(&(self->time_lastPos_set), NULL); gettimeofday(&(self->time_lastPos_set), NULL);
} }
static bool report_motion(pDMC2280Driv self, bool isIdle) { /*
SConnection *pDumCon; * TODO: idle and final reporting
MotCallback sCall; * when final, always report MOTDRIVE and MOTEND
double current_time, skip_time; * when idle, if time and motion > threshold, report MOTDRIVE and MOTEND
long lDelta; * when neither, if time and motion > threshold, report MOTDRIVE but not MOTEND
*/
static bool report_motion(pDMC2280Driv self, ReportType reportType) {
double current_time;
bool doMotDrive = false;
bool doMotEnd = false;
if (self->pMot == NULL) if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name); self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot); assert(self->pMot);
current_time = DoubleTime(); current_time = DoubleTime();
skip_time = 0.001 * getMotorParam(self, "movecount"); if (reportType == eReportFinal) {
if (self->lastReportTime + skip_time <= current_time) { doMotDrive = true;
if (self->abs_encoder) doMotEnd = true;
lDelta = fabs(self->currCounts - self->lastReportedCounts); } else {
else double skip_time;
lDelta = fabs(self->currSteps - self->lastReportedSteps); skip_time = 0.001 * getMotorParam(self, "movecount");
if (lDelta > 1) { if (self->lastReportTime + skip_time <= current_time) {
pDumCon = SCCreateDummyConnection(pServ->pSics); long lDelta;
MotorGetSoftPosition(self->pMot, pDumCon, &sCall.fVal); if (self->abs_encoder)
SCDeleteConnection(pDumCon); lDelta = fabs(self->currCounts - self->lastReportedCounts);
sCall.pName = self->pMot->name; else
lDelta = fabs(self->currSteps - self->lastReportedSteps);
if (reportType == eReportIdle) {
if (lDelta > 10) {
doMotDrive = true;
doMotEnd = true;
}
} else
doMotDrive = true;
}
}
if (doMotDrive || doMotEnd) {
SConnection *pDumCon;
MotCallback sCall;
pDumCon = SCCreateDummyConnection(pServ->pSics);
MotorGetSoftPosition(self->pMot, pDumCon, &sCall.fVal);
SCDeleteConnection(pDumCon);
sCall.pName = self->pMot->name;
if (doMotDrive) {
InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall); InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall);
if (isIdle) if (doMotEnd)
InvokeCallBack(self->pMot->pCall, MOTEND, &sCall); InvokeCallBack(self->pMot->pCall, MOTEND, &sCall);
self->lastReportedCounts = self->currCounts; self->lastReportedCounts = self->currCounts;
self->lastReportedSteps = self->currSteps; self->lastReportedSteps = self->currSteps;
@ -2361,8 +2385,12 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
return; return;
} }
/* if the motor moved, update any observers */ /* if the motor moved, update any observers */
if (report_motion(self, true)) if (self->doReportMotion) {
set_lastMotion(self, self->currSteps, self->currCounts); (void) report_motion(self, eReportFinal);
self->doReportMotion = false;
} else {
(void) report_motion(self, eReportIdle);
}
if (trace_switches || self->debug) if (trace_switches || self->debug)
DMC_SetTimer(self, self->motorPollFast); DMC_SetTimer(self, self->motorPollFast);
else else
@ -3172,6 +3200,7 @@ static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event) {
if (self->oscillate_count > 0 && self->oscillate_counter >= self->oscillate_count) if (self->oscillate_count > 0 && self->oscillate_counter >= self->oscillate_count)
self->doOscillate = false; self->doOscillate = false;
if (!self->doOscillate) { if (!self->doOscillate) {
(void) report_motion(self, eReportFinal);
change_state(self, DMCState_OffTimer); /* TODO: check this is OK */ change_state(self, DMCState_OffTimer); /* TODO: check this is OK */
return; return;
} }
@ -3325,7 +3354,7 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
} }
} }
if (self->doReportMotion) if (self->doReportMotion)
(void) report_motion(self, false); (void) report_motion(self, eReportMotion);
/* if the status response can be handled here, return */ /* if the status response can be handled here, return */
if (motorHandleStatus(self)) if (motorHandleStatus(self))
return; return;