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

merge work in progress

Browse Source
This commit is contained in:
Douglas Clowes
2012-11-26 15:10:25 +11:00
parent 9a1be5a80b
commit 49123234d0
6 changed files with 37 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
motor.c
View File

@@ -81,6 +81,7 @@
#define IGNOREFAULT 10
#define MOVECOUNT 11
extern double DoubleTime(void);
/*-------------------------------------------------------------------------*/
static void *MotorGetInterface(void *pData, int iID)
@@ -344,8 +345,9 @@ static int checkPosition(pMotor self, SConnection * pCon)
SCWrite(pCon, pBueffel, eWarning);
return HWFault;
}
snprintf(pBueffel, 131, "WARNING: %s off position by %f",
self->name, absf(fHard - self->fTarget));
snprintf(pBueffel, 131, "WARNING: %s off position by %f%s",
self->name, absf(fHard - self->fTarget),
self->fTarget > fHard ? "-" : "");
SCWrite(pCon, pBueffel, eLog);
status = statusRunTo(self, pCon);
return status;
@@ -354,16 +356,19 @@ static int checkPosition(pMotor self, SConnection * pCon)
}
/*--------------------------------------------------------------------*/
static void finishDriving(pMotor self, SConnection * pCon)
void finishDriving(pMotor self, SConnection * pCon)
{
MotCallback sCall;
MotorGetSoftPosition(self, pCon, &sCall.fVal);
sCall.pName = self->name;
self->fPosition = sCall.fVal;
self->fPosition = sCall.fVal;
InvokeCallBack(self->pCall, MOTDRIVE, &sCall); /* send also very last position */
InvokeCallBack(self->pCall, MOTEND, &sCall);
tracePar(self->name,"%f",sCall.fVal);
if (self->moving) {
InvokeCallBack(self->pCall, MOTDRIVE, &sCall); /* send also very last position */
InvokeCallBack(self->pCall, MOTEND, &sCall);
tracePar(self->name,"%f",sCall.fVal);
}
self->moving = 0;
self->running = 0;
}
@@ -466,13 +471,15 @@ static void handleMoveCallback(pMotor self, SConnection * pCon)
{
MotCallback sCall;
self->posCount++;
if (self->posCount >= ObVal(self->ParArray, MOVECOUNT)) {
double current_time, skip_time;
current_time = DoubleTime();
skip_time = 0.001 * ObVal(self->ParArray,MOVECOUNT);
if(self->last_report_time + skip_time <= current_time) {
MotorGetSoftPosition(self, pCon, &sCall.fVal);
sCall.pName = self->name;
InvokeCallBack(self->pCall, MOTDRIVE, &sCall);
tracePar(self->name,"%f",sCall.fVal);
self->posCount = 0;
self->last_report_time = current_time;
}
}
@@ -738,8 +745,10 @@ static long MotorRunImpl(void *sulf, SConnection * pCon, float fNew)
self->posFaultCount = 0;
self->retryCount = 0;
self->stopped = 0;
self->moving = 1;
self->fTarget = fHard;
InvokeCallBack(self->pCall, HDBVAL, self);
self->last_report_time = 0.0;
self->posCount = 0;
iRet = self->pDriver->RunTo(self->pDriver, fHard);
if (iRet != OKOK) { /* try three times to fix it */
@@ -1066,7 +1075,10 @@ int MotorCreate(SConnection * pCon, SicsInterp * pSics, void *pData,
}
/* create the interpreter command */
iRet = AddCommand(pSics, argv[1], MotorAction, MotorKill, pNew);
if (pNew->pActionRoutine)
iRet = AddCommand(pSics, argv[1], pNew->pActionRoutine, MotorKill, pNew);
else
iRet = AddCommand(pSics, argv[1], MotorAction, MotorKill, pNew);
if (!iRet) {
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: duplicate command %s not created", argv[1]);
SCWrite(pCon, pBueffel, eError);
@@ -1287,6 +1299,7 @@ int MotorAction(SConnection * pCon, SicsInterp * pSics, void *pData,
}
pMoti->lastValue = fValue;
RemoveCallbackCon(self->pCall, pCon);
lID = RegisterCallback(self->pCall, MOTDRIVE, InterestCallback,
pMoti, KillInfo);
DeleteTokenList(pList);