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