- First working version of the TRICS collision protection module

motreg.c

@@ -0,0 +1,125 @@
+/*-------------------------------------------------------------------------
+			R e g M o t
+
+  This is  a helper module for the Anti Collider. It handles all the
+  stuff necessary for dealing with a single motor. For more
+  information see the file anticollider.tex.
+
+  copyright: see file copyright
+ 
+  Mark Koennecke, August 2002
+-----------------------------------------------------------------------*/
+#include <stdlib.h>
+#include <assert.h>
+#include "fortify.h"
+#include "motreg.h"
+
+/*--------------------------------------------------------------------*/
+pMotReg RegisterMotor(char *name, SicsInterp *pSics,
+		      long (*NewSetValue)(void *pData, SConnection *pCon,
+					  float farget),
+		      int (*NewCheckStatus)(void *pData,SConnection *pCon) ){
+  CommandList *pCom = NULL;
+  pIDrivable pDriv = NULL;
+  pMotReg pNew = NULL;
+
+  /*
+    find motor data structures
+  */
+  pCom = FindCommand(pSics,name);
+  if(pCom == NULL){
+    return NULL;
+  }
+  pDriv = GetDrivableInterface(pCom->pData);
+  if(pDriv == NULL){
+    return NULL;
+  }
+
+  /*
+    everything seems OK. Allocate data structure and initialize 
+  */
+  pNew = (pMotReg)malloc(sizeof(MotReg));
+  if(pNew == NULL){
+    return NULL;
+  }
+  memset(pNew,0,sizeof(MotReg));
+
+  pNew->motorData = pCom->pData;
+  pNew->motorName = strdup(name);
+  pNew->originalSetValue = pDriv->SetValue;
+  pNew->originalCheckStatus = pDriv->CheckStatus;
+  pNew->iActive = 0;
+
+  pDriv->SetValue = NewSetValue;
+  pDriv->CheckStatus = NewCheckStatus;
+
+  return pNew;
+}
+/*---------------------------------------------------------------------*/
+void KillRegMot(void *pData){
+  pMotReg self = (pMotReg)pData;
+
+  if(self == NULL){
+    return;
+  }
+  if(self->motorName != NULL){
+    free(self->motorName);
+  }
+  free(self);
+}
+/*------------------------------------------------------------------------*/
+void SetRegMotTarget(pMotReg self, float fValue){
+  assert(self);
+
+  self->targetPosition = fValue;
+  self->iActive = 1;
+}
+/*------------------------------------------------------------------------*/
+void CreateTargetString(pMotReg self, char pBueffel[80]) {
+  assert(self);
+
+  if(strlen(self->motorName) + 20 < 80) {
+    sprintf(pBueffel," %s %12.4f ", self->motorName, self->targetPosition);
+  }
+}
+/*-----------------------------------------------------------------------*/
+int RegMotMatch(pMotReg self, char *name){
+  assert(self);
+  if(strcmp(self->motorName, name) == 0) {
+    return 1;
+  }
+  return 0;
+}
+/*----------------------------------------------------------------------*/
+int StartRegMot(pMotReg self, SConnection *pCon, float fValue){
+  int ret;
+  long (*oldSet)(void *pmotorData, SConnection *pCon, float fValue);
+  pIDrivable pDriv = NULL;
+  
+
+  assert(self);
+  pDriv = GetDrivableInterface(self->motorData);
+  assert(pDriv);
+  oldSet = pDriv->SetValue;
+  pDriv->SetValue = self->originalSetValue;
+  ret = StartDevice(pServ->pExecutor, self->motorName,
+		    FindDescriptor(self->motorData),
+		    self->motorData,
+		    pCon,
+		    fValue);
+
+  pDriv->SetValue = oldSet;
+  self->iActive = 1;
+  return ret;
+}
+/*----------------------------------------------------------------------*/
+int CheckRegMot(pMotReg self, SConnection *pCon){
+  int stat;
+
+  assert(self);
+  stat = self->originalCheckStatus(self->motorData,pCon);
+  if(stat != HWBusy){
+    self->iActive = 0;
+  }
+  return stat;
+}