406 lines
11 KiB
C
406 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,SCGetRunLevel(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);
|
|
SCPrintf(pCon,eError, "%s returned %s", Tcl_DStringValue(&command), Tcl_GetStringResult(pServ->pSics->pTcl));
|
|
/*
|
|
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 {
|
|
snprintf(pBueffel,131,
|
|
"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);
|
|
snprintf(pBueffel,131, "%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) {
|
|
snprintf(pBueffel,255, "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]);
|
|
strlcpy(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;
|
|
}
|