diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 8e7c1a67..c3f6f47c 100644 --- a/site_ansto/motor_dmc2280.c +++ b/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) { - int motSpeed; - motSpeed = abs((int)(speed * self->stepsPerX + 0.5)); - return motSpeed; +static int motSpeed(pDMC2280Driv self, float axisSpeed) { + int speed; + speed = abs((int)(axisSpeed * self->stepsPerX + 0.5)); + 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) { - int motAccel; - motAccel = abs((int)(accel * self->stepsPerX + 0.5)); - return motAccel; +static int motAccel(pDMC2280Driv self, float axisAccel) { + int accel; + accel = abs((int)(axisAccel * self->stepsPerX + 0.5)); + 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) { - int motDecel; - motDecel = abs((int)(decel * self->stepsPerX + 0.5)); - return motDecel; +static int motDecel(pDMC2280Driv self, float axisDecel) { + int decel; + decel = abs((int)(axisDecel * self->stepsPerX + 0.5)); + 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]; - bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; + char switchesAscii[10]; +#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 {