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

minor, trivial and cosmetic changes not related to state machine implementation

Browse Source 
r1938 | dcl | 2007-05-04 13:47:21 +1000 (Fri, 04 May 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-05-04 13:47:21 +10:00
parent 911c0d8450
commit bbdc2f820d
1 changed files with 75 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
site_ansto/motor_dmc2280.c
View File

@@ -37,12 +37,12 @@
* with valid values.
* \see getDMCSetting
*/
enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration};
typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv;
typedef struct __command Command, *pCommand;
typedef int (*CommandCallback)(pCommand pCmd);
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
@@ -92,7 +92,7 @@ struct __MoDriv {
int motorHome; /**< motor home position in steps */
int noPowerSave; /**< Flag = 1 to leave motors on after a move */
float stepsPerX; /**< steps per physical unit */
int abs_endcoder; /**< Flag = 1 if there is an abs enc */
int abs_encoder; /**< Flag = 1 if there is an abs enc */
int absEncHome; /**< Home position in counts for abs enc */
float cntsPerX; /**< absolute encoder counts per physical unit */
int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */
@@ -119,6 +119,7 @@ struct __MoDriv {
int DMC2280MotionControl = 1; /* defaults to enabled */
#define AIR_POLL_TIMER 1000
#define AIRPADS_DOWN 0
#define AIRPADS_RAISE 1
#define AIRPADS_UP 2
@@ -134,7 +135,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
#define RVRSLIM -8
#define FWDLIM -9
#define POSFAULT -11 // NOT SET
#define BADCUSHION -12 // NOT SET
#define BADCUSHION -12 // Air Cushion Fault
#define ERRORLIM -13
#define IMPOSSIBLE_LIM_SW -14
#define BGFAIL -15 // NOT SET
@@ -144,7 +145,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
#define MOTCMDTMO -19 /* Motor Command Timeout */
/*--------------------------------------------------------------------*/
#define STATUSMOVING 128 /* Motor is moving */
#define STATUSERRORLIMIT 64 /* Number of errorss exceed limit */
#define STATUSERRORLIMIT 64 /* Number of errors exceed limit */
#define STATUSOFF 32 /* Motor off */
#define STATUSFWDLIMIT 8 /* Forward limit switch active */
#define STATUSRVRSLIMIT 4 /* Reverse limit switch active */
@@ -321,7 +322,7 @@ static int DMC_Rx(void* ctx, int rxchar) {
return iRet;
}
static int DMC_SendCmd(pMultiChan unit,
static int DMC_SendCommand(pMultiChan unit,
char* command, int cmd_len,
CommandCallback callback, void* context, int rsp_len)
{
@@ -354,6 +355,15 @@ static int DMC_SendCmd(pMultiChan unit,
return MultiChanEnque(unit, myCmd, DMC_Tx, DMC_Rx);
}
static int DMC_SendCmd(pDMC2280Driv self,
char* command,
CommandCallback callback)
{
return DMC_SendCommand(self->mcc,
command, strlen(command),
callback, self, CMDLEN);
}
static void DMC_Notify(void* context, int event)
{
pDMC2280Driv self = (pDMC2280Driv) context;
@@ -447,7 +457,7 @@ static int DMC_transact(pDMC2280Driv self, void *send, int sendLen,
assert(self);
txn.transReply = reply;
txn.transWait = 1;
DMC_SendCmd(self->mcc,
DMC_SendCommand(self->mcc,
send, sendLen,
TransCallback, &txn, replyLen);
while (txn.transWait == 1)
@@ -460,7 +470,7 @@ static int DMC_transact(pDMC2280Driv self, void *send, int sendLen,
static int DMC2280Queue(pDMC2280Driv self, char *cmd, CommandCallback cb) {
if (cb == NULL)
cb = SendCallback;
return DMC_SendCmd(self->mcc, cmd, strlen(cmd), cb, self, CMDLEN);
return DMC_SendCmd(self, cmd, cb);
}
/** \brief Sends a DMC2280 command to the motor controller.
@@ -704,7 +714,7 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
}
}
if (1 == self->abs_endcoder) {
if (1 == self->abs_encoder) {
absEncHome = self->absEncHome;
cntsPerX = self->cntsPerX;
#if 0
@@ -798,8 +808,10 @@ static int airpad_callback(pCommand pCmd) {
if (resp_len > 0) {
float fReply;
if (self->debug)
SICSLogWrite(resp, eStatus);
if (self->debug) {
SICSLogWrite(pCmd->inp_buf, eStatus);
SICSLogWrite(pCmd->out_buf, eStatus);
}
fReply = (float) atof(resp);
if (self->airpad_state == AIRPADS_RAISE && fReply > 0) {
int iRet;
@@ -838,7 +850,8 @@ static int airpad_timeout(void* ctx, int mode) {
return 0;
}
--self->airpad_counter;
NetWatchRegisterTimer(&self->airpad_timer, 1000,
NetWatchRegisterTimer(&self->airpad_timer,
AIR_POLL_TIMER,
airpad_timeout, self);
if (FAILURE == DMC2280Queue(self, "MG APDONE", airpad_callback)) {
@@ -876,7 +889,8 @@ static int DMC_AirPads(pDMC2280Driv self, int flag) {
return 0;
}
self->airpad_counter = 10;
NetWatchRegisterTimer(&self->airpad_timer, 1000,
NetWatchRegisterTimer(&self->airpad_timer,
AIR_POLL_TIMER,
airpad_timeout, self);
return 1;
}
@@ -913,7 +927,7 @@ static int motoff_timeout(void* context, int mode) {
* - 0 MOTOR BLOCKED, no significant change in position detected.
*/
static int checkMotion(void *pData) {
float precision, steps, counts, ratio_obs, ratio_exp, ratio_cmp;
float steps, counts, ratio_obs, ratio_exp, ratio_cmp;
long int usec_TimeDiff;
struct timeval now;
@@ -921,7 +935,7 @@ static int checkMotion(void *pData) {
self = (pDMC2280Driv)pData;
assert(self != NULL);
/* we can only test if there is an absolute encoder */
if (!self->abs_endcoder)
if (!self->abs_encoder)
return 1;
if (self->time_lastPos_set.tv_sec == 0) {
/* first time - initialise the data */
@@ -937,9 +951,6 @@ static int checkMotion(void *pData) {
usec_TimeDiff -= self->time_lastPos_set.tv_usec;
if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
return 1;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
MotorGetPar(self->pMot,"precision",&precision);
if (FAILURE == readMotion(self, &steps, &counts))
return 0;
/* If not stepping, then not blocked */
@@ -1014,7 +1025,7 @@ static int DMC2280GetPos(void *pData, float *fPos){
reply[0]='\0';
self = (pDMC2280Driv)pData;
assert(self != NULL);
if (1 == self->abs_endcoder) {
if (1 == self->abs_encoder) {
if (readAbsEnc(self, &absEncPos) == FAILURE)
return HWFault;
*fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home;
@@ -1066,9 +1077,7 @@ static int DMC2280Run(void *pData,float fValue){
float currPos;
DMC2280GetPos(pData, &currPos);
self->lastPosition = currPos;
#if 1
self->time_lastPos_set.tv_sec = 0;
#endif
} while (0);
self->fTarget = fValue;
@@ -1172,7 +1181,7 @@ static int DMC2280Status(void *pData){
return HWFault;
}
#ifdef BACKLASHFIX
if (self->abs_endcoder == 1) {
if (self->abs_encoder == 1) {
/* Make sure that the servo loop is closed by checking if
* the CLSLOOP thread is running on the controller.*/
if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply))
@@ -1521,7 +1530,7 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->backlash_offset;
return 1;
}
if (self->abs_endcoder != 0) {
if (self->abs_encoder != 0) {
if (strcasecmp(name,"absenc") == 0) {
if (readAbsEnc(self, fValue) == SUCCESS)
return 1;
@@ -1709,7 +1718,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
/* Invoke Home Run routine in controller,
* managers only */
if(self->abs_endcoder == 0 && strcasecmp(name,"homerun") == 0) {
if(self->abs_encoder == 0 && strcasecmp(name,"homerun") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
@@ -1786,14 +1795,16 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
* \param *name (r) name of motor.
* \param *pCon (r) connection object.
*/
static void DMC2280StrList(void *self, char *name, SConnection *pCon){
static void DMC2280StrList(pDMC2280Driv self, char *name, SConnection *pCon){
char buffer[BUFFLEN];
snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, ((pDMC2280Driv)self)->long_name);
snprintf(buffer, BUFFLEN, "%s.part = %s\n", name, self->part);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, ((pDMC2280Driv)self)->axisLabel);
snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, self->long_name);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, ((pDMC2280Driv)self)->units);
snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, self->axisLabel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, self->units);
SCWrite(pCon, buffer, eStatus);
return;
}
@@ -1802,50 +1813,51 @@ static void DMC2280StrList(void *self, char *name, SConnection *pCon){
* \param *name (r) name of motor.
* \param *pCon (r) connection object.
*/
static void DMC2280List(void *self, char *name, SConnection *pCon){
static void DMC2280List(void *pData, char *name, SConnection *pCon){
pDMC2280Driv self = (pDMC2280Driv) pData;
char buffer[BUFFLEN];
snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, ((pDMC2280Driv)self)->home);
snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->home);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, ((pDMC2280Driv)self)->speed);
snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, self->speed);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, ((pDMC2280Driv)self)->maxSpeed);
snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, self->maxSpeed);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, ((pDMC2280Driv)self)->accel);
snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, self->accel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, ((pDMC2280Driv)self)->maxAccel);
snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, self->maxAccel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, ((pDMC2280Driv)self)->decel);
snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, self->decel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, ((pDMC2280Driv)self)->maxDecel);
snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, self->maxDecel);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, ((pDMC2280Driv)self)->motOffDelay);
snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, self->motOffDelay);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, ((pDMC2280Driv)self)->debug);
snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, self->debug);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, ((pDMC2280Driv)self)->settle);
snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, self->settle);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, ((pDMC2280Driv)self)->blockage_ckInterval);
snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, self->blockage_ckInterval);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, ((pDMC2280Driv)self)->blockage_thresh);
snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, self->blockage_thresh);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, ((pDMC2280Driv)self)->blockage_ratio);
snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, self->blockage_ratio);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, ((pDMC2280Driv)self)->blockage_fail);
snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, self->blockage_fail);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, ((pDMC2280Driv)self)->backlash_offset);
snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, self->backlash_offset);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads);
snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, self->has_airpads);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder);
snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, self->abs_encoder);
SCWrite(pCon, buffer, eStatus);
if (((pDMC2280Driv)self)->abs_endcoder) {
snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, ((pDMC2280Driv)self)->absEncHome);
if (self->abs_encoder) {
snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, ((pDMC2280Driv)self)->cntsPerX);
snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX);
SCWrite(pCon, buffer, eStatus);
}
snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, ((pDMC2280Driv)self)->stepsPerX);
snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX);
SCWrite(pCon, buffer, eStatus);
return;
}
@@ -1987,6 +1999,14 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
pNew->backlash_offset = 0.0;
/* PARAMETERS: Fetch parameter values */
/* Debug: this motor driver logs exchanges */
if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL)
pNew->debug=0;
else {
sscanf(pPtr,"%d",&(pNew->debug));
}
if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) {
strncpy(pNew->long_name, pPtr, sizeof(pNew->long_name));
pNew->long_name[sizeof(pNew->long_name) - 1] = '\0';
@@ -2047,13 +2067,6 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
else
sscanf(pPtr,"%d",&(pNew->motOffDelay));
/* Debug: this motor driver logs exchanges */
if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL)
pNew->debug=0;
else {
sscanf(pPtr,"%d",&(pNew->debug));
}
/* SETTLE: this motor need time to settle */
if ((pPtr=getParam(pCon, interp, params,"settle",_OPTIONAL)) == NULL)
pNew->settle=0;
@@ -2077,15 +2090,15 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
/* 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;
pNew->abs_encoder = 0;
else {
sscanf(pPtr,"%d",&(pNew->abs_endcoder));
sscanf(pPtr,"%d",&(pNew->abs_encoder));
if ((pPtr=getParam(pCon, interp, params,"absenchome",_REQUIRED)) == NULL)
pNew->absEncHome=0;
pNew->absEncHome = 0;
else
sscanf(pPtr,"%d",&(pNew->absEncHome));
if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL)
pNew->cntsPerX=1.0;
pNew->cntsPerX = 1.0;
else
sscanf(pPtr,"%f",&(pNew->cntsPerX));
}
@@ -2150,6 +2163,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
return 1;
}
else if (strcasecmp("list", argv[1]) == 0) {
/* Handle in generic motor driver */
}
else if (strcasecmp("slist", argv[1]) == 0) {
DMC2280StrList(self, argv[0], pCon);
@@ -2176,8 +2190,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
return 1;
}
newZero = (currPos - newValue);
MotorSetPar(self->pMot, pCon, "softzero", newZero);
return 1;
return MotorSetPar(self->pMot, pCon, "softzero", newZero);
}
}
return MotorAction(pCon, pSics, pData, argc, argv);