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

Refactoring, reduce duplicate code, eliminate obsolete code.

Browse Source 
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
2007-03-23 18:06:20 +11:00
parent fd8618b453
commit 082355318f
1 changed files with 287 additions and 327 deletions
Download Patch File Download Diff File Expand all files Collapse all files

578
site_ansto/motor_dmc2280.c
View File

@@ -32,9 +32,7 @@
#define UNITSLEN 256 #define UNITSLEN 256
#define TEXTPARLEN 1024 #define TEXTPARLEN 1024
void KillRS232(/*@only@*/ void *pData); #define CMDLEN 1024
/*@+incondefs@*/
/** \brief Used to ensure that the getDMCSetting function is called /** \brief Used to ensure that the getDMCSetting function is called
* with valid values. * with valid values.
* \see getDMCSetting * \see getDMCSetting
@@ -81,8 +79,8 @@ typedef struct __MoDriv {
float decel; /**< physical units per second^2 */ float decel; /**< physical units per second^2 */
float maxDecel; /**< physical units per second^2 */ float maxDecel; /**< physical units per second^2 */
char axisLabel; char axisLabel;
char lastCmd[1024]; char lastCmd[CMDLEN];
char dmc2280Error[1024]; char dmc2280Error[CMDLEN];
float home; /**< home position for axis, default=0 */ float home; /**< home position for axis, default=0 */
int motorHome; /**< motor home position in steps */ int motorHome; /**< motor home position in steps */
int noPowerSave; /**< Flag = 1 to leave motors on after a move */ 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 INIT_STR_SIZE 256
#define STR_RESIZE_LENGTH 256 #define STR_RESIZE_LENGTH 256
#define CMDLEN 1024
#define BUFFLEN 512 #define BUFFLEN 512
#define _SAVEPOWER 0 #define _SAVEPOWER 0
@@ -167,7 +164,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
static int DMC2280Halt(void *pData); static int DMC2280Halt(void *pData);
static int DMC2280SetPar(void *pData, SConnection *pCon, static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue); char *name, float newValue);
static int DMC2280Receive(pDMC2280Driv self, /*@out@*/ char *reply);
/** \brief Convert axis speed in physical units to /** \brief Convert axis speed in physical units to
* motor speed in steps/sec. * motor speed in steps/sec.
@@ -234,6 +231,39 @@ static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) {
return FAILURE; 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. /** \brief Sends a DMC2280 command to the motor controller.
* *
* If the command fails it displays the DMC2280 error message to the client * 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 /* First character returned by controller is
'?' for an invalid command or '?' 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) { static int DMC2280Send(pDMC2280Driv self, char *command) {
char cmdValid, reply[256]; char cmdValid, reply[256];
char *GetEMsg = "TC 1"; 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. * \brief Sends a command and waits for a response
* *
* Note: The timeout for readRS232TillTerm is set by DMC2280Connect * \param self motor data
* \param self (rw) provides access to the motor's data structure * \param cmd command to send
* \param *reply (w) the data from the DMC2280. * \param reply space to return response
* \return * \return
* - SUCCESS
* - FAILURE
* \see SUCCESS FAILURE
*/ */
static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) { static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
int i, status, retries=1, dataLen=255; int status;
reply[0] = '\0';
for (i=0; i<retries; i++) { if (FAILURE == DMC2280Send(self, cmd))
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;
} if (FAILURE == DMC2280Receive(self, reply))
}
return FAILURE; return FAILURE;
return OKOK;
} }
/** \brief Record the given posn and timestamp it. /** \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); 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. /** \brief Reads motion.
* *
@@ -431,13 +372,11 @@ static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){
* FAILURE * FAILURE
*/ */
static int readMotion(pDMC2280Driv self, float *steps, float *counts) { static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
char reply[1024]; char reply[CMDLEN];
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel); snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return FAILURE;
if (FAILURE == DMC2280Receive(self, reply))
return FAILURE; return FAILURE;
if (2 != sscanf(reply, "%f %f", steps, counts)) if (2 != sscanf(reply, "%f %f", steps, counts))
@@ -453,13 +392,11 @@ static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
* FAILURE * FAILURE
*/ */
static int readAbsEnc(pDMC2280Driv self, float *pos) { static int readAbsEnc(pDMC2280Driv self, float *pos) {
char reply[1024]; char reply[CMDLEN];
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "TP%c", self->axisLabel); snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return FAILURE;
if (FAILURE == DMC2280Receive(self, reply))
return FAILURE; return FAILURE;
*pos = (float) atoi(reply); *pos = (float) atoi(reply);
@@ -474,13 +411,11 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) {
* FAILURE * FAILURE
*/ */
static int ReadThread(pDMC2280Driv self, int thread, float *pos) { static int ReadThread(pDMC2280Driv self, int thread, float *pos) {
char reply[1024]; char reply[CMDLEN];
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG _XQ%d", thread); snprintf(cmd, CMDLEN, "MG _XQ%d", thread);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return 0;
if (FAILURE == DMC2280Receive(self, reply))
return 0; return 0;
*pos = (float) atoi(reply); *pos = (float) atoi(reply);
@@ -495,13 +430,11 @@ static int ReadThread(pDMC2280Driv self, int thread, float *pos) {
* FAILURE * FAILURE
*/ */
static int readHomeRun(pDMC2280Driv self, float *pos) { static int readHomeRun(pDMC2280Driv self, float *pos) {
char reply[1024]; char reply[CMDLEN];
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG HOMERUN"); snprintf(cmd, CMDLEN, "MG HOMERUN");
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return FAILURE;
if (FAILURE == DMC2280Receive(self, reply))
return FAILURE; return FAILURE;
*pos = (float) atoi(reply); *pos = (float) atoi(reply);
@@ -547,7 +480,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
* */ * */
static int DMC2280GetPos(void *pData, float *fPos){ static int DMC2280GetPos(void *pData, float *fPos){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
char reply[1024]; char reply[CMDLEN];
char cmd[CMDLEN]; char cmd[CMDLEN];
float absEncPos, motorPos; float absEncPos, motorPos;
@@ -560,9 +493,7 @@ static int DMC2280GetPos(void *pData, float *fPos){
*fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home; *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home;
} else { } else {
snprintf(cmd, ERRLEN, "TD%c", self->axisLabel); snprintf(cmd, ERRLEN, "TD%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return HWFault;
if (FAILURE == DMC2280Receive(self, reply))
return HWFault; return HWFault;
motorPos =(float)atof(reply); motorPos =(float)atof(reply);
*fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home; *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home;
@@ -570,136 +501,23 @@ static int DMC2280GetPos(void *pData, float *fPos){
return OKOK; return OKOK;
} }
static int DMC2280Run(void *pData,float fValue); /**
* \brief calculate and issue the motion commands
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 self provides access to a motor's data
* \param fValue target position in physical units, software zeros * \param fValue target position in physical units, software zeros
* have already been applied. * have already been applied.
* \return * \return
* - OKOK request succeeded * - OKOK request succeeded
* - HWFault request failed * - HWFault request failed
* */
static int DMC2280Run(void *pData,float fValue){ */
pDMC2280Driv self = NULL; static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
char axis; char axis;
char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN]; char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN];
int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn; int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn;
float target; 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; axis=self->axisLabel;
motorHome = self->motorHome; motorHome = self->motorHome;
stepsPerX=self->stepsPerX; stepsPerX=self->stepsPerX;
@@ -743,6 +561,174 @@ static int DMC2280Run(void *pData,float fValue){
return OKOK; 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 /** \brief Check if the axis has moved significantly since
* the last check. * the last check.
* *
@@ -762,6 +748,13 @@ static int checkMotion(void *pData) {
pDMC2280Driv self; pDMC2280Driv self;
self = (pDMC2280Driv)pData; self = (pDMC2280Driv)pData;
assert(self != NULL); 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); gettimeofday(&now, NULL);
usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec; usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec;
usec_TimeDiff *= 1000000; usec_TimeDiff *= 1000000;
@@ -775,7 +768,7 @@ static int checkMotion(void *pData) {
if (FAILURE == readMotion(self, &steps, &counts)) if (FAILURE == readMotion(self, &steps, &counts))
return 0; return 0;
/* If not stepping, then not blocked */ /* 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 */ /* just update the timestamp */
set_lastMotion(pData, self->lastSteps, self->lastCounts); set_lastMotion(pData, self->lastSteps, self->lastCounts);
return 1; return 1;
@@ -799,48 +792,18 @@ static int checkMotion(void *pData) {
set_lastMotion(pData, steps, counts); set_lastMotion(pData, steps, counts);
return 1; return 1;
} else { } 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); set_lastMotion(pData, steps, counts);
return 1; return 1;
} }
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, /** \brief Returns the motor status while it's moving,
* implements the GetStatus method in the MotorDriver interface. * implements the GetStatus method in the MotorDriver interface.
@@ -857,9 +820,9 @@ static int DMC2280Status(void *pData){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
char cmd[CMDLEN]; char cmd[CMDLEN];
int switches; int switches;
char switchesAscii[10]; char switchesAscii[CMDLEN];
#ifdef BACKLASHFIX #ifdef BACKLASHFIX
char reply[256]; char reply[CMDLEN];
int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus; int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus;
int SHOULD_FIXPOS=1, should_fixpos; int SHOULD_FIXPOS=1, should_fixpos;
#endif #endif
@@ -880,9 +843,7 @@ static int DMC2280Status(void *pData){
/* Get status of switches /* Get status of switches
* see TS (Tell Switches) in Galil manc2xx.pdf */ * see TS (Tell Switches) in Galil manc2xx.pdf */
snprintf(cmd, CMDLEN, "TS%c", self->axisLabel); snprintf(cmd, CMDLEN, "TS%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280SendReceive(self, cmd, switchesAscii))
return HWFault;
if (FAILURE == DMC2280Receive(self, switchesAscii))
return HWFault; return HWFault;
sscanf(switchesAscii, "%d", &switches); sscanf(switchesAscii, "%d", &switches);
moving = (switches & STATUSMOVING)>0; moving = (switches & STATUSMOVING)>0;
@@ -935,9 +896,7 @@ static int DMC2280Status(void *pData){
if (self->abs_endcoder == 1) { if (self->abs_endcoder == 1) {
/* Make sure that the servo loop is closed by checking if /* Make sure that the servo loop is closed by checking if
* the CLSLOOP thread is running on the controller.*/ * the CLSLOOP thread is running on the controller.*/
if (FAILURE == DMC2280Send(self, "MG _XQ1")) if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply))
return HWFault;
if (FAILURE == DMC2280Receive(self, reply))
return HWFault; return HWFault;
sscanf(reply, "%d", &servoLoopStatus); sscanf(reply, "%d", &servoLoopStatus);
if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) { if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) {
@@ -946,9 +905,7 @@ static int DMC2280Status(void *pData){
return HWFault; return HWFault;
} }
snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel); snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280SendReceive(self, cmd, reply))
return HWFault;
if (FAILURE == DMC2280Receive(self, reply))
return HWFault; return HWFault;
sscanf(reply, "%d", &should_fixpos); sscanf(reply, "%d", &should_fixpos);
if (should_fixpos == SHOULD_FIXPOS) { if (should_fixpos == SHOULD_FIXPOS) {
@@ -991,15 +948,11 @@ static int DMC2280Status(void *pData){
if (self->motOffDelay > 0 ) { if (self->motOffDelay > 0 ) {
NetWatchRegisterTimer(&self->motor_timer, NetWatchRegisterTimer(&self->motor_timer,
self->motOffDelay, self->motOffDelay,
motor_callback, self); motor_timeout, self);
return HWIdle; return HWIdle;
} }
if (FAILURE == DMC2280Send(self, "FTUBE=0")) if (!DMC_AirPads(self, 0))
return HWFault; return HWFault;
self->airpad_state = AIRPADS_LOWER;
self->airpad_counter = 10;
NetWatchRegisterTimer(&self->airpad_timer, 1000,
airpad_callback, self);
return HWIdle; return HWIdle;
} }
if (self->noPowerSave == _SAVEPOWER) { if (self->noPowerSave == _SAVEPOWER) {
@@ -1009,7 +962,7 @@ static int DMC2280Status(void *pData){
#else #else
NetWatchRegisterTimer(&self->motor_timer, NetWatchRegisterTimer(&self->motor_timer,
self->motOffDelay, self->motOffDelay,
motor_callback, self); motor_timeout, self);
return HWIdle; return HWIdle;
#endif #endif
} else { } else {
@@ -1106,6 +1059,7 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
break; break;
default: default:
/* FIXME What's the default */ /* FIXME What's the default */
snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
break; break;
} }
strncpy(self->errorMsg, error, (size_t)errLen); strncpy(self->errorMsg, error, (size_t)errLen);
@@ -1173,6 +1127,7 @@ static int DMC2280Halt(void *pData){
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280Send(self, cmd))
return HWFault; return HWFault;
/* TODO: FIXME cannot do this until motor stops */
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280Send(self, cmd))
return HWFault; return HWFault;
@@ -1596,11 +1551,17 @@ static void KillDMC2280(/*@only@*/void *pData){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData; self = (pDMC2280Driv)pData;
assert(self != NULL); assert(self != NULL);
if (self->name) if (self->name) {
free(self->name); free(self->name);
if (self->errorMsg) self->name = NULL;
}
if (self->errorMsg) {
free(self->errorMsg); free(self->errorMsg);
free(self); self->errorMsg = NULL;
}
/* Not required as performed in caller
* free(self);
*/
return; return;
} }
/*@only@*/ prs232 createRS232(char *host, int iPort); /*@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 * - Get the parameter from the parameter array, see PARAMETERS: below
* - If the parameter requires an abs enc then add it after ABSENC: * - 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; pDMC2280Driv pNew = NULL;
char *pPtr = NULL; char *pPtr = NULL;
char buffer[132]; char buffer[132];
@@ -1713,7 +1674,6 @@ static void KillDMC2280(/*@only@*/void *pData){
return NULL; 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)); pNew->name = (char *)malloc(sizeof(char)*(strlen(motor)+1));
if (pNew->name == NULL) { if (pNew->name == NULL) {
(void) SCWrite(pCon,"ERROR: no memory to allocate motor driver", (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",