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

Add motion reporting for driver run and ascillate commands

Browse Source
This commit is contained in:
Douglas Clowes
2014-03-13 12:15:58 +11:00
parent 5eec1a9cd5
commit 0536c16843
1 changed files with 35 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
site_ansto/motor_dmc2280.c
View File

@ -196,6 +196,9 @@ struct __MoDriv {
double lastPosition; /**< Position at last position check */ double lastPosition; /**< Position at last position check */
int lastSteps; int lastSteps;
int lastCounts; int lastCounts;
double lastReportTime;
int lastReportedCounts;
int lastReportedSteps;
double origPosition; /**< Position at motor start */ double origPosition; /**< Position at motor start */
double origTime; /**< Time at motor start */ double origTime; /**< Time at motor start */
int origSteps; int origSteps;
@ -271,6 +274,7 @@ struct __MoDriv {
double S_r; double S_r;
bool doTracking; bool doTracking;
TrackEntryBuffer *trackHead, *trackTail; TrackEntryBuffer *trackHead, *trackTail;
bool doReportMotion; /**< flag for reporting motion when not driving */
bool doOscillate; /**< flag for motor oscillation */ bool doOscillate; /**< flag for motor oscillation */
bool oscillate_up; /**< next oscillation move is to high position */ bool oscillate_up; /**< next oscillation move is to high position */
bool oscillate_init; /**< initial move is not counted */ bool oscillate_init; /**< initial move is not counted */
@ -1149,18 +1153,36 @@ static void set_lastMotion(pDMC2280Driv self, int steps, int counts) {
gettimeofday(&(self->time_lastPos_set), NULL); gettimeofday(&(self->time_lastPos_set), NULL);
} }
static void report_motion(pDMC2280Driv self) { static bool report_motion(pDMC2280Driv self, bool isIdle) {
SConnection *pDumCon; SConnection *pDumCon;
MotCallback sCall; MotCallback sCall;
double current_time, skip_time;
long lDelta;
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);
pDumCon = SCCreateDummyConnection(pServ->pSics); current_time = DoubleTime();
MotorGetSoftPosition(self->pMot,pDumCon,&sCall.fVal); skip_time = 0.001 * getMotorParam(self, "movecount");
SCDeleteConnection(pDumCon); if (self->lastReportTime + skip_time <= current_time) {
sCall.pName = self->pMot->name; if (self->abs_encoder)
InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall); lDelta = fabs(self->currCounts - self->lastReportedCounts);
InvokeCallBack(self->pMot->pCall, MOTEND, &sCall); else
lDelta = fabs(self->currSteps - self->lastReportedSteps);
if (lDelta > 1) {
pDumCon = SCCreateDummyConnection(pServ->pSics);
MotorGetSoftPosition(self->pMot, pDumCon, &sCall.fVal);
SCDeleteConnection(pDumCon);
sCall.pName = self->pMot->name;
InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall);
if (isIdle)
InvokeCallBack(self->pMot->pCall, MOTEND, &sCall);
self->lastReportedCounts = self->currCounts;
self->lastReportedSteps = self->currSteps;
self->lastReportTime = current_time;
return true;
}
}
return false;
} }
/** \brief Reads absolute encoder. /** \brief Reads absolute encoder.
@ -2329,7 +2351,6 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* Status Response */ if (self->subState == 1) { /* Status Response */
int iRet; int iRet;
long lDelta;
iRet = rspStatus(self, pCmd->inp_buf); iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
@ -2340,14 +2361,8 @@ 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 (self->abs_encoder) if (report_motion(self, true))
lDelta = fabs(self->currCounts - self->lastCounts);
else
lDelta = fabs(self->currSteps - self->lastSteps);
if (lDelta > 10) {
set_lastMotion(self, self->currSteps, self->currCounts); set_lastMotion(self, self->currSteps, self->currCounts);
report_motion(self);
}
if (trace_switches || self->debug) if (trace_switches || self->debug)
DMC_SetTimer(self, self->motorPollFast); DMC_SetTimer(self, self->motorPollFast);
else else
@ -3309,6 +3324,8 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
ent->Counts = self->currCounts; ent->Counts = self->currCounts;
} }
} }
if (self->doReportMotion)
(void) report_motion(self, false);
/* 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;
@ -3972,6 +3989,7 @@ static int DMC2280Run(void *pData,float fValue){
if (fValue >= self->fLower && fValue <= self->fUpper) { if (fValue >= self->fLower && fValue <= self->fUpper) {
self->fTarget = fValue; self->fTarget = fValue;
self->doOscillate = false; self->doOscillate = false;
self->doReportMotion = false;
state_cmd_execute(self, CMD_RUN); state_cmd_execute(self, CMD_RUN);
return 1; return 1;
} }
@ -5492,6 +5510,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
else else
self->oscillate_up = false; self->oscillate_up = false;
self->doOscillate = true; self->doOscillate = true;
self->doReportMotion = true;
self->oscillate_init = true; self->oscillate_init = true;
self->oscillate_counter = 0; self->oscillate_counter = 0;
state_cmd_execute(self, CMD_RUN); state_cmd_execute(self, CMD_RUN);
@ -5561,6 +5580,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
self->fTarget = fHard; self->fTarget = fHard;
SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard); SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard);
self->doOscillate = false; self->doOscillate = false;
self->doReportMotion = true;
state_cmd_execute(self, CMD_RUN); state_cmd_execute(self, CMD_RUN);
} }
} }