Refactoring, reduce duplicate code, eliminate obsolete code.
Fix a few bugs. Improve blocked motor detection code. Align with MultiChan version r1708 | dcl | 2007-03-23 18:06:20 +1100 (Fri, 23 Mar 2007) | 5 lines
This commit is contained in:
Douglas Clowes
@@ -32,9 +32,7 @@
|
||||
|
||||
#define UNITSLEN 256
|
||||
#define TEXTPARLEN 1024
|
||||
void KillRS232(/*@only@*/ void *pData);
|
||||
/*@+incondefs@*/
|
||||
|
||||
#define CMDLEN 1024
|
||||
/** \brief Used to ensure that the getDMCSetting function is called
|
||||
* with valid values.
|
||||
* \see getDMCSetting
|
||||
@@ -81,8 +79,8 @@ typedef struct __MoDriv {
|
||||
float decel; /**< physical units per second^2 */
|
||||
float maxDecel; /**< physical units per second^2 */
|
||||
char axisLabel;
|
||||
char lastCmd[1024];
|
||||
char dmc2280Error[1024];
|
||||
char lastCmd[CMDLEN];
|
||||
char dmc2280Error[CMDLEN];
|
||||
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 */
|
||||
@@ -144,7 +142,6 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
|
||||
|
||||
#define INIT_STR_SIZE 256
|
||||
#define STR_RESIZE_LENGTH 256
|
||||
#define CMDLEN 1024
|
||||
#define BUFFLEN 512
|
||||
#define _SAVEPOWER 0
|
||||
|
||||
@@ -167,7 +164,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
|
||||
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.
|
||||
@@ -234,6 +231,39 @@ static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) {
|
||||
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, 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 Sends a DMC2280 command to the motor controller.
|
||||
*
|
||||
* If the command fails it displays the DMC2280 error message to the client
|
||||
@@ -249,7 +279,9 @@ static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) {
|
||||
*/
|
||||
/* First character returned by controller is
|
||||
'?' for an invalid command or
|
||||
':' or space for a valid command */
|
||||
':' for a valid command with no response
|
||||
' ' for a valid command with a response (':' follows)
|
||||
*/
|
||||
static int DMC2280Send(pDMC2280Driv self, char *command) {
|
||||
char cmdValid, reply[256];
|
||||
char *GetEMsg = "TC 1";
|
||||
@@ -300,37 +332,22 @@ static int DMC2280Send(pDMC2280Driv self, char *command) {
|
||||
}
|
||||
}
|
||||
|
||||
/** \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.
|
||||
/**
|
||||
* \brief Sends a command and waits for a response
|
||||
*
|
||||
* \param self motor data
|
||||
* \param cmd command to send
|
||||
* \param reply space to return response
|
||||
* \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;
|
||||
static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
|
||||
int status;
|
||||
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return FAILURE;
|
||||
if (FAILURE == DMC2280Receive(self, reply))
|
||||
return FAILURE;
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
/** \brief Record the given posn and timestamp it.
|
||||
@@ -345,82 +362,6 @@ static void set_lastMotion(pDMC2280Driv self, float steps, float counts) {
|
||||
gettimeofday(&(self->time_lastPos_set), NULL);
|
||||
}
|
||||
|
||||
#if 0
|
||||
/** \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);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/** \brief Reads motion.
|
||||
*
|
||||
@@ -431,13 +372,11 @@ static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){
|
||||
* FAILURE
|
||||
*/
|
||||
static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
|
||||
char reply[1024];
|
||||
char reply[CMDLEN];
|
||||
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))
|
||||
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
|
||||
return FAILURE;
|
||||
|
||||
if (2 != sscanf(reply, "%f %f", steps, counts))
|
||||
@@ -453,13 +392,11 @@ static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
|
||||
* FAILURE
|
||||
*/
|
||||
static int readAbsEnc(pDMC2280Driv self, float *pos) {
|
||||
char reply[1024];
|
||||
char reply[CMDLEN];
|
||||
char cmd[CMDLEN];
|
||||
|
||||
snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return FAILURE;
|
||||
if (FAILURE == DMC2280Receive(self, reply))
|
||||
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
|
||||
return FAILURE;
|
||||
|
||||
*pos = (float) atoi(reply);
|
||||
@@ -474,13 +411,11 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) {
|
||||
* FAILURE
|
||||
*/
|
||||
static int ReadThread(pDMC2280Driv self, int thread, float *pos) {
|
||||
char reply[1024];
|
||||
char reply[CMDLEN];
|
||||
char cmd[CMDLEN];
|
||||
|
||||
snprintf(cmd, CMDLEN, "MG _XQ%d", thread);
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return 0;
|
||||
if (FAILURE == DMC2280Receive(self, reply))
|
||||
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
|
||||
return 0;
|
||||
|
||||
*pos = (float) atoi(reply);
|
||||
@@ -495,13 +430,11 @@ static int ReadThread(pDMC2280Driv self, int thread, float *pos) {
|
||||
* FAILURE
|
||||
*/
|
||||
static int readHomeRun(pDMC2280Driv self, float *pos) {
|
||||
char reply[1024];
|
||||
char reply[CMDLEN];
|
||||
char cmd[CMDLEN];
|
||||
|
||||
snprintf(cmd, CMDLEN, "MG HOMERUN");
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return FAILURE;
|
||||
if (FAILURE == DMC2280Receive(self, reply))
|
||||
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
|
||||
return FAILURE;
|
||||
|
||||
*pos = (float) atoi(reply);
|
||||
@@ -547,7 +480,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
|
||||
* */
|
||||
static int DMC2280GetPos(void *pData, float *fPos){
|
||||
pDMC2280Driv self = NULL;
|
||||
char reply[1024];
|
||||
char reply[CMDLEN];
|
||||
char cmd[CMDLEN];
|
||||
float absEncPos, motorPos;
|
||||
|
||||
@@ -560,9 +493,7 @@ static int DMC2280GetPos(void *pData, float *fPos){
|
||||
*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))
|
||||
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
|
||||
return HWFault;
|
||||
motorPos =(float)atof(reply);
|
||||
*fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home;
|
||||
@@ -570,136 +501,23 @@ static int DMC2280GetPos(void *pData, float *fPos){
|
||||
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.
|
||||
/**
|
||||
* \brief calculate and issue the motion commands
|
||||
*
|
||||
* \param *pData provides access to a motor's data
|
||||
* \param self 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;
|
||||
|
||||
*/
|
||||
static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
|
||||
char axis;
|
||||
char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN];
|
||||
int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn;
|
||||
float target;
|
||||
self = (pDMC2280Driv)pData;
|
||||
assert(self != NULL);
|
||||
|
||||
/* If Motion Control is off, report HWFault */
|
||||
if (DMC2280MotionControl != 1) {
|
||||
if (DMC2280MotionControl == 0)
|
||||
self->errorCode = MOTIONCONTROLOFF;
|
||||
else
|
||||
self->errorCode = MOTIONCONTROLUNK;
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
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;
|
||||
@@ -743,6 +561,174 @@ static int DMC2280Run(void *pData,float fValue){
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* \brief request the airpad status periodically
|
||||
*/
|
||||
static int airpad_timeout(void* ctx, int mode) {
|
||||
pDMC2280Driv self = (pDMC2280Driv) ctx;
|
||||
char reply[CMDLEN];
|
||||
float fReply;
|
||||
|
||||
self->airpad_timer = NULL;
|
||||
|
||||
if (self->airpad_state == AIRPADS_UP ||
|
||||
self->airpad_state == AIRPADS_DOWN)
|
||||
return 0;
|
||||
|
||||
if (self->airpad_counter <= 0) {
|
||||
self->errorCode = BADCUSHION;
|
||||
self->airpad_state = AIRPADS_DOWN;
|
||||
return 0;
|
||||
}
|
||||
if (FAILURE == DMC2280SendReceive(self, "MG APDONE", 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 = DMC2280RunCommon(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_timeout, self);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief initiate the raising or lowering of the airpads
|
||||
*
|
||||
* \param self motor data
|
||||
* \param flag 1 for raise and 0 for lower
|
||||
* \return 1 for SUCCESS or 0 for FAILURE
|
||||
*/
|
||||
static int DMC_AirPads(pDMC2280Driv self, int flag) {
|
||||
char *cmd = NULL;
|
||||
|
||||
if (self->airpad_timer)
|
||||
NetWatchRemoveTimer(self->airpad_timer);
|
||||
self->airpad_timer = NULL;
|
||||
if (flag) {
|
||||
cmd = "FTUBE=1";
|
||||
self->airpad_state = AIRPADS_RAISE;
|
||||
}
|
||||
else {
|
||||
cmd = "FTUBE=0";
|
||||
self->airpad_state = AIRPADS_LOWER;
|
||||
}
|
||||
if (FAILURE == DMC2280Send(self, cmd)) {
|
||||
self->airpad_state = AIRPADS_DOWN;
|
||||
self->errorCode = BADCUSHION;
|
||||
return 0;
|
||||
}
|
||||
self->airpad_counter = 10;
|
||||
NetWatchRegisterTimer(&self->airpad_timer, 1000,
|
||||
airpad_timeout, self);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief turn the motor off after a delay
|
||||
*
|
||||
* \param context motor data
|
||||
* \param mode
|
||||
*/
|
||||
static int motor_timeout(void* context, int mode) {
|
||||
pDMC2280Driv self = (pDMC2280Driv) context;
|
||||
char cmd[CMDLEN];
|
||||
|
||||
self->motor_timer = NULL;
|
||||
if (self->has_airpads) {
|
||||
DMC_AirPads(self, 0);
|
||||
return 0;
|
||||
}
|
||||
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
|
||||
DMC2280Send(self, cmd);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief handle the run command for a motor with airpads
|
||||
*
|
||||
* \param self motor data
|
||||
* \param fValue new motor position
|
||||
*/
|
||||
static int DMC2280RunAir(pDMC2280Driv self, float fValue) {
|
||||
self->fTarget = fValue;
|
||||
if (!DMC_AirPads(self, 1))
|
||||
return HWFault;
|
||||
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;
|
||||
self = (pDMC2280Driv)pData;
|
||||
assert(self != NULL);
|
||||
|
||||
/* If Motion Control is off, report HWFault */
|
||||
if (DMC2280MotionControl != 1) {
|
||||
if (DMC2280MotionControl == 0)
|
||||
self->errorCode = MOTIONCONTROLOFF;
|
||||
else
|
||||
self->errorCode = MOTIONCONTROLUNK;
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
if (self->motor_timer)
|
||||
NetWatchRemoveTimer(self->motor_timer);
|
||||
self->motor_timer = NULL;
|
||||
|
||||
if (self->settle)
|
||||
self->time_settle_done.tv_sec = 0;
|
||||
|
||||
/*
|
||||
* Note: this will read the current position
|
||||
*/
|
||||
do {
|
||||
#if 1
|
||||
self->time_lastPos_set.tv_sec = 0;
|
||||
#else
|
||||
float currPos;
|
||||
DMC2280GetPos(pData, &currPos);
|
||||
set_lastPos(pData, currPos);
|
||||
#endif
|
||||
} while (0);
|
||||
|
||||
/*
|
||||
* Note: this passes control to a timer routine
|
||||
*/
|
||||
if (self->has_airpads)
|
||||
if (self->airpad_state != AIRPADS_UP)
|
||||
return DMC2280RunAir(self, fValue);
|
||||
|
||||
return DMC2280RunCommon(self, fValue);
|
||||
}
|
||||
|
||||
/** \brief Check if the axis has moved significantly since
|
||||
* the last check.
|
||||
*
|
||||
@@ -762,6 +748,13 @@ static int checkMotion(void *pData) {
|
||||
pDMC2280Driv self;
|
||||
self = (pDMC2280Driv)pData;
|
||||
assert(self != NULL);
|
||||
if (self->time_lastPos_set.tv_sec == 0) {
|
||||
/* first time - initialise the data */
|
||||
if (FAILURE == readMotion(self, &steps, &counts))
|
||||
return 0;
|
||||
set_lastMotion(pData, steps, counts);
|
||||
return 1;
|
||||
}
|
||||
gettimeofday(&now, NULL);
|
||||
usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec;
|
||||
usec_TimeDiff *= 1000000;
|
||||
@@ -775,7 +768,7 @@ static int checkMotion(void *pData) {
|
||||
if (FAILURE == readMotion(self, &steps, &counts))
|
||||
return 0;
|
||||
/* If not stepping, then not blocked */
|
||||
if (fabs(steps - self->lastSteps) < self->blockage_thresh * self->cntsPerX) {
|
||||
if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
|
||||
/* just update the timestamp */
|
||||
set_lastMotion(pData, self->lastSteps, self->lastCounts);
|
||||
return 1;
|
||||
@@ -799,48 +792,18 @@ static int checkMotion(void *pData) {
|
||||
set_lastMotion(pData, steps, counts);
|
||||
return 1;
|
||||
} else {
|
||||
if (self->debug) {
|
||||
char msg[132];
|
||||
snprintf(msg, sizeof(msg), "Motion check pass: obs=%f, exp=%f",
|
||||
ratio_obs, ratio_exp);
|
||||
SICSLogWrite(msg, eError);
|
||||
}
|
||||
set_lastMotion(pData, steps, counts);
|
||||
return 1;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
#if 0
|
||||
/** \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;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/** \brief Returns the motor status while it's moving,
|
||||
* implements the GetStatus method in the MotorDriver interface.
|
||||
@@ -857,9 +820,9 @@ static int DMC2280Status(void *pData){
|
||||
pDMC2280Driv self = NULL;
|
||||
char cmd[CMDLEN];
|
||||
int switches;
|
||||
char switchesAscii[10];
|
||||
char switchesAscii[CMDLEN];
|
||||
#ifdef BACKLASHFIX
|
||||
char reply[256];
|
||||
char reply[CMDLEN];
|
||||
int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus;
|
||||
int SHOULD_FIXPOS=1, should_fixpos;
|
||||
#endif
|
||||
@@ -880,9 +843,7 @@ static int DMC2280Status(void *pData){
|
||||
/* 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))
|
||||
if (FAILURE == DMC2280SendReceive(self, cmd, switchesAscii))
|
||||
return HWFault;
|
||||
sscanf(switchesAscii, "%d", &switches);
|
||||
moving = (switches & STATUSMOVING)>0;
|
||||
@@ -935,9 +896,7 @@ static int DMC2280Status(void *pData){
|
||||
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))
|
||||
if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply))
|
||||
return HWFault;
|
||||
sscanf(reply, "%d", &servoLoopStatus);
|
||||
if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) {
|
||||
@@ -946,9 +905,7 @@ static int DMC2280Status(void *pData){
|
||||
return HWFault;
|
||||
}
|
||||
snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel);
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return HWFault;
|
||||
if (FAILURE == DMC2280Receive(self, reply))
|
||||
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
|
||||
return HWFault;
|
||||
sscanf(reply, "%d", &should_fixpos);
|
||||
if (should_fixpos == SHOULD_FIXPOS) {
|
||||
@@ -991,15 +948,11 @@ static int DMC2280Status(void *pData){
|
||||
if (self->motOffDelay > 0 ) {
|
||||
NetWatchRegisterTimer(&self->motor_timer,
|
||||
self->motOffDelay,
|
||||
motor_callback, self);
|
||||
motor_timeout, self);
|
||||
return HWIdle;
|
||||
}
|
||||
if (FAILURE == DMC2280Send(self, "FTUBE=0"))
|
||||
if (!DMC_AirPads(self, 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) {
|
||||
@@ -1009,7 +962,7 @@ static int DMC2280Status(void *pData){
|
||||
#else
|
||||
NetWatchRegisterTimer(&self->motor_timer,
|
||||
self->motOffDelay,
|
||||
motor_callback, self);
|
||||
motor_timeout, self);
|
||||
return HWIdle;
|
||||
#endif
|
||||
} else {
|
||||
@@ -1053,50 +1006,50 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
|
||||
strncpy(error, "Command Timeout", (size_t)errLen);
|
||||
break;
|
||||
case BADADR:
|
||||
strncpy(error,"Bad address",(size_t)errLen);
|
||||
strncpy(error, "Bad address", (size_t)errLen);
|
||||
break;
|
||||
case BADBSY:
|
||||
strncpy(error,"Motor still busy",(size_t)errLen);
|
||||
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);
|
||||
strncpy(error, "Bad parameter", (size_t)errLen);
|
||||
break;
|
||||
case BADUNKNOWN:
|
||||
strncpy(error,"Unknown error condition",(size_t)errLen);
|
||||
strncpy(error, "Unknown error condition", (size_t)errLen);
|
||||
break;
|
||||
case BADSTP:
|
||||
strncpy(error,"Motor is stopped",(size_t)errLen);
|
||||
strncpy(error, "Motor is stopped", (size_t)errLen);
|
||||
break;
|
||||
case BADEMERG:
|
||||
strncpy(error,"Emergency stop is engaged",(size_t)errLen);
|
||||
strncpy(error, "Emergency stop is engaged", (size_t)errLen);
|
||||
break;
|
||||
case BGFAIL:
|
||||
strncpy(error,"Begin not possible due to limit switch",(size_t)errLen);
|
||||
strncpy(error, "Begin not possible due to limit switch", (size_t)errLen);
|
||||
break;
|
||||
case RVRSLIM:
|
||||
strncpy(error,"Crashed into limit switch",(size_t)errLen);
|
||||
strncpy(error, "Crashed into limit switch", (size_t)errLen);
|
||||
break;
|
||||
case FWDLIM:
|
||||
strncpy(error,"Crashed into limit switch",(size_t)errLen);
|
||||
strncpy(error, "Crashed into limit switch", (size_t)errLen);
|
||||
break;
|
||||
case POSFAULT:
|
||||
strncpy(error,"Positioning fault detected",(size_t)errLen);
|
||||
strncpy(error, "Positioning fault detected", (size_t)errLen);
|
||||
break;
|
||||
case BADCUSHION:
|
||||
strncpy(error,"Air cushion problem",(size_t)errLen);
|
||||
strncpy(error, "Air cushion problem", (size_t)errLen);
|
||||
break;
|
||||
case ERRORLIM:
|
||||
strncpy(error,"Axis error exceeds error limit",(size_t)errLen);
|
||||
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);
|
||||
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);
|
||||
strncpy(error, "STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen);
|
||||
break;
|
||||
case MOTIONCONTROLOFF:
|
||||
strncpy(error, "MOTION CONTROL SEEMS TO BE DISABLED", (size_t)errLen);
|
||||
@@ -1106,9 +1059,10 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
|
||||
break;
|
||||
default:
|
||||
/* FIXME What's the default */
|
||||
snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
|
||||
break;
|
||||
}
|
||||
strncpy(self->errorMsg,error,(size_t)errLen);
|
||||
strncpy(self->errorMsg, error, (size_t)errLen);
|
||||
}
|
||||
|
||||
/** \brief Attempts to recover from an error. Implements the TryAndFixIt
|
||||
@@ -1173,6 +1127,7 @@ static int DMC2280Halt(void *pData){
|
||||
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return HWFault;
|
||||
/* TODO: FIXME cannot do this until motor stops */
|
||||
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return HWFault;
|
||||
@@ -1596,11 +1551,17 @@ static void KillDMC2280(/*@only@*/void *pData){
|
||||
pDMC2280Driv self = NULL;
|
||||
self = (pDMC2280Driv)pData;
|
||||
assert(self != NULL);
|
||||
if (self->name)
|
||||
if (self->name) {
|
||||
free(self->name);
|
||||
if (self->errorMsg)
|
||||
self->name = NULL;
|
||||
}
|
||||
if (self->errorMsg) {
|
||||
free(self->errorMsg);
|
||||
free(self);
|
||||
self->errorMsg = NULL;
|
||||
}
|
||||
/* Not required as performed in caller
|
||||
* free(self);
|
||||
*/
|
||||
return;
|
||||
}
|
||||
/*@only@*/ prs232 createRS232(char *host, int iPort);
|
||||
@@ -1660,7 +1621,7 @@ static void KillDMC2280(/*@only@*/void *pData){
|
||||
* - 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){
|
||||
MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
|
||||
pDMC2280Driv pNew = NULL;
|
||||
char *pPtr = NULL;
|
||||
char buffer[132];
|
||||
@@ -1713,7 +1674,6 @@ static void KillDMC2280(/*@only@*/void *pData){
|
||||
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",
|
||||
|
||||
Reference in New Issue
Block a user