- First working version of the TRICS collision protection module
This commit is contained in:
cvs
375
anticollider.c
Normal file
375
anticollider.c
Normal file
@@ -0,0 +1,375 @@
|
||||
/*----------------------------------------------------------------------
|
||||
This is the implementation file for the AntiCollider, a complex movements
|
||||
control module for SICS. See anticoliider.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 divised 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);
|
||||
}
|
||||
printf("%s\n", Tcl_DStringValue(&command));
|
||||
|
||||
/*
|
||||
kill old collider sequence
|
||||
*/
|
||||
LLDdelete(self->sequenceList);
|
||||
self->sequenceList = LLDcreate(sizeof(Sequence));
|
||||
self->level = 0;
|
||||
|
||||
/*
|
||||
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,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){
|
||||
StopExe(pServ->pExecutor,"all");
|
||||
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;
|
||||
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){
|
||||
StartRegMot(pMot,pCon,seq.target);
|
||||
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,usMugger)){
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user