linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
75711dde2ac76a4dbc4c291b819b4a058f6f4696
sics/site_ansto/motor_dmc2280.c
Douglas Clowes b043246eac Parameterise the motion checks for detecting blocked motors 
r1544 | dcl | 2007-02-22 16:42:12 +1100 (Thu, 22 Feb 2007) | 2 lines
2012-11-15 13:01:38 +11:00

1693 lines
52 KiB
C
Raw Blame History

/** \file motor_dmc2280.c
* \brief Driver for Galil DMC2280 motor controller.
*
* Implements a SICS motor object with a MotorDriver interface.
*
* Copyright: see file Copyright.txt
*
* Ferdi Franceschini November 2005
*
* TODO
* - check for motors enabled on plc
* - Check error bit, see Dan's email
*/
#include <stdlib.h>
/* ISO C Standard: 7.16 Boolean type and values <stdbool.h> */
#include <stdbool.h>
#include <math.h>
#include <float.h>
#include <assert.h>
#include <string.h>
#include <stdarg.h>
#include <sys/time.h>
#include <fortify.h>
#include <sics.h>
#include <rs232controller.h>
#include <nwatch.h>
#include <modriv.h>
#include <motor.h>
#include <dynstring.h>
#include <time.h>
#include "anstoutil.h"
/*
#include "splint/splint_fortify.h"
#include "splint/splint_tclDecls.h"
#include "splint/splint_dynstring.h"
#include "splint/splint_SCinter.h"
*/
/*@-incondefs@*/
/* XXX Should this also free pData */
#if 0
int readRS232(prs232 self, /*@out@*/void *data, /*@out@*/int *dataLen);
int readRS232TillTerm(prs232 self, /*@out@*/void *data, int *datalen);
#endif
void KillRS232(/*@only@*/ void *pData);
/*@+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
------------------------------------------------------------------------*/
typedef struct __MoDriv {
/* general motor driver interface
fields. _REQUIRED!
*/
float fUpper; /**< hard upper limit */
float fLower; /**< hard lower limit */
char *name; /**< motor name */
int (*GetPosition)(void *self, float *fPos);
int (*RunTo)(void *self, float fNewVal);
int (*GetStatus)(void *self);
void (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
int (*TryAndFixIt)(void *self,int iError, float fNew);
int (*Halt)(void *self);
int (*GetDriverPar)(void *self, char *name,
float *value);
int (*SetDriverPar)(void *self,SConnection *pCon,
char *name, float newValue);
void (*ListDriverPar)(void *self, char *motorName,
SConnection *pCon);
void (*KillPrivate)(/*@only@*/void *self);
/* DMC-2280 specific fields */
prs232 controller;
pMotor pMot; /**< Points to logical motor object */
int errorCode;
char *errorMsg; /**< Points to memory for error messages */
char units[256]; /**< physical units for axis */
float speed; /**< physical units per second */
float maxSpeed; /**< physical units per second */
float accel; /**< physical units per second^2 */
float maxAccel; /**< physical units per second^2 */
float decel; /**< physical units per second^2 */
float maxDecel; /**< physical units per second^2 */
char axisLabel;
char lastCmd[1024];
char dmc2280Error[1024];
float home; /**< home position for axis, default=0 */
int motorHome; /**< motor home position in steps */
int noPowerSave; /**< Flag = 1 to leave motors on after a move */
int stepsPerX; /**< steps per physical unit */
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 */
float lastPosition; /**< Position at last position check */
float lastSteps;
float lastCounts;
struct timeval time_lastPos_set; /**< Time when lastPosition was set */
float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */
float blockage_thresh; /**< motion threshold for blockage checking */
float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */
int blockage_fail; /**< flag =1 if we should fail the motor */
int has_airpads; /**< Flag = 1 if there is are airpads for this motor */
float fTarget;
int settle;
struct timeval time_settle_done;
int airpad_state;
int airpad_counter;
pNWTimer airpad_timer;
pNWTimer motor_timer;
int debug;
} DMC2280Driv, *pDMC2280Driv;
#define AIRPADS_DOWN 0
#define AIRPADS_RAISE 1
#define AIRPADS_UP 2
#define AIRPADS_LOWER 3
/*------------------- error codes ----------------------------------*/
#define BADADR -1 // NOT SET: Unknown host/port?
#define BADBSY -2
#define BADCMD -3
#define BADPAR -4 // NOT SET: Does SICS already check parameter types?
#define BADUNKNOWN -5
#define BADSTP -6 // NOT SET
#define BADEMERG -7 // NOT SET: ESTOP
#define RVRSLIM -8
#define FWDLIM -9
#define POSFAULT -11 // NOT SET
#define BADCUSHION -12 // NOT SET
#define ERRORLIM -13
#define IMPOSSIBLE_LIM_SW -14
#define BGFAIL -15 // NOT SET
#define BLOCKED -16
/*--------------------------------------------------------------------*/
#define STATUSMOVING 128 /* Motor is moving */
#define STATUSERRORLIMIT 64 /* Number of errorss exceed limit */
#define STATUSOFF 32 /* Motor off */
#define STATUSFWDLIMIT 8 /* Forward limit switch active */
#define STATUSRVRSLIMIT 4 /* Reverse limit switch active */
#define INIT_STR_SIZE 256
#define STR_RESIZE_LENGTH 256
#define CMDLEN 1024
#define BUFFLEN 512
#define _SAVEPOWER 0
#define SETPOS "setpos"
#define HOME "home"
#define HARDLOWERLIM "hardlowerlim"
#define HARDUPPERLIM "hardupperlim"
#define UNITS "units"
#define SPEED "speed"
#define MAXSPEED "maxspeed"
#define ACCEL "accel"
#define MAXACCEL "maxaccel"
#define DECEL "decel"
#define MAXDECEL "maxdecel"
#define MOTOFFDELAY "motoffdelay"
#define AIRPADS "airpads"
#define SETTLE "settle"
#define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval"
static int DMC2280Halt(void *pData);
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue);
static int DMC2280Receive(pDMC2280Driv self, /*@out@*/ char *reply);
/** \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 axisSpeed) {
int speed;
speed = abs((int)(axisSpeed * self->stepsPerX + 0.5));
return speed;
}
/** \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 axisAccel) {
int accel;
accel = abs((int)(axisAccel * self->stepsPerX + 0.5));
return accel;
}
/** \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 axisDecel) {
int decel;
decel = abs((int)(axisDecel * self->stepsPerX + 0.5));
return decel;
}
/** \brief Reads a single character from the DMC2280 controller.
*
* On failure it sets the errorCode field in the motor's data structure
* \param self (rw) provides access to the motor's data structure
* \param *reply (w) the character read from the controller
* \return
* - SUCCESS
* - FAILURE
* \see SUCCESS FAILURE
*/
static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) {
int i, status, retries=1, dataLen=1;
reply[0] = '\0';
for (i=0; i<retries; i++) {
status=readRS232(self->controller, reply, &dataLen);
switch (status) {
case 1:
return SUCCESS;
case TIMEOUT:
self->errorCode = status;
continue;
default:
self->errorCode = status;
return FAILURE;
}
}
return FAILURE;
}
/** \brief Sends a DMC2280 command to the motor controller.
*
* If the command fails it displays the DMC2280 error message to the client
* and writes it to the log file, also sets errorCode field in motor's
* data structure.
*
* \param self (rw) provides access to the motor's data structure
* \param *command (r) DMC2280 command
* \return
* - SUCCESS
* - FAILURE
* \see SUCCESS FAILURE
*/
/* First character returned by controller is
'?' for an invalid command or
':' or space for a valid command */
static int DMC2280Send(pDMC2280Driv self, char *command) {
char cmdValid, reply[256];
char *GetEMsg = "TC 1";
int status;
if ((self->lastCmd) != command) {
/*@-mayaliasunique@ this won't happen unless they overlap */
strncpy(self->lastCmd, command, CMDLEN);
/*@+mayaliasunique@*/
}
if (self->debug)
SICSLogWrite(command, eStatus);
/*@+matchanyintegral@ let size_t from strlen match int */
status = writeRS232(self->controller, command, strlen(command));
/*@-matchanyintegral@*/
if (status != 1) {
self->errorCode = status;
return FAILURE;
}
if (FAILURE == (status = DMC2280ReadChar(self, &cmdValid))) {
return FAILURE;
} else {
switch (cmdValid) {
case ':':
case ' ':
return SUCCESS;
case '?':
/*@+matchanyintegral@ let size_t from strlen match int */
status = writeRS232(self->controller, GetEMsg, strlen(GetEMsg));
/*@-matchanyintegral@*/
if (status != 1) {
self->errorCode = status;
return FAILURE;
}
if (FAILURE == DMC2280Receive(self, reply))
return HWFault;
strncpy(self->dmc2280Error, reply, CMDLEN);
SICSLogWrite(reply, eError);
self->errorCode = BADCMD;
return FAILURE;
default:
self->errorCode = BADUNKNOWN;
return FAILURE;
}
}
}
/** \brief Gets output from the DMC2280, the abstract motor code should
* handle retries if the request times out.
*
* Note: The timeout for readRS232TillTerm is set by DMC2280Connect
* \param self (rw) provides access to the motor's data structure
* \param *reply (w) the data from the DMC2280.
* \return
* - SUCCESS
* - FAILURE
* \see SUCCESS FAILURE
*/
static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) {
int i, status, retries=1, dataLen=255;
reply[0] = '\0';
for (i=0; i<retries; i++) {
status=readRS232TillTerm(self->controller, reply, &dataLen);
switch (status) {
case 1:
if (self->debug)
SICSLogWrite(reply, eStatus);
return dataLen;
case TIMEOUT:
self->errorCode = status;
continue;
/* TODO case INCOMPLETE: */
default:
self->errorCode = status;
return FAILURE;
}
}
return FAILURE;
}
/** \brief Record the given posn and timestamp it.
*
* \param *pData provides access to a motor's data
* \param posn, the axis position which you want to remember.
* */
static void set_lastMotion(pDMC2280Driv self, float steps, float counts) {
assert(self != NULL);
self->lastSteps = steps;
self->lastCounts = counts;
gettimeofday(&(self->time_lastPos_set), NULL);
}
/** \brief Record the given posn and timestamp it.
*
* \param *pData provides access to a motor's data
* \param posn, the axis position which you want to remember.
* */
static void set_lastPos(pDMC2280Driv self, float posn) {
assert(self != NULL);
self->lastPosition = posn;
gettimeofday(&(self->time_lastPos_set), NULL);
}
/**\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 motion.
*
* \param *steps motor steps
* \param *counts encoder counts
* \return
* SUCCESS
* FAILURE
*/
static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
char reply[1024];
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return FAILURE;
if (FAILURE == DMC2280Receive(self, reply))
return FAILURE;
if (2 != sscanf(reply, "%f %f", steps, counts))
return FAILURE;
return SUCCESS;
}
/** \brief Reads absolute encoder.
*
* \param *pos is the absolute encoder reading on SUCCESS.
* \return
* SUCCESS
* FAILURE
*/
static int readAbsEnc(pDMC2280Driv self, float *pos) {
char reply[1024];
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return FAILURE;
if (FAILURE == DMC2280Receive(self, reply))
return FAILURE;
*pos = (float) atoi(reply);
return SUCCESS;
}
/** \brief Reads motor position, implements the GetPosition
* method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \param *fPos contains the motor position in physical units after a call.
* \return
* - OKOK request succeeded
* - HWFault request failed
* */
static int DMC2280GetPos(void *pData, float *fPos){
pDMC2280Driv self = NULL;
char reply[1024];
char cmd[CMDLEN];
float absEncPos, motorPos;
reply[0]='\0';
self = (pDMC2280Driv)pData;
assert(self != NULL);
if (1 == self->abs_endcoder) {
if (readAbsEnc(self, &absEncPos) == FAILURE)
return HWFault;
*fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home;
} else {
snprintf(cmd, ERRLEN, "TD%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
if (FAILURE == DMC2280Receive(self, reply))
return HWFault;
motorPos =(float)atof(reply);
*fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home;
}
return OKOK;
}
static int DMC2280Run(void *pData,float fValue);
static int airpad_callback(void* context, int mode) {
pDMC2280Driv self = (pDMC2280Driv) context;
char reply[1024];
float fReply;
self->airpad_timer = NULL;
if (FAILURE == DMC2280Send(self, "MG APDONE")) {
self->errorCode = BADCUSHION;
self->airpad_state = AIRPADS_DOWN;
return 0;
}
if (FAILURE == DMC2280Receive(self, reply)) {
self->errorCode = BADCUSHION;
self->airpad_state = AIRPADS_DOWN;
return 0;
}
fReply = (float) atof(reply);
if (self->airpad_state == AIRPADS_RAISE && fReply > 0) {
int iRet;
self->airpad_state = AIRPADS_UP;
iRet = DMC2280Run(self, self->fTarget);
if (iRet != OKOK)
self->errorCode = BADCUSHION;
return 0;
}
if (self->airpad_state == AIRPADS_LOWER && fReply == 0) {
self->airpad_state = AIRPADS_DOWN;
return 0;
}
if (self->airpad_counter <= 0) {
self->errorCode = BADCUSHION;
self->airpad_state = AIRPADS_DOWN;
return 0;
}
--self->airpad_counter;
NetWatchRegisterTimer(&self->airpad_timer, 1000,
airpad_callback, self);
return 0;
}
static int motor_callback(void* context, int mode) {
pDMC2280Driv self = (pDMC2280Driv) context;
char cmd[CMDLEN];
self->motor_timer = NULL;
if (self->has_airpads) {
if (FAILURE == DMC2280Send(self, "FTUBE=0")) {
self->errorCode = BADCUSHION;
return 0;
}
self->airpad_state = AIRPADS_LOWER;
self->airpad_counter = 10;
NetWatchRegisterTimer(&self->airpad_timer, 1000,
airpad_callback, self);
return 0;
}
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
DMC2280Send(self, cmd);
return 0;
}
static int DMC2280RunAir(void *pData, float fValue) {
pDMC2280Driv self = (pDMC2280Driv)pData;
self->fTarget = fValue;
if (self->airpad_timer)
NetWatchRemoveTimer(self->airpad_timer);
self->airpad_timer = NULL;
if (FAILURE == DMC2280Send(self, "FTUBE=1"))
return HWFault;
self->airpad_state = AIRPADS_RAISE;
self->airpad_counter = 10;
NetWatchRegisterTimer(&self->airpad_timer, 1000,
airpad_callback, self);
return OKOK;
}
/** \brief DMC2280 implementation of the RunTo
* method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \param fValue target position in physical units, software zeros
* have already been applied.
* \return
* - OKOK request succeeded
* - HWFault request failed
* */
static int DMC2280Run(void *pData,float fValue){
pDMC2280Driv self = NULL;
char axis;
char cmd[CMDLEN], SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN];
int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn;
float target;
self = (pDMC2280Driv)pData;
assert(self != NULL);
if (self->motor_timer)
NetWatchRemoveTimer(self->motor_timer);
self->motor_timer = NULL;
if (self->has_airpads)
if (self->airpad_state != AIRPADS_UP)
return DMC2280RunAir(self, fValue);
if (self->settle)
self->time_settle_done.tv_sec = 0;
do {
#if 1
float steps, counts;
if (FAILURE == readMotion(self, &steps, &counts))
return HWFault;
set_lastMotion(pData, steps, counts);
#else
float currPos;
DMC2280GetPos(pData, &currPos);
set_lastPos(pData, currPos);
#endif
} while (0);
axis=self->axisLabel;
motorHome = self->motorHome;
stepsPerX=self->stepsPerX;
snprintf(SHx, CMDLEN, "SH%c", axis);
snprintf(BGx, CMDLEN, "BG%c", axis);
target = fValue - self->home;
if (1 == self->abs_endcoder) {
absEncHome = self->absEncHome;
cntsPerX = self->cntsPerX;
/* PAF=-((absEncHome-_TPF)/-cntsPerX + target)*stepsPerX + _TDF */
snprintf(absPosCmd, CMDLEN,
"PA%c=(((%d-_TP%c)/%d)+%f)*%d + _TD%c",
axis,
absEncHome,
axis,
cntsPerX,
target,
stepsPerX,
axis);
#ifdef BACKLASHFIX
snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis, (int) (target * cntsPerX + absEncHome + 0.5));
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
#endif
} else {
newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5);
snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn);
}
if (FAILURE == DMC2280Send(self, SHx))
return HWFault;
if (FAILURE == DMC2280Send(self, absPosCmd))
return HWFault;
if (FAILURE == DMC2280Send(self, BGx))
return HWFault;
return OKOK;
}
/** \brief Check if the axis has moved significantly since
* the last check.
*
* The motion is checked against the expected at intervals of
* pDMC2280Driv->blockage_ckInterval.
*
* \param *pData provides access to a motor's data
* \return
* - 1 MOTOR OK, position has changed significantly during move
* - 0 MOTOR BLOCKED, no significant change in position detected.
*/
static int checkMotion(void *pData) {
float precision, steps, counts, ratio_obs, ratio_exp, ratio_cmp;
long int usec_TimeDiff;
struct timeval now;
pDMC2280Driv self;
self = (pDMC2280Driv)pData;
assert(self != NULL);
gettimeofday(&now, NULL);
usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec;
usec_TimeDiff *= 1000000;
usec_TimeDiff += now.tv_usec;
usec_TimeDiff -= self->time_lastPos_set.tv_usec;
if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
return 1;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
MotorGetPar(self->pMot,"precision",&precision);
if (FAILURE == readMotion(self, &steps, &counts))
return 0;
/* If not stepping, then not blocked */
if (fabs(steps - self->lastSteps) < self->blockage_thresh * self->cntsPerX) {
/* just update the timestamp */
set_lastMotion(pData, self->lastSteps, self->lastCounts);
return 1;
}
/* calculate observed and expected steps per count ratios */
ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts);
ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX;
ratio_cmp = ratio_obs / ratio_exp;
/* less than half or more than double is trouble */
if (ratio_cmp > self->blockage_ratio || (1.0 / ratio_cmp) > self->blockage_ratio) {
char msg[132];
snprintf(msg, sizeof(msg), "Motion check fail: obs=%f, exp=%f",
ratio_obs, ratio_exp);
SICSLogWrite(msg, eError);
snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%d/%d",
steps, self->lastSteps, counts, self->lastCounts,
self->stepsPerX, self->cntsPerX);
SICSLogWrite(msg, eError);
if (self->blockage_fail)
return 0;
set_lastMotion(pData, steps, counts);
return 1;
} else {
set_lastMotion(pData, steps, counts);
return 1;
}
return 1;
}
/** \brief Check if the axis position has changed significantly since
* the last check.
*
* The position change is checked against the 'precision' at intervals of
* pDMC2280Driv->blockage_ckInterval.
*
* \param *pData provides access to a motor's data
* \return
* - 1 MOTOR OK, position has changed significantly during move
* - 0 MOTOR BLOCKED, no significant change in position detected.
*/
static int checkPosition(void *pData) {
float precision, currPos;
long int usec_TimeDiff;
struct timeval now;
pDMC2280Driv self;
self = (pDMC2280Driv)pData;
assert(self != NULL);
gettimeofday(&now, NULL);
usec_TimeDiff = (now.tv_sec - (self->time_lastPos_set).tv_sec)*1e6 + (now.tv_usec - (self->time_lastPos_set).tv_usec);
if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
return 1;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
MotorGetPar(self->pMot,"precision",&precision);
DMC2280GetPos(pData, &currPos);
if ( precision - fabs(self->lastPosition - currPos) >= FLT_EPSILON) {
return 0;
} else {
set_lastPos(pData, currPos);
return 1;
}
}
/** \brief Returns the motor status while it's moving,
* implements the GetStatus method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \return
* - HWFault hardware fault or status cannot be read.
* - HWPosFault controller was unable to position the motor.
* - HWBusy The motor is still driving.
* - HWWarn There is a warning from the controller.
* - HWIdle The motor has finished driving and is idle.
*/
static int DMC2280Status(void *pData){
pDMC2280Driv self = NULL;
char cmd[CMDLEN];
int switches;
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);
/*
* If we are waiting for the motor or airpads then we
* are busy
*/
if (self->motor_timer || self->airpad_timer)
return HWBusy;
/* 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);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
if (FAILURE == DMC2280Receive(self, switchesAscii))
return HWFault;
sscanf(switchesAscii, "%d", &switches);
moving = (switches & STATUSMOVING)>0;
fwd_limit_active = !((switches & STATUSFWDLIMIT)>0);
rvrs_limit_active = !((switches & STATUSRVRSLIMIT)>0);
errorlimit = (switches & STATUSERRORLIMIT)>0;
if (fwd_limit_active && rvrs_limit_active) {
self->errorCode = IMPOSSIBLE_LIM_SW;
return HWFault;
}
if (moving) {
int iRet;
/* If pos hasn't changed since last
* check then stop and scream */
#if 0
iRet = checkPosition(pData);
#else
iRet = checkMotion(pData);
#endif
if (iRet == 0) {
DMC2280Halt(pData);
self->errorCode = BLOCKED;
return HWFault;
} else {
self->errorCode = BADBSY;
return HWBusy;
}
} else {
/* If motor stopped check limits and error status */
if (fwd_limit_active) {
self->errorCode = FWDLIM;
return HWFault;
} else if (rvrs_limit_active) {
self->errorCode = RVRSLIM;
return HWFault;
} else if (errorlimit) {
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.*/
if (FAILURE == DMC2280Send(self, "MG _XQ1"))
return HWFault;
if (FAILURE == DMC2280Receive(self, reply))
return HWFault;
sscanf(reply, "%d", &servoLoopStatus);
if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) {
/* Start subroutine on controller to close the servo loop */
if (FAILURE == DMC2280Send(self, "XQ#CLSLOOP"))
return HWFault;
}
snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
if (FAILURE == DMC2280Receive(self, reply))
return HWFault;
sscanf(reply, "%d", &should_fixpos);
if (should_fixpos == SHOULD_FIXPOS) {
snprintf(cmd, CMDLEN, "%cFIXPOS=1", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
self->errorCode=BADBSY;
return HWBusy;
}
}
#endif
/*
* When we get here, the motion has completed and we
* must determine when and how to shut off the motor
*/
if (self->settle) {
struct timeval *then = &self->time_settle_done;
struct timeval now;
gettimeofday(&now, NULL);
if (then->tv_sec == 0 ||
(then->tv_sec - now.tv_sec) > self->settle) {
gettimeofday(then, NULL);
then->tv_sec += self->settle;
return HWBusy;
} else {
if ((now.tv_sec > then->tv_sec) ||
((now.tv_sec == then->tv_sec) &&
(now.tv_usec >= then->tv_usec))) {
/* it's finished, fall through */
} else {
return HWBusy;
}
}
}
if (self->has_airpads) {
if (self->airpad_state == AIRPADS_DOWN)
return HWIdle;
if (self->airpad_state == AIRPADS_LOWER)
return HWBusy;
if (self->motOffDelay > 0 ) {
NetWatchRegisterTimer(&self->motor_timer,
self->motOffDelay,
motor_callback, self);
return HWIdle;
}
if (FAILURE == DMC2280Send(self, "FTUBE=0"))
return HWFault;
self->airpad_state = AIRPADS_LOWER;
self->airpad_counter = 10;
NetWatchRegisterTimer(&self->airpad_timer, 1000,
airpad_callback, self);
return HWIdle;
}
if (self->noPowerSave == _SAVEPOWER) {
if (self->motOffDelay > 0 ) {
#if 0
snprintf(cmd, CMDLEN, "AT %d; MO%c", self->motOffDelay, self->axisLabel);
#else
NetWatchRegisterTimer(&self->motor_timer,
self->motOffDelay,
motor_callback, self);
return HWIdle;
#endif
} else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
}
DMC2280Send(self, cmd);
}
return HWIdle;
}
}
/** \brief DMC2280 implementation of the GetError
* method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \param *iCode error code returned to abstract motor
* \param *error error message
* \param errLen maximum error message length allowed by abstract motor
*/
static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
assert(self != NULL);
/* Allocate errLen bytes for error messages */
if (self->errorMsg == NULL) {
self->errorMsg = (char *) malloc(errLen);
if (self->errorMsg == NULL) {
*iCode = 0;
return;
}
}
*iCode = self->errorCode;
switch(*iCode){
case NOTCONNECTED:
case TIMEOUT:
case BADSEND:
case BADMEMORY:
case INCOMPLETE:
getRS232Error(*iCode, error, errLen);
break;
case BADADR:
strncpy(error,"Bad address",(size_t)errLen);
break;
case BADBSY:
strncpy(error,"Motor still busy",(size_t)errLen);
break;
case BADCMD:
snprintf(error, (size_t)errLen, "Bad command: '%s'\ndmcError: ", self->lastCmd);
strncat(error, self->dmc2280Error, (size_t)errLen);
break;
case BADPAR:
strncpy(error,"Bad parameter",(size_t)errLen);
break;
case BADUNKNOWN:
strncpy(error,"Unknown error condition",(size_t)errLen);
break;
case BADSTP:
strncpy(error,"Motor is stopped",(size_t)errLen);
break;
case BADEMERG:
strncpy(error,"Emergency stop is engaged",(size_t)errLen);
break;
case BGFAIL:
strncpy(error,"Begin not possible due to limit switch",(size_t)errLen);
break;
case RVRSLIM:
strncpy(error,"Crashed into limit switch",(size_t)errLen);
break;
case FWDLIM:
strncpy(error,"Crashed into limit switch",(size_t)errLen);
break;
case POSFAULT:
strncpy(error,"Positioning fault detected",(size_t)errLen);
break;
case BADCUSHION:
strncpy(error,"Air cushion problem",(size_t)errLen);
break;
case ERRORLIM:
strncpy(error,"Axis error exceeds error limit",(size_t)errLen);
break;
case IMPOSSIBLE_LIM_SW:
strncpy(error,"Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen);
break;
case BLOCKED:
strncpy(error,"STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen);
break;
default:
/* FIXME What's the default */
break;
}
strncpy(self->errorMsg,error,(size_t)errLen);
}
/** \brief Attempts to recover from an error. Implements the TryAndFixIt
* method in the MotorDriver interface.
*
* \param *pData provides access to a motor's data
* \param iCode error code returned by DMC2280Error
* \param fValue unused, target position
* \return A return code which informs the abstract motors next action.
* - MOTREDO try to redo the last move.
* - MOTFAIL move failed, give up.
*/
static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
assert(self != NULL);
switch(iCode){
case BADADR:
return MOTFAIL;
case BADCMD:
//case TIMEOUT:
case BADPAR:
case BLOCKED:
return MOTFAIL;
case POSFAULT:
case BADSEND:
case TIMEOUT:
case BADMEMORY: /* Won't happen if MonConnect sets the send terminator */
case INCOMPLETE:
return MOTREDO;
case NOTCONNECTED:
initRS232(self->controller);
return MOTREDO;
break;
default:
return MOTFAIL;
break;
}
}
/** \brief Emergency halt. Implements the Halt
* method in the MotorDriver interface.
*
* Cannot set maximum deceleration because the DC command
* is not valid for absolute (ie PA) moves. See DC description
* in DMC-2xxx command ref (ie manc2xxx.pdf)
* \param *pData provides access to a motor's data
*
* XXX Does abstract motor use the return values?
*/
static int DMC2280Halt(void *pData){
pDMC2280Driv self = NULL;
char cmd[CMDLEN];
self = (pDMC2280Driv)pData;
assert(self != NULL);
/* Stop motor */
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
else
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.
* \return
* - 1 request succeeded
* - 0 request failed
* */
static int DMC2280GetPar(void *pData, char *name,
float *fValue){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
/* XXX Maybe move this to a configuration parameter.*/
if(strcmp(name,HOME) == 0) {
*fValue = self->home;
return 1;
}
if(strcmp(name,HARDLOWERLIM) == 0) {
*fValue = self->fLower;
return 1;
}
if(strcmp(name,HARDUPPERLIM) == 0) {
*fValue = self->fUpper;
return 1;
}
if(strcmp(name,SPEED) == 0) {
*fValue = self->speed;
return 1;
}
if(strcmp(name,MAXSPEED) == 0) {
*fValue = self->maxSpeed;
return 1;
}
if(strcmp(name,ACCEL) == 0) {
*fValue = self->accel;
return 1;
}
if(strcmp(name,MAXACCEL) == 0) {
*fValue = self->maxAccel;
return 1;
}
if(strcmp(name,DECEL) == 0) {
*fValue = self->decel;
return 1;
}
if(strcmp(name,MAXDECEL) == 0) {
*fValue = self->maxDecel;
return 1;
}
if(strcmp(name,MOTOFFDELAY) == 0) {
*fValue = self->motOffDelay;
return 1;
}
if(strcmp(name,"debug") == 0) {
*fValue = self->debug;
return 1;
}
if(strcmp(name,SETTLE) == 0) {
*fValue = self->settle;
return 1;
}
if(strcmp(name,AIRPADS) == 0) {
*fValue = self->has_airpads;
return 1;
}
if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
*fValue = self->blockage_ckInterval;
return 1;
}
if(strcmp(name,"blockage_thresh") == 0) {
*fValue = self->blockage_thresh;
return 1;
}
if(strcmp(name,"blockage_ratio") == 0) {
*fValue = self->blockage_ratio;
return 1;
}
if(strcmp(name,"blockage_fail") == 0) {
*fValue = self->blockage_fail;
return 1;
}
if (self->abs_endcoder != 0) {
if (strcmp(name,"absenc") == 0) {
if (readAbsEnc(self, fValue) == SUCCESS)
return 1;
else
return 0;
}
}
return 0;
}
/** \brief Sets the named parameter, implements the SetDriverPar
* method in the MotorDriver interface.
*
* Note: The SetDriverPar method in the MotorDriver interface only
* allows float values to be set.
* \param *pData (rw) provides access to a motor's data
* \param *pCon (r) connection object.
* \param *name (r) of the parameter to set.
* \param *newValue (r) new value.
* \return
* - 1 request succeeded
* - 0 request failed
* */
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue){
pDMC2280Driv self = NULL;
char pError[ERRLEN];
char cmd[CMDLEN];
float currPos, oldZero, newZero;
self = (pDMC2280Driv)pData;
/* XXX Maybe move this to a configuration parameter.*/
/* Set home, managers only. Users should set softposition */
if(strcmp(name,HOME) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
if ( (self->fLower - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
SCWrite(pCon, pError, eError);
return 1;
}
if ( (newValue - self->fUpper) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
SCWrite(pCon, pError, eError);
return 1;
}
self->home = newValue;
return 1;
}
}
if(strcmp(name,SETPOS) == 0) {
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
MotorGetPar(self->pMot,"softzero",&oldZero);
newZero = (self->pMot->fPosition - newValue) + oldZero;
MotorSetPar(self->pMot,pCon,"softzero",newZero);
return 1;
}
/* Set motor off delay, managers only */
if(strcmp(name,MOTOFFDELAY) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->motOffDelay = newValue;
return 1;
}
}
/* Debug, managers only */
if(strcmp(name,"debug") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->debug = newValue;
return 1;
}
}
/* Setttle, managers only */
if(strcmp(name,SETTLE) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->settle = newValue;
return 1;
}
}
/* Set airpads, managers only */
if(strcmp(name,AIRPADS) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->has_airpads = newValue;
return 1;
}
}
/* Set interval between blocked motor checks,
* managers only */
if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->blockage_ckInterval = newValue;
return 1;
}
}
/* Set interval between blocked motor checks,
* managers only */
if(strcmp(name,"blockage_thresh") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->blockage_thresh = newValue;
return 1;
}
}
/* Set interval between blocked motor checks,
* managers only */
if(strcmp(name,"blockage_ratio") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->blockage_ratio = newValue;
return 1;
}
}
/* Set interval between blocked motor checks,
* managers only */
if(strcmp(name,"blockage_fail") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->blockage_fail = newValue;
return 1;
}
}
/* Set speed */
if(strcmp(name,SPEED) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, SPEED, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxSpeed ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, SPEED, self->maxSpeed);
SCWrite(pCon, pError, eError);
return 1;
}
self->speed = newValue;
snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self, self->speed));
if (FAILURE == DMC2280Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
}
/* Set Acceleration */
if(strcmp(name,ACCEL) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, ACCEL, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxAccel ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, ACCEL, self->maxAccel);
SCWrite(pCon, pError, eError);
return 1;
}
self->accel = newValue;
snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel));
if (FAILURE == DMC2280Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
}
/* Set Deceleration */
if(strcmp(name,DECEL) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, DECEL, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxDecel ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, DECEL, self->maxDecel);
SCWrite(pCon, pError, eError);
return 1;
}
self->decel = newValue;
snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel));
if (FAILURE == DMC2280Send(self, cmd))
return 0; /* FIXME should signal a HWFault */
return 1;
}
return 0;
}
/** \brief List the motor parameters to the client.
* \param self (r) provides access to the motor's data structure
* \param *name (r) name of motor.
* \param *pCon (r) connection object.
*/
static void DMC2280List(void *self, char *name, SConnection *pCon){
char buffer[BUFFLEN];
snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, ((pDMC2280Driv)self)->axisLabel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, ((pDMC2280Driv)self)->home);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, ((pDMC2280Driv)self)->units);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, ((pDMC2280Driv)self)->speed);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, ((pDMC2280Driv)self)->maxSpeed);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, ((pDMC2280Driv)self)->accel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, ((pDMC2280Driv)self)->maxAccel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, ((pDMC2280Driv)self)->decel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, ((pDMC2280Driv)self)->maxDecel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, ((pDMC2280Driv)self)->motOffDelay);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, ((pDMC2280Driv)self)->debug);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, ((pDMC2280Driv)self)->settle);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, ((pDMC2280Driv)self)->blockage_ckInterval);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, ((pDMC2280Driv)self)->blockage_thresh);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, ((pDMC2280Driv)self)->blockage_ratio);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, ((pDMC2280Driv)self)->blockage_fail);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder);
SCWrite(pCon, buffer, eStatus);
if (((pDMC2280Driv)self)->abs_endcoder) {
snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, ((pDMC2280Driv)self)->absEncHome);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.cntsPerX = %d\n", name, ((pDMC2280Driv)self)->cntsPerX);
SCWrite(pCon, buffer, eStatus);
}
return;
}
/** \brief Free memory if motor is removed
* \param *pData (rw) provides access to the motor's data structure
*/
static void KillDMC2280(/*@only@*/void *pData){
pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData;
assert(self != NULL);
if (self->name)
free(self->name);
if (self->errorMsg)
free(self->errorMsg);
free(self);
return;
}
/*@only@*/ prs232 createRS232(char *host, int iPort);
/** \brief Open a connection to the motor controller
* \param *pCon (r) connection object.
* \param *host (r) DMC2280 host address or name.
* \param port DMC2280 port number
* \return controller structure
*/
/*@null@*/ /*@only@*/ static prs232 DMC2280Connect(/*@dependent@*/SConnection *pCon, char *host, int port) {
prs232 controller=NULL;
char pError[ERRLEN];
int msecTimeout = 5000;
controller=createRS232(host,port);
if (controller==NULL) {
snprintf(pError, ERRLEN,
"ERROR: failed to create controller for %s at port %d",
host, port);
SCWrite(pCon,pError,eError);
return NULL;
}
if ( initRS232(controller) != 1) {
snprintf(pError, ERRLEN,
"ERROR: failed to connect to %s at port %d",
controller->pHost, controller->iPort);
SCWrite(pCon,pError,eError);
KillRS232(controller);
return NULL;
}
setRS232ReplyTerminator(controller,"&\r\n:");
setRS232SendTerminator(controller,"\r\n");
setRS232Timeout(controller, msecTimeout);
return controller;
}
/** \brief Create a driver for the DMC2280 Galil controller.
*
* This is called by the Motor configuration command in the
* SICS configuration file when you create a DMC2280 motor.
*
* Usage:\n
* Motor stth DMC2280 paramArray\n
* - stth is the motor name
* - DMC2280 is the motor type that will lead to calling this function.
* - paramArray is a Tcl array of the motor parameters.
*
* \param *pCon (r) connection object.
* \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;
char *pPtr = NULL;
char buffer[132];
char pError[ERRLEN];
char cmd[CMDLEN];
int port;
Tcl_Interp *interp;
buffer[0]='\0';
interp = InterpGetTcl(pServ->pSics);
/*
allocate and initialize data structure
*/
pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv));
if(NULL == pNew){
(void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",
eError);
return NULL;
}
memset(pNew, 0, sizeof(DMC2280Driv));
pNew->controller = NULL;
pNew->name = NULL;
pNew->errorCode = 0;
pNew->errorMsg = NULL;
pNew->lastCmd[0] = '\0';
pNew->dmc2280Error[0] = '\0';
pNew->absEncHome = 0;
pNew->cntsPerX = 0;
/* Get hostname and port from the list of named parameters */
if ((pPtr=getParam(pCon, interp, params,"port",1)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
if (sscanf(pPtr,"%d",&port)==0)
port = getPortNum(pCon, pPtr);
if ((pPtr=getParam(pCon, interp, params,"host",1)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
strncpy(buffer,pPtr, 131);
/* Connect to the controller */
pNew->controller = DMC2280Connect(pCon, buffer,port);
if( pNew->controller == NULL ) {
snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
SCWrite(pCon,pError,eError);
KillDMC2280(pNew);
return NULL;
}
/*FIXME Tell splint that there's no memory leak because pointers are being initialised here */
pNew->name = (char *)malloc(sizeof(char)*(strlen(motor)+1));
if (pNew->name == NULL) {
(void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",
eError);
KillDMC2280(pNew);
return NULL;
}
pNew->pMot = NULL;
strcpy(pNew->name, motor);
pNew->home = 0.0;
pNew->fLower = 0.0;//(float)atof(argv[2]);
pNew->fUpper = 0.0;//(float)atof(argv[3]);
pNew->GetPosition = DMC2280GetPos;
pNew->RunTo = DMC2280Run;
pNew->GetStatus = DMC2280Status;
pNew->GetError = DMC2280Error;
pNew->TryAndFixIt = DMC2280Fix;
pNew->Halt = DMC2280Halt;
pNew->GetDriverPar = DMC2280GetPar;
pNew->SetDriverPar = DMC2280SetPar;
pNew->ListDriverPar = DMC2280List;
pNew->KillPrivate = KillDMC2280;
pNew->blockage_ckInterval = 0.5;
pNew->blockage_thresh = 0.5;
pNew->blockage_ratio = 2.0;
pNew->blockage_fail = 0;
/* PARAMETERS: Fetch parameter values */
if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->fLower));
if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->fUpper));
if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%s",pNew->units);
if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->speed));
pNew->maxSpeed = pNew->speed;
if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->accel));
pNew->maxAccel = pNew->accel;
if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->decel));
pNew->maxDecel = pNew->decel;
if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%c",&(pNew->axisLabel));
if ((pPtr=getParam(pCon, interp, params,"stepsperx",_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%d",&(pNew->stepsPerX));
if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL)
pNew->motorHome=0;
else
sscanf(pPtr,"%d",&(pNew->motorHome));
if ((pPtr=getParam(pCon, interp, params,"nopowersave",_OPTIONAL)) == NULL)
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));
/* Debug: this motor driver logs exchanges */
if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL)
pNew->debug=0;
else {
sscanf(pPtr,"%d",&(pNew->debug));
}
/* SETTLE: this motor need time to settle */
if ((pPtr=getParam(pCon, interp, params,"settle",_OPTIONAL)) == NULL)
pNew->settle=0;
else {
sscanf(pPtr,"%d",&(pNew->settle));
}
/* AIRPADS: this motor need airpads */
if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL)
pNew->has_airpads=0;
else {
sscanf(pPtr,"%d",&(pNew->has_airpads));
}
/* 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 {
sscanf(pPtr,"%d",&(pNew->abs_endcoder));
if ((pPtr=getParam(pCon, interp, params,"absenchome",_REQUIRED)) == NULL)
pNew->absEncHome=0;
else
sscanf(pPtr,"%d",&(pNew->absEncHome));
if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL)
pNew->cntsPerX=1;
else
sscanf(pPtr,"%d",&(pNew->cntsPerX));
}
/* Set speed */
snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed));
if (FAILURE == DMC2280Send(pNew, cmd))
return NULL;
/* Set acceleration */
snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel));
if (FAILURE == DMC2280Send(pNew, cmd))
return NULL;
/* Set deceleration */
snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel));
if (FAILURE == DMC2280Send(pNew, cmd))
return NULL;
/* TODO Initialise current position and target to get a sensible initial list output */
return (MotorDriver *)pNew;
}
Reference in New Issue View Git Blame Copy Permalink