linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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
This commit is contained in:
Ferdi Franceschini
2006-07-13 14:46:41 +10:00
committed by Douglas Clowes
parent cb5b6e704e
commit d72d0b9905
1 changed files with 131 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

147
site_ansto/motor_dmc2280.c
View File

@@ -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) {
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 {