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

- Adapted indenation to new agreed upon system

Browse Source 
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
2009-02-13 09:00:03 +00:00
parent a3dcad2bfa
commit 91d4af0541
405 changed files with 88101 additions and 88173 deletions
Download Patch File Download Diff File Expand all files Collapse all files

307
anticollider.c
View File

@@ -27,14 +27,16 @@ 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){
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);
pMot = FindMotFromDataStructure(myCollider->motorList, pData);
if (pMot != NULL) {
SetRegMotTarget(pMot, fTarget);
myCollider->isDirty = 1;
} else {
return 0;
@@ -42,33 +44,34 @@ static long ReplacementSetValue(void *pData, SConnection *pCon, float fTarget){
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){
static int ReplacementCheckStatus(void *pData, SConnection * pCon)
{
pMotReg pMot = NULL;
assert(myCollider != NULL);
if(myCollider->isDirty == 1){
if (myCollider->isDirty == 1) {
myCollider->isDirty = 0;
StartDevice(pServ->pExecutor,
"anticollider",
myCollider->pDes,
myCollider,
pCon,
77.77);
"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){
static long ColliderSetValue(void *pData, SConnection * pCon,
float fTarget)
{
pAntiCollider self = (pAntiCollider) pData;
int iRet;
pMotReg pMot = NULL;
@@ -78,79 +81,80 @@ static long ColliderSetValue(void *pData, SConnection *pCon, float fTarget){
Tcl_DString command;
/*
build command list
*/
if(self->colliderScript == NULL){
SCWrite(pCon,"ERROR: no collider script defined",eError);
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);
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;
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);
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);
SCWrite(pCon,pServ->pSics->pTcl->result,eError);
*/
SCSetInterrupt(pCon, eAbortOperation);
return 0;
}
/*
we are set
*/
we are set
*/
Tcl_DStringFree(&command);
return 1;
}
/*----------------------------------------------------------------------
The Collider CheckStatus function
-----------------------------------------------------------------------*/
static int ColliderCheckStatus(void *pData, SConnection *pCon){
static int ColliderCheckStatus(void *pData, SConnection * pCon)
{
int count = 0;
pAntiCollider self = (pAntiCollider) pData;
assert(self);
if(SCGetInterrupt(pCon) != eContinue){
if (SCGetInterrupt(pCon) != eContinue) {
return HWIdle;
}
count = CheckAllMotors(self->motorList,pCon);
if(count == 0){
count = CheckAllMotors(self->motorList, pCon);
if (count == 0) {
self->level++;
count = StartLevel(self->level,
self->sequenceList,
self->motorList,
pCon);
if(count == 0){
self->sequenceList, self->motorList, pCon);
if (count == 0) {
/*
no more levels. All done
*/
no more levels. All done
*/
return HWIdle;
} else {
return HWBusy;
@@ -159,27 +163,35 @@ static int ColliderCheckStatus(void *pData, SConnection *pCon){
return HWBusy;
}
}
/*----------------------------------------------------------------------
Most of these are dummies........
-----------------------------------------------------------------------*/
static int ColliderHalt(void *pData){
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){
static int ColliderLimits(void *self, float fVal, char *error, int iErren)
{
return 1;
}
/*--------------------------------------------------------------------*/
static float ColliderGetValue(void *self, SConnection *pCon){
static float ColliderGetValue(void *self, SConnection * pCon)
{
return 77.77;
}
/*--------------------------------------------------------------------*/
int StartLevel(int level, int sequenceList, int motorList, SConnection *pCon){
int StartLevel(int level, int sequenceList, int motorList,
SConnection * pCon)
{
Sequence seq;
pMotReg pMot = NULL;
int iRet, status;
@@ -187,99 +199,109 @@ int StartLevel(int level, int sequenceList, int motorList, SConnection *pCon){
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);
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++;
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);
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){
static void ListSequence(int sequenceList, SConnection * pCon)
{
Sequence seq;
int iRet;
char pBueffel[132];
SCWrite(pCon,"level motor target",eValue);
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);
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) {
static void *ColliderGetInterface(void *pData, int iID)
{
pAntiCollider self = NULL;
self = (pAntiCollider)pData;
self = (pAntiCollider) pData;
assert(self);
if(iID == DRIVEID){
return self->pDriv;
if (iID == DRIVEID) {
return self->pDriv;
}
return NULL;
}
return NULL;
}
/*----------------------------------------------------------------------*/
void KillCollider(void *pData){
pAntiCollider self = (pAntiCollider)pData;
if(self == NULL){
void KillCollider(void *pData)
{
pAntiCollider self = (pAntiCollider) pData;
if (self == NULL) {
return;
}
if(self->pDes != NULL){
if (self->pDes != NULL) {
DeleteDescriptor(self->pDes);
}
if(self->pDriv != NULL){
if (self->pDriv != NULL) {
free(self->pDriv);
}
if(self->colliderScript != NULL){
if (self->colliderScript != NULL) {
free(self->colliderScript);
}
if(self->motorList > 0){
if (self->motorList > 0) {
KillMotList(self->motorList);
}
if(self->sequenceList > 0){
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);
/*-----------------------------------------------------------------------*/
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));
memset(myCollider, 0, sizeof(AntiCollider));
myCollider->pDes = CreateDescriptor("AntiCollider");
myCollider->pDriv = CreateDrivableInterface();
if(!myCollider->pDes || !myCollider->pDriv){
if (!myCollider->pDes || !myCollider->pDriv) {
KillCollider(myCollider);
SCWrite(pCon,"ERROR: out of memory when generating AntiCollider",eError);
SCWrite(pCon, "ERROR: out of memory when generating AntiCollider",
eError);
return 0;
}
@@ -293,91 +315,90 @@ int AntiColliderFactory(SConnection *pCon, SicsInterp *pSics,
myCollider->motorList = LLDcreate(sizeof(void *));
myCollider->sequenceList = LLDcreate(sizeof(Sequence));
AddCommand(pSics,"anticollision",AntiColliderAction,
KillCollider,
myCollider);
AddCommand(pSics, "anticollision", AntiColliderAction,
KillCollider, myCollider);
return 1;
}
/*------------------------------------------------------------------------*/
int AntiColliderAction(SConnection *pCon, SicsInterp *pSics,
void *pData,
int argc, char *argv[]){
pAntiCollider self = (pAntiCollider)pData;
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;
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);
} 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);
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)){
if (strcmp(argv[1], "script") == 0) {
if (!SCMatchRights(pCon, usMugger)) {
return 0;
}
if(self->colliderScript != NULL){
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)){
} 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);
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);
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);
SCWrite(pCon, "ERROR: out of memory registering motor", eError);
return 0;
}
} else if(strcmp(argv[1],"add") == 0){
if(argc < 5){
} else if (strcmp(argv[1], "add") == 0) {
if (argc < 5) {
SCWrite(pCon,
"ERROR: InsUfficient number of arguments to anticollicion add",
eError);
"ERROR: InsUfficient number of arguments to anticollicion add",
eError);
return 0;
}
seq.level = atoi(argv[2]);
strncpy(seq.pMotor,argv[3],79);
strncpy(seq.pMotor, argv[3], 79);
seq.target = atof(argv[4]);
LLDnodeAppendFrom(self->sequenceList,&seq);
LLDnodeAppendFrom(self->sequenceList, &seq);
SCSendOK(pCon);
return 1;
}
SCWrite(pCon,"ERROR: anticollider command not understood",eError);
}
SCWrite(pCon, "ERROR: anticollider command not understood", eError);
return 0;
}