51a60375d6
SKIPPED: psi/dornier2.c psi/ecbdriv.c psi/el734hp.c psi/libpsi.a psi/make_gen psi/makefile_linux psi/pimotor.c psi/pipiezo.c psi/sinqhttp.c psi/tcpdornier.c psi/velodornier.c
123 lines
3.0 KiB
C
123 lines
3.0 KiB
C
/*-----------------------------------------------------------------------
|
|
A couple of utility functions for handling a list of MotReg
|
|
structures . This is a helper module for the anticollider collision
|
|
control system. See anticollider.tex for more details.
|
|
|
|
copyright: see file copyright
|
|
|
|
Mark Koennecke, August 2002
|
|
-------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include "fortify.h"
|
|
#include "lld.h"
|
|
#include "motreglist.h"
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
int MakeMotList(){
|
|
return LLDcreate(sizeof(pMotReg));
|
|
}
|
|
/*----------------------------------------------------------------------*/
|
|
pMotReg FindMotEntry(int iList, char *name){
|
|
int iRet;
|
|
pMotReg pMot = NULL;
|
|
|
|
iRet = LLDnodePtr2First(iList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(iList,&pMot);
|
|
if(pMot != NULL){
|
|
if(RegMotMatch(pMot,name)){
|
|
return pMot;
|
|
}
|
|
}
|
|
iRet = LLDnodePtr2Next(iList);
|
|
}
|
|
return NULL;
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
pMotReg FindMotFromDataStructure(int iList, void *pData){
|
|
int iRet;
|
|
pMotReg pMot = NULL;
|
|
|
|
iRet = LLDnodePtr2First(iList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(iList,&pMot);
|
|
if(pMot != NULL){
|
|
if(pMot->motorData == pData){
|
|
return pMot;
|
|
}
|
|
}
|
|
iRet = LLDnodePtr2Next(iList);
|
|
}
|
|
return NULL;
|
|
}
|
|
/*----------------------------------------------------------------------*/
|
|
int CheckAllMotors(int iList, SConnection *pCon){
|
|
int iRet, count = 0;
|
|
pMotReg pMot = NULL;
|
|
|
|
iRet = LLDnodePtr2First(iList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(iList,&pMot);
|
|
if(pMot != NULL){
|
|
CheckRegMot(pMot,pCon);
|
|
if(pMot->iActive){
|
|
count++;
|
|
}
|
|
}
|
|
iRet = LLDnodePtr2Next(iList);
|
|
}
|
|
return count;
|
|
}
|
|
/*----------------------------------------------------------------------*/
|
|
void StopAllMotors(int iList){
|
|
int iRet, count = 0;
|
|
pMotReg pMot = NULL;
|
|
pIDrivable pDriv;
|
|
|
|
iRet = LLDnodePtr2First(iList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(iList,&pMot);
|
|
if(pMot != NULL){
|
|
if(pMot->iActive){
|
|
pDriv = (pIDrivable)GetDrivableInterface(pMot->motorData);
|
|
if(pDriv){
|
|
pDriv->Halt(pMot->motorData);
|
|
}
|
|
pMot->iActive = 0;
|
|
}
|
|
}
|
|
iRet = LLDnodePtr2Next(iList);
|
|
}
|
|
}
|
|
/*----------------------------------------------------------------------*/
|
|
void DeactivateAllMotors(int iList){
|
|
int iRet, count = 0;
|
|
pMotReg pMot = NULL;
|
|
pIDrivable pDriv;
|
|
|
|
iRet = LLDnodePtr2First(iList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(iList,&pMot);
|
|
if(pMot != NULL){
|
|
pMot->iActive = 0;
|
|
}
|
|
iRet = LLDnodePtr2Next(iList);
|
|
}
|
|
}
|
|
/*----------------------------------------------------------------------*/
|
|
void KillMotList(int iList){
|
|
int iRet;
|
|
pMotReg pMot = NULL;
|
|
|
|
iRet = LLDnodePtr2First(iList);
|
|
while(iRet != 0){
|
|
LLDnodeDataTo(iList,&pMot);
|
|
if(pMot != NULL){
|
|
KillRegMot(pMot);
|
|
}
|
|
iRet = LLDnodePtr2Next(iList);
|
|
}
|
|
LLDdelete(iList);
|
|
}
|