Now using tolerance to compensate for backlash instead of the CLSLOOP code.

Added convenience function to get acceleration and speed from the controller.

r1025 | ffr | 2006-07-13 14:46:41 +1000 (Thu, 13 Jul 2006) | 3 lines

site_ansto/motor_dmc2280.c

@@ -45,6 +45,12 @@ void KillRS232(/*@only@*/ void *pData);
 /* The pointer to the Tcl interpreter is owned by SICS and should not be modified */
 /*@observer@*//*@dependent@*/  Tcl_Interp *InterpGetTcl(SicsInterp *pSics);
 /*@+incondefs@*/
+
+/** \brief Used to ensure that the getDMCSetting function is called 
+ * with valid values.
+ * \see getDMCSetting
+ */
+enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration};
 /*-----------------------------------------------------------------------
   The motor driver structure. Please note that the first set of fields has 
   be identical with the fields of AbstractModriv in  ../modriv.h 
@@ -91,6 +97,7 @@ typedef struct __MoDriv {
   int abs_endcoder; /**<  Flag = 1 if there is an abs enc */
   int absEncHome; /**<  Home position in counts for abs enc */
   int cntsPerX; /**<  absolute encoder counts per physical unit */
+  int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */
 } DMC2280Driv, *pDMC2280Driv;
 /*------------------- error codes ----------------------------------*/
 #define BADADR     -1 // NOT SET: Unknown host/port?
@@ -136,39 +143,44 @@ typedef struct __MoDriv {
 #define DECEL "decel"
 #define MAXDECEL "maxDecel"
 
+static int DMC2280SetPar(void *pData, SConnection *pCon,
+    char *name, float newValue);
 static int DMC2280Receive(pDMC2280Driv self, /*@out@*/ char *reply);
 
-/** \brief Convert motor speed from physical units to steps/sec
+/** \brief Convert axis speed in physical units to
+ * motor speed in steps/sec.
  * \param self (r) provides access to the motor's data structure
  * \param speed in physical units, eg mm/sec degrees/sec 
  * \return the speed in motor steps/sec
  */
-static int motSpeed(pDMC2280Driv self, float speed) {
+static int motSpeed(pDMC2280Driv self, float axisSpeed) {
-  int motSpeed; 
+  int speed; 
-  motSpeed = abs((int)(speed * self->stepsPerX + 0.5));
+  speed = abs((int)(axisSpeed * self->stepsPerX + 0.5));
-  return motSpeed;
+  return speed;
 }
 
-/** \brief Convert motor acceleration from physical units to steps/sec^2
+/** \brief Convert axis acceleration in physical units to
+ * to motor speed in steps/sec^2
  * \param self (r) provides access to the motor's data structure
  * \param acceleration in physical units, eg mm/sec^2 degrees/sec^2 
  * \return the acceleration in motor steps/sec^2
  */
-static int motAccel(pDMC2280Driv self, float accel) {
+static int motAccel(pDMC2280Driv self, float axisAccel) {
-  int motAccel;
+  int accel;
-  motAccel = abs((int)(accel * self->stepsPerX + 0.5));
+  accel = abs((int)(axisAccel * self->stepsPerX + 0.5));
-  return motAccel;
+  return accel;
 }
 
-/** \brief Convert motor deceleration from physical units to steps/sec^2
+/** \brief Convert axis deceleration in physical units to
+ * motor deceleration in steps/sec^2
  * \param self (r) provides access to the motor's data structure
  * \param deceleration in physical units, eg mm/sec^2 degrees/sec^2 
  * \return the deceleration in motor steps/sec^2
  */
-static int motDecel(pDMC2280Driv self, float decel) {
+static int motDecel(pDMC2280Driv self, float axisDecel) {
-  int motDecel;
+  int decel;
-  motDecel = abs((int)(decel * self->stepsPerX + 0.5));
+  decel = abs((int)(axisDecel * self->stepsPerX + 0.5));
-  return motDecel;
+  return decel;
 }
 
 /** \brief Reads a single character from the DMC2280 controller.
@@ -296,6 +308,71 @@ static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) {
   return FAILURE;
 }
 
+/**\brief Convenience function for getting speed, acceleration
+ * or deceleration
+ * 
+ * \param *pData provides access to a motor's data
+ * \param cmdIndex selects value to request from controller.
+ * \return Either speed acceleration or deceleration as requested.
+ * \see dmcsetting getMotSpeed getMotAccel getMotDecel
+ */
+static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){
+  pDMC2280Driv self = NULL;
+  char cmd[CMDLEN], reply[256];
+  int dmcSetting;
+
+  self = (pDMC2280Driv)pData;
+  switch (cmdIndex) {
+    case dmcspeed:
+      snprintf(cmd, CMDLEN, "MG _SP%c", self->axisLabel);
+      break;
+    case dmcacceleration:
+      snprintf(cmd, CMDLEN, "MG _AC%c", self->axisLabel);
+      break;
+    case dmcdeceleration:
+      snprintf(cmd, CMDLEN, "MG _DC%c", self->axisLabel);
+  }
+  if (FAILURE == DMC2280Send(self, cmd))
+    return HWFault;
+  if (FAILURE == DMC2280Receive(self, reply))
+    return HWFault;
+  dmcSetting =atoi(reply);
+  return dmcSetting;
+}
+
+/** \brief Call this to make sure that the speed, 
+ * acceleration and deceleration are set to the correct value.\n
+ * XXX Unused: This will interfere with progs running on the 
+ * controller like #LIMSWI which sets maximum deceleration when a
+ * limit switch is hit.
+ */
+/*@unused@*/static void ckSpeedAccelDecel(pDMC2280Driv self) {
+  int motSetting;
+  char cmd[CMDLEN];
+
+  motSetting = getDMCSetting(self, dmcspeed);
+  /* Reset speed if it has been changed externally */
+  if (motSetting != motSpeed(self, self->speed)) {
+    snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self,self->speed));
+    DMC2280Send(self, cmd);
+  }
+
+    /* Reset acceleration if it has been changed externally */
+    motSetting = getDMCSetting(self, dmcacceleration);
+    if (motSetting != motAccel(self, self->accel)) {
+    snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self,self->accel));
+    DMC2280Send(self, cmd);
+    }
+
+    /* Reset deceleration if it has been changed externally */
+    motSetting = getDMCSetting(self, dmcdeceleration);
+    if (motSetting != motDecel(self, self->decel)) {
+    snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self,self->decel));
+    DMC2280Send(self, cmd);
+    }
+}
+
+
 /** \brief Reads motor position, implements the GetPosition
  * method in the MotorDriver interface.
  *
@@ -368,9 +445,11 @@ static int DMC2280Run(void *pData,float fValue){
     snprintf(cmd, CMDLEN, "DP%c=(_TP%c - %d)*(%d/%d) + %d",axis,axis,absEncHome,stepsPerX,cntsPerX,motorHome);
     if (FAILURE == DMC2280Send(self, cmd)) 
       return HWFault;
+#ifdef BACKLASHFIX
     snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis, (int) (target * cntsPerX + absEncHome + 0.5));
     if (FAILURE == DMC2280Send(self, cmd))
       return HWFault;
+#endif
   }
 
   if (FAILURE == DMC2280Send(self, absPosCmd))
@@ -398,13 +477,18 @@ static int DMC2280Status(void *pData){
   pDMC2280Driv self = NULL;
   char cmd[CMDLEN];
   int switches;
-  char switchesAscii[10], reply[256];
+  char switchesAscii[10];
-  bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
+#ifdef BACKLASHFIX
+  char reply[256];
   int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus;
   int SHOULD_FIXPOS=1, should_fixpos;
+#endif
+  bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
 
   self = (pDMC2280Driv)pData;
   assert(self != NULL);
+  /* Make sure that speed, accel and decel are set correctly */
+  /* ckSpeedAccelDecel(self); */
   /* Get status of switches
    * see TS (Tell Switches) in Galil manc2xx.pdf */
   snprintf(cmd, CMDLEN, "TS%c", self->axisLabel);
@@ -437,6 +521,7 @@ static int DMC2280Status(void *pData){
       self->errorCode = ERRORLIM;
       return HWFault;
     }
+#ifdef BACKLASHFIX
     if (self->abs_endcoder == 1) {
       /* Make sure that the servo loop is closed by checking if
        * the CLSLOOP thread is running on the controller.*/
@@ -464,8 +549,13 @@ static int DMC2280Status(void *pData){
         return HWBusy;
       }
     }
+#endif
     if (self->noPowerSave == _SAVEPOWER) {
-      snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
+      if (self->motOffDelay > 0 ) {
+        snprintf(cmd, CMDLEN, "AT %d; MO%c", self->motOffDelay, self->axisLabel);
+      } else {
+        snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
+      }
       DMC2280Send(self, cmd);
     }
     return HWIdle;
@@ -594,11 +684,20 @@ static int DMC2280Halt(void *pData){
 
   return 1;
 }
+
 /** \brief Fetches the value of the named parameter,
  * implements the GetDriverPar method in the MotorDriver interface.
  *
  * Note: The GetDriverPar method in the MotorDriver interface only
  * allows float values to be returned.
+ *
+ * If the speed, acceleration or deceleration is requested then 
+ * this compares the setting on the controller to the required setting,
+ * if they don't match then the controller is set to the required value.
+ *
+ * Note: Doesn't warn if the speed, acceleration, or deceleration set on
+ * the controller differ from the required settings.
+ * 
  * \param *pData (r) provides access to a motor's data
  * \param *name (r) the name of the parameter to fetch.
  * \param *fValue (w) the parameter's value.
@@ -867,6 +966,12 @@ static void KillDMC2280(/*@only@*/void *pData){
  * \param *motor (r) motor name
  * \param *params (r) configuration parameter array.
  * \return a reference to Motordriver structure
+ *
+ * NOTES:\n
+ * -Adding parameters
+ *  - Add a field for the parameter to the DMC2280Driv struct
+ *  - Get the parameter from the parameter array, see PARAMETERS: below
+ *  - If the parameter requires an abs enc then add it after ABSENC: 
  */
 /*@only@*//*@null@*/ MotorDriver *CreateDMC2280(/*@observer@*/SConnection *pCon, /*@observer@*/char *motor, /*@observer@*/char *params){
   pDMC2280Driv pNew = NULL;
@@ -940,6 +1045,8 @@ static void KillDMC2280(/*@only@*/void *pData){
   pNew->SetDriverPar = DMC2280SetPar;
   pNew->ListDriverPar = DMC2280List;
   pNew->KillPrivate = KillDMC2280;
+
+  /* PARAMETERS: Fetch parameter values */
   if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) {
     KillDMC2280(pNew);
     return NULL;
@@ -981,6 +1088,12 @@ static void KillDMC2280(/*@only@*/void *pData){
     pNew->noPowerSave=_SAVEPOWER;
   else
     sscanf(pPtr,"%d",&(pNew->noPowerSave));
+  if ((pPtr=getParam(pCon, interp, params,"motOffDelay",_OPTIONAL)) == NULL)
+    pNew->motOffDelay=0;
+  else
+    sscanf(pPtr,"%d",&(pNew->motOffDelay));
+
+  /* ABSENC: If the parameter requires an abs enc add it to the else block */
   if ((pPtr=getParam(pCon, interp, params,"absEnc",_OPTIONAL)) == NULL)
     pNew->abs_endcoder=0;
   else {