0825c48986
- Fixed a bug in hmdata.c - Fixed an issue with tempoerature writing through RemObjects in mesure - Added auxiliary reflections to tasub - Make maximize use soft motor positions
384 lines
11 KiB
C
384 lines
11 KiB
C
/*----------------------------------------------------------------------
|
|
This is the implementation file for the AntiCollider, a complex movements
|
|
control module for SICS. See anticollider.tex for more information.
|
|
|
|
copyright: see file copyright
|
|
|
|
Mark Koennecke, August 2002
|
|
------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <tcl.h>
|
|
#include "fortify.h"
|
|
#include "lld.h"
|
|
#include "motreglist.h"
|
|
#include "anticollider.i"
|
|
#include "anticollider.h"
|
|
|
|
/*---------------------------------------------------------------------
|
|
As there should be only one AntiCollider in a system, I use a static
|
|
pointer to the AntiCollider here in order to facilitate access.
|
|
Otherwise more complex mechanisms must be devised in order to pass this
|
|
pointer into ColliderSetValue and ColliderCheckStatus
|
|
----------------------------------------------------------------------*/
|
|
static pAntiCollider myCollider = NULL;
|
|
|
|
/*--------------------------------------------------------------------
|
|
the replacement function for the motor's drivable interface SetValue
|
|
function. It enters the new target into the motor list.
|
|
---------------------------------------------------------------------*/
|
|
static long ReplacementSetValue(void *pData, SConnection *pCon, float fTarget){
|
|
pMotReg pMot = NULL;
|
|
|
|
assert(myCollider != NULL);
|
|
|
|
pMot = FindMotFromDataStructure(myCollider->motorList,pData);
|
|
if(pMot != NULL){
|
|
SetRegMotTarget(pMot,fTarget);
|
|
myCollider->isDirty = 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
/*---------------------------------------------------------------------
|
|
The replacement CheckStatus function for controlled motors.
|
|
Start AntiCollider if not running and finish. Rest of work done by
|
|
AntiCollider.
|
|
-----------------------------------------------------------------------*/
|
|
static int ReplacementCheckStatus(void *pData, SConnection *pCon){
|
|
pMotReg pMot = NULL;
|
|
|
|
assert(myCollider != NULL);
|
|
|
|
if(myCollider->isDirty == 1){
|
|
myCollider->isDirty = 0;
|
|
StartDevice(pServ->pExecutor,
|
|
"anticollider",
|
|
myCollider->pDes,
|
|
myCollider,
|
|
pCon,
|
|
77.77);
|
|
return HWIdle;
|
|
} else {
|
|
return HWIdle;
|
|
}
|
|
}
|
|
/*------------------------------------------------------------------------
|
|
The collider SetValue function
|
|
-------------------------------------------------------------------------*/
|
|
static long ColliderSetValue(void *pData, SConnection *pCon, float fTarget){
|
|
pAntiCollider self = (pAntiCollider) pData;
|
|
int iRet;
|
|
pMotReg pMot = NULL;
|
|
char pBueffel[80];
|
|
char *ruenBuffer = NULL;
|
|
|
|
Tcl_DString command;
|
|
|
|
/*
|
|
build command list
|
|
*/
|
|
if(self->colliderScript == NULL){
|
|
SCWrite(pCon,"ERROR: no collider script defined",eError);
|
|
return 0;
|
|
}
|
|
|
|
Tcl_DStringInit(&command);
|
|
Tcl_DStringAppend(&command,self->colliderScript,-1);
|
|
iRet = LLDnodePtr2First(self->motorList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(self->motorList,&pMot);
|
|
if(pMot != NULL){
|
|
if(pMot->iActive){
|
|
CreateTargetString(pMot,pBueffel);
|
|
Tcl_DStringAppend(&command, pBueffel,-1);
|
|
pMot->iActive = 0;
|
|
}
|
|
}
|
|
iRet = LLDnodePtr2Next(self->motorList);
|
|
}
|
|
|
|
/*
|
|
kill old collider sequence
|
|
*/
|
|
LLDdelete(self->sequenceList);
|
|
self->sequenceList = LLDcreate(sizeof(Sequence));
|
|
self->level = -1; /* otherwise level 0 will not be started */
|
|
|
|
/*
|
|
evaluate colliderScript
|
|
*/
|
|
iRet = Tcl_Eval(pServ->pSics->pTcl,Tcl_DStringValue(&command));
|
|
if(iRet != TCL_OK){
|
|
SCWrite(pCon,"ERROR: Movement not possible or bad collider script",eError);
|
|
SCWrite(pCon,Tcl_DStringValue(&command),eError);
|
|
/*
|
|
SCWrite(pCon,pServ->pSics->pTcl->result,eError);
|
|
*/
|
|
SCSetInterrupt(pCon,eAbortOperation);
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
we are set
|
|
*/
|
|
Tcl_DStringFree(&command);
|
|
return 1;
|
|
}
|
|
/*----------------------------------------------------------------------
|
|
The Collider CheckStatus function
|
|
-----------------------------------------------------------------------*/
|
|
static int ColliderCheckStatus(void *pData, SConnection *pCon){
|
|
int count = 0;
|
|
pAntiCollider self = (pAntiCollider) pData;
|
|
|
|
assert(self);
|
|
|
|
if(SCGetInterrupt(pCon) != eContinue){
|
|
return HWIdle;
|
|
}
|
|
|
|
count = CheckAllMotors(self->motorList,pCon);
|
|
if(count == 0){
|
|
self->level++;
|
|
count = StartLevel(self->level,
|
|
self->sequenceList,
|
|
self->motorList,
|
|
pCon);
|
|
if(count == 0){
|
|
/*
|
|
no more levels. All done
|
|
*/
|
|
return HWIdle;
|
|
} else {
|
|
return HWBusy;
|
|
}
|
|
} else {
|
|
return HWBusy;
|
|
}
|
|
}
|
|
/*----------------------------------------------------------------------
|
|
Most of these are dummies........
|
|
-----------------------------------------------------------------------*/
|
|
static int ColliderHalt(void *pData){
|
|
pAntiCollider self = (pAntiCollider) pData;
|
|
|
|
StopAllMotors(self->motorList);
|
|
self->level = 999999999;
|
|
return 1;
|
|
}
|
|
/*---------------------------------------------------------------------*/
|
|
static int ColliderLimits(void *self, float fVal, char *error,
|
|
int iErren){
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------*/
|
|
static float ColliderGetValue(void *self, SConnection *pCon){
|
|
return 77.77;
|
|
}
|
|
/*--------------------------------------------------------------------*/
|
|
int StartLevel(int level, int sequenceList, int motorList, SConnection *pCon){
|
|
Sequence seq;
|
|
pMotReg pMot = NULL;
|
|
int iRet, status;
|
|
int count = 0;
|
|
char pBueffel[132];
|
|
|
|
iRet = LLDnodePtr2First(sequenceList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(sequenceList,&seq);
|
|
if(seq.level == level){
|
|
pMot = FindMotEntry(motorList,seq.pMotor);
|
|
if(pMot){
|
|
status = StartRegMot(pMot,pCon,seq.target);
|
|
/*
|
|
* I have to ignore the problem here: if I do not increment the count
|
|
* all the other levels will not be drive and the anticollider
|
|
* gets into a mess
|
|
*/
|
|
count++;
|
|
} else {
|
|
sprintf(pBueffel,"ERROR: motor %s, requested from anticollider script",
|
|
seq.pMotor);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
SCWrite(pCon,"ERROR: motor NOT found, fix script!",eError);
|
|
}
|
|
}
|
|
iRet = LLDnodePtr2Next(sequenceList);
|
|
}
|
|
return count;
|
|
}
|
|
/*--------------------------------------------------------------------*/
|
|
static void ListSequence(int sequenceList, SConnection *pCon){
|
|
Sequence seq;
|
|
int iRet;
|
|
char pBueffel[132];
|
|
|
|
SCWrite(pCon,"level motor target",eValue);
|
|
iRet = LLDnodePtr2First(sequenceList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(sequenceList,&seq);
|
|
sprintf(pBueffel,"%d %s %f",seq.level,seq.pMotor,seq.target);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
iRet = LLDnodePtr2Next(sequenceList);
|
|
}
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void *ColliderGetInterface(void *pData, int iID) {
|
|
pAntiCollider self = NULL;
|
|
|
|
self = (pAntiCollider)pData;
|
|
assert(self);
|
|
|
|
if(iID == DRIVEID){
|
|
return self->pDriv;
|
|
}
|
|
return NULL;
|
|
}
|
|
/*----------------------------------------------------------------------*/
|
|
void KillCollider(void *pData){
|
|
pAntiCollider self = (pAntiCollider)pData;
|
|
|
|
if(self == NULL){
|
|
return;
|
|
}
|
|
|
|
if(self->pDes != NULL){
|
|
DeleteDescriptor(self->pDes);
|
|
}
|
|
if(self->pDriv != NULL){
|
|
free(self->pDriv);
|
|
}
|
|
if(self->colliderScript != NULL){
|
|
free(self->colliderScript);
|
|
}
|
|
if(self->motorList > 0){
|
|
KillMotList(self->motorList);
|
|
}
|
|
if(self->sequenceList > 0){
|
|
LLDdelete(self->sequenceList);
|
|
}
|
|
free(self);
|
|
myCollider = NULL;
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
int AntiColliderFactory(SConnection *pCon, SicsInterp *pSics,
|
|
void *pData,
|
|
int argc, char *argv[]){
|
|
|
|
myCollider = (pAntiCollider)malloc(sizeof(AntiCollider));
|
|
if(myCollider == NULL){
|
|
SCWrite(pCon,"ERROR: out of memory when generating AntiCollider",eError);
|
|
return 0;
|
|
}
|
|
memset(myCollider,0,sizeof(AntiCollider));
|
|
|
|
myCollider->pDes = CreateDescriptor("AntiCollider");
|
|
myCollider->pDriv = CreateDrivableInterface();
|
|
if(!myCollider->pDes || !myCollider->pDriv){
|
|
KillCollider(myCollider);
|
|
SCWrite(pCon,"ERROR: out of memory when generating AntiCollider",eError);
|
|
return 0;
|
|
}
|
|
|
|
myCollider->pDes->GetInterface = ColliderGetInterface;
|
|
myCollider->pDriv->Halt = ColliderHalt;
|
|
myCollider->pDriv->CheckLimits = ColliderLimits;
|
|
myCollider->pDriv->SetValue = ColliderSetValue;
|
|
myCollider->pDriv->CheckStatus = ColliderCheckStatus;
|
|
myCollider->pDriv->GetValue = ColliderGetValue;
|
|
|
|
myCollider->motorList = LLDcreate(sizeof(void *));
|
|
myCollider->sequenceList = LLDcreate(sizeof(Sequence));
|
|
|
|
AddCommand(pSics,"anticollision",AntiColliderAction,
|
|
KillCollider,
|
|
myCollider);
|
|
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
int AntiColliderAction(SConnection *pCon, SicsInterp *pSics,
|
|
void *pData,
|
|
int argc, char *argv[]){
|
|
pAntiCollider self = (pAntiCollider)pData;
|
|
Sequence seq;
|
|
char pBueffel[256];
|
|
pMotReg pMot = NULL;
|
|
|
|
assert(self != NULL);
|
|
|
|
if(argc > 1){
|
|
if(strcmp(argv[1],"clear") == 0){
|
|
if(!SCMatchRights(pCon,usUser)){
|
|
return 0;
|
|
}
|
|
LLDdelete(self->sequenceList);
|
|
self->sequenceList = LLDcreate(sizeof(Sequence));
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
} else if(strcmp(argv[1],"list") == 0){
|
|
ListSequence(self->sequenceList,pCon);
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
if(argc < 3){
|
|
SCWrite(pCon,"ERROR : insufficient number of arguments to anticollision",
|
|
eError);
|
|
return 0;
|
|
}
|
|
|
|
strtolower(argv[1]);
|
|
if(strcmp(argv[1],"script") == 0){
|
|
if(!SCMatchRights(pCon,usMugger)){
|
|
return 0;
|
|
}
|
|
if(self->colliderScript != NULL){
|
|
free(self->colliderScript);
|
|
}
|
|
self->colliderScript = strdup(argv[2]);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
} else if(strcmp(argv[1],"register") == 0){
|
|
if(!SCMatchRights(pCon,usMugger)){
|
|
return 0;
|
|
}
|
|
if(FindDrivable(pSics,argv[2]) == NULL){
|
|
sprintf(pBueffel,"ERROR: %s is NOT drivable, cannot register",argv[2]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
pMot = RegisterMotor(argv[2],pSics,
|
|
ReplacementSetValue,
|
|
ReplacementCheckStatus);
|
|
if(pMot){
|
|
LLDnodeAppendFrom(self->motorList,&pMot);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
} else {
|
|
SCWrite(pCon,"ERROR: out of memory registering motor",eError);
|
|
return 0;
|
|
}
|
|
} else if(strcmp(argv[1],"add") == 0){
|
|
if(argc < 5){
|
|
SCWrite(pCon,
|
|
"ERROR: InsUfficient number of arguments to anticollicion add",
|
|
eError);
|
|
return 0;
|
|
}
|
|
seq.level = atoi(argv[2]);
|
|
strncpy(seq.pMotor,argv[3],79);
|
|
seq.target = atof(argv[4]);
|
|
LLDnodeAppendFrom(self->sequenceList,&seq);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
SCWrite(pCon,"ERROR: anticollider command not understood",eError);
|
|
return 0;
|
|
}
|
|
|
|
|