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

- First working version of the TRICS collision protection module

Browse Source
This commit is contained in:
cvs
2002-08-14 14:24:00 +00:00
parent a59f15d5f0
commit 3ba5f28b65
35 changed files with 1427 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
motor.c
View File

@ -9,6 +9,7 @@
Mark Koennecke, November 1996
revised: Mark Koennecke, June 1997
callback added: Mark Koennecke, August 1997
endscript facility added: Mark Koennecke, August 2002
Copyright:
@ -195,7 +196,9 @@
SCSetInterrupt(pCon,iVal);
}
}
/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------
Refactor
--------------------------------------------------------------------------*/
static int MotorStatus(void *sulf, SConnection *pCon)
{
float fHard;
@ -250,6 +253,8 @@
/* motor thinks he is done */
if( (iRet == OKOK) || (iRet == HWIdle))
{
MotorGetSoftPosition(self,pCon,&sCall.fVal);
InvokeCallBack(self->pCall, MOTEND, &sCall);
self->fPosition = fHard;
if(absf(fHard - self->fTarget) > ObVal(self->ParArray,PREC))
{
@ -342,8 +347,7 @@
iRetry = 0;
return iRet;
}
/*---------------------------------------------------------------------------*/
pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv)
/*---------------------------------------------------------------------------*/ pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv)
{
pMotor pM = NULL;
@ -380,6 +384,7 @@
ObParInit(pM->ParArray,ECOUNT,"failafter",3.0,usMugger);
pDriv->GetPosition(pDriv,&(pM->fPosition));
pM->fTarget = pM->fPosition;
pM->endScriptID = 0;
/* copy arguments */
pM->pDriver = pDriv;
@ -1027,7 +1032,32 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
SCWrite(pInfo->pCon,pBueffel,eValue);
return 1;
}
/*------------------------------------------------------------------------*/
static void KillScript(void *pData)
{
if(pData != NULL)
{
free(pData);
}
}
/*------------------------ The endscript callback function ----------------*/
static int EndScriptCallback(int iEvent, void *pEvent, void *pUser)
{
char *pScript = NULL;
MotCallback *psCall;
char pCommand[1024];
int iRet;
assert(pEvent);
assert(pUser);
psCall = (MotCallback *)pEvent;
pScript = (char *)pUser;
sprintf(pCommand,"%s %f",pScript,psCall->fVal);
iRet = Tcl_Eval(pServ->pSics->pTcl,pCommand);
return iRet;
}
/*----------------------------------------------------------------------------
The wrapper function for a motor. Commands currently supported are:
@ -1037,6 +1067,8 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
motorname reset : puts softlimits to default
motorname interest : starts sending automatic
notifications when driving
motorname endscript : script to call when motor
finishes driving
-----------------------------------------------------------------------------*/
int MotorAction(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[])
@ -1122,7 +1154,6 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
DeleteTokenList(pList);
SCSendOK(pCon);
return 1;
}
else if(strcmp(pCurrent->text,"uninterest") == 0)
{
@ -1131,6 +1162,31 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
DeleteTokenList(pList);
return 1;
}
else if(strcmp(pCurrent->text,"endscript") == 0) /* endscript */
{
if(!SCMatchRights(pCon,usMugger))
{
return 0;
}
if(self->endScriptID != 0)
{
RemoveCallback(self->pCall, self->endScriptID);
self->endScriptID = 0;
}
pCurrent = pCurrent->pNext;
if(!pCurrent)
{
SCWrite(pCon,"ERROR: scriptname argument missing",eError);
return 0;
}
self->endScriptID =
RegisterCallback(self->pCall, MOTEND, EndScriptCallback,
strdup(pCurrent->text), KillScript);
SCRegister(pCon,pSics, self->pCall,self->endScriptID);
DeleteTokenList(pList);
SCSendOK(pCon);
return 1;
}
else /* one of the parameter commands or error left now */
{
pName = pCurrent;