PSI sics-cvs-psi_pre-ansto

motreglist.c

@@ -0,0 +1,107 @@
+/*-----------------------------------------------------------------------
+ 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 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);
+}