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

51
site_ansto/motor_dmc2280.c
View File

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