1125 lines
36 KiB
C
1125 lines
36 KiB
C
/** \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 ESTOP flag on controller.
|
|
* - ESTOP=1 means emergency stop, motion on all axes must stop.
|
|
* - ESTOP=0 means all OK.
|
|
*/
|
|
#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 <fortify.h>
|
|
#include <sics.h>
|
|
#include <rs232controller.h>
|
|
#include <modriv.h>
|
|
#include <dynstring.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);
|
|
/* Storage returned by Tcl_GetVar2 is owned by the Tcl interpreter and should not be modified */
|
|
/*@observer@*//*@dependent@*/ char *Tcl_GetVar2(Tcl_Interp *interp, char *name1, char *name2, int flags);
|
|
/* 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
|
|
------------------------------------------------------------------------*/
|
|
typedef struct __MoDriv {
|
|
/* general motor driver interface
|
|
fields. _REQUIRED!
|
|
*/
|
|
float fUpper; /* upper limit */
|
|
float fLower; /* lower limit */
|
|
char *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;
|
|
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 */
|
|
} DMC2280Driv, *pDMC2280Driv;
|
|
/*------------------- 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 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 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"
|
|
|
|
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=20, 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@*/
|
|
}
|
|
|
|
/*@+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=20, dataLen=255;
|
|
reply[0] = '\0';
|
|
for (i=0; i<retries; i++) {
|
|
status=readRS232TillTerm(self->controller, reply, &dataLen);
|
|
switch (status) {
|
|
case 1:
|
|
return dataLen;
|
|
case TIMEOUT:
|
|
self->errorCode = status;
|
|
continue;
|
|
/* TODO case INCOMPLETE: */
|
|
default:
|
|
self->errorCode = status;
|
|
return FAILURE;
|
|
}
|
|
}
|
|
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.
|
|
*
|
|
* \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) {
|
|
snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
|
|
if (FAILURE == DMC2280Send(self, cmd))
|
|
return HWFault;
|
|
if (FAILURE == DMC2280Receive(self, reply))
|
|
return HWFault;
|
|
absEncPos =(float)atof(reply);
|
|
*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;
|
|
}
|
|
/** \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);
|
|
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;
|
|
newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5);
|
|
snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn);
|
|
|
|
if (1 == self->abs_endcoder) {
|
|
/* Ensure that the defined motor position matches actual position */
|
|
absEncHome = self->absEncHome;
|
|
cntsPerX = self->cntsPerX;
|
|
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))
|
|
return HWFault;
|
|
if (FAILURE == DMC2280Send(self, SHx))
|
|
return HWFault;
|
|
if (FAILURE == DMC2280Send(self, BGx))
|
|
return HWFault;
|
|
return OKOK;
|
|
}
|
|
|
|
|
|
/** \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);
|
|
/* 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) {
|
|
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
|
|
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;
|
|
}
|
|
}
|
|
/** \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 reverse limit switch",(size_t)errLen);
|
|
break;
|
|
case FWDLIM:
|
|
strncpy(error,"Crashed into forward 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;
|
|
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:
|
|
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;
|
|
}
|
|
return MOTFAIL;
|
|
}
|
|
/** \brief Emergency halt. Implements the Halt
|
|
* method in the MotorDriver interface.
|
|
*
|
|
* Uses maximum deceleration
|
|
* \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);
|
|
/* Set maximum deceleration to stop motor */
|
|
snprintf(cmd, CMDLEN, "DC%c", motDecel(self, self->maxDecel));
|
|
if (FAILURE == DMC2280Send(self, cmd))
|
|
return HWFault;
|
|
/* Stop motor */
|
|
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
|
|
if (FAILURE == DMC2280Send(self, cmd))
|
|
return HWFault;
|
|
/* Restore deceleration */
|
|
snprintf(cmd, CMDLEN, "DC%c", motDecel(self, self->decel));
|
|
if (FAILURE == DMC2280Send(self, cmd))
|
|
return HWFault;
|
|
|
|
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;
|
|
|
|
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;
|
|
}
|
|
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];
|
|
|
|
self = (pDMC2280Driv)pData;
|
|
|
|
/* Set home */
|
|
if(strcmp(name,HOME) == 0) {
|
|
if ( (self->fLower - newValue) > FLT_EPSILON) {
|
|
snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", HOME, self->fLower);
|
|
SCWrite(pCon, pError, eError);
|
|
return 1;
|
|
}
|
|
if ( (newValue - self->fUpper) > FLT_EPSILON) {
|
|
snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", HOME, self->fUpper);
|
|
SCWrite(pCon, pError, eError);
|
|
return 1;
|
|
}
|
|
self->home = newValue;
|
|
return 1;
|
|
}
|
|
|
|
/* Set upper limit, lower limit */
|
|
if(strcmp(name,HARDLOWERLIM) == 0) {
|
|
self->fLower = newValue;
|
|
return 1;
|
|
}
|
|
if(strcmp(name,HARDUPPERLIM) == 0) {
|
|
self->fUpper = newValue;
|
|
return 1;
|
|
}
|
|
|
|
/* Set speed */
|
|
if(strcmp(name,SPEED) == 0) {
|
|
if ((0.0 - newValue) > FLT_EPSILON) {
|
|
snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", SPEED, 0.0);
|
|
SCWrite(pCon, pError, eError);
|
|
return 1;
|
|
}
|
|
if ((newValue - self->maxSpeed ) > FLT_EPSILON) {
|
|
snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", 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 must be greater than or equal to %f", ACCEL, 0.0);
|
|
SCWrite(pCon, pError, eError);
|
|
return 1;
|
|
}
|
|
if ((newValue - self->maxAccel ) > FLT_EPSILON) {
|
|
snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", 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 must be greater than or equal to %f", DECEL, 0.0);
|
|
SCWrite(pCon, pError, eError);
|
|
return 1;
|
|
}
|
|
if ((newValue - self->maxDecel ) > FLT_EPSILON) {
|
|
snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", 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);
|
|
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 usecTimeout = 50000; /* 50msec timeout */
|
|
|
|
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, usecTimeout);
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
sscanf(pPtr,"%d",&port);
|
|
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;
|
|
}
|
|
strcpy(pNew->name, motor);
|
|
pNew->home = 0.0;
|
|
pNew->fLower = 0.0;//(float)atof(argv[2]);
|
|
pNew->fUpper = 100.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;
|
|
|
|
/* PARAMETERS: Fetch parameter values */
|
|
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));
|
|
|
|
/* 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))
|
|
exit(EXIT_FAILURE);
|
|
/* Set acceleration */
|
|
snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel));
|
|
if (FAILURE == DMC2280Send(pNew, cmd))
|
|
exit(EXIT_FAILURE);
|
|
/* Set deceleration */
|
|
snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel));
|
|
if (FAILURE == DMC2280Send(pNew, cmd))
|
|
exit(EXIT_FAILURE);
|
|
/* TODO Initialise current position and target to get a sensible initial list output */
|
|
return (MotorDriver *)pNew;
|
|
}
|