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

periodically poll and update the motor position, specifically for hipadaba, to capture motion that is not commanded (e.g. homing slit motors and turning motors by hand)

Browse Source 
r2029 | dcl | 2007-06-27 10:58:42 +1000 (Wed, 27 Jun 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-06-27 10:58:42 +10:00
parent 423e9c65c0
commit d27a9cb9b6
1 changed files with 98 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
site_ansto/motor_dmc2280.c
View File

@@ -152,8 +152,10 @@ struct __MoDriv {
int creep_val;
int preseek; /**< Flag = 1 if current move is a backlash preseek */
int has_fsm; /**< Flag = 1 if motor has finite state machine driver model */
int run_flag; /**< Flag = 1 if RUN command has been issued and deferred */
int driver_status;
StateFunc myState;
StateFunc myPrevState;
int subState;
pNWTimer state_timer; /**< motor state timer */
SConnection *trace;
@@ -162,7 +164,8 @@ struct __MoDriv {
int DMC2280MotionControl = 1; /* defaults to enabled */
#define AIR_POLL_TIMER 1000
#define MOTOR_POLL_TIMER 50
#define MOTOR_POLL_FAST 100
#define MOTOR_POLL_SLOW 1000
#define ON_SETTLE_TIMER 1000
#define MAX_CREEP_STEPS 100
@@ -1108,6 +1111,11 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event);
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event);
static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event);
static int state_msg_callback(pAsyncTxn pCmd);
static int state_tmr_callback(void* ctx, int mode);
static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd);
static char* state_name(StateFunc func) {
if (func == DMCState_Unknown) return "DMCState_Unknown";
@@ -1119,6 +1127,7 @@ static char* state_name(StateFunc func) {
if (func == DMCState_OffTimer) return "DMCState_OffTimer";
if (func == DMCState_AirOff) return "DMCState_AirOff";
if (func == DMCState_MotorOff) return "DMCState_MotorOff";
if (func == DMCState_WaitStatus) return "DMCState_WaitStatus";
return "<unknown_state>";
}
@@ -1196,8 +1205,14 @@ static void change_state(pDMC2280Driv self, StateFunc func) {
if (self->trace)
SCWrite(self->trace, line, eStatus);
}
self->myPrevState = self->myState;
self->myState = func;
self->subState = 0;
if (func == DMCState_Idle) {
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_SLOW,
state_tmr_callback, self);
}
}
static void unhandled_event(pDMC2280Driv self, pEvtEvent event) {
@@ -1339,6 +1354,14 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
char cmd[CMDLEN];
switch (event->event_type) {
case eTimerEvent:
snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
self->axisLabel,
self->axisLabel,
self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
change_state(self, DMCState_WaitStatus);
return;
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
@@ -1397,10 +1420,15 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
} while (0);
break;
case eCommandEvent:
if (self->state_timer) {
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
}
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
self->driver_status = HWBusy;
snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
@@ -1562,7 +1590,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'B') { /* BG */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_TIMER,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
@@ -1600,7 +1628,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
return;
}
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_TIMER,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
@@ -1717,7 +1745,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
state_tmr_callback, self);
else
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_TIMER,
MOTOR_POLL_FAST,
state_tmr_callback, self);
self->driver_status = HWIdle;
change_state(self, DMCState_OffTimer);
@@ -1762,7 +1790,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'S') { /* ST */
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_TIMER,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
@@ -1774,7 +1802,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
iFlags = self->currFlags;
if (iFlags & STATUSMOVING) {
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_TIMER,
MOTOR_POLL_FAST,
state_tmr_callback, self);
return;
}
@@ -1930,6 +1958,66 @@ static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event) {
self->errorCode = STATEERROR;
}
static void DMCState_WaitStatus(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eTimerEvent:
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
if (pCmd->out_buf[0] == 'M') { /* MG _TD%c,_TP%c,_TS%c */
int iRet;
iRet = set_currMotion(self, pCmd->inp_buf);
if (iRet == 0)
break;
change_state(self, DMCState_Idle);
if (self->run_flag) {
self->run_flag = 0;
state_cmd_execute(self, CMD_RUN);
}
else {
float fDelta;
if (self->abs_encoder)
fDelta = fabs(self->currCounts - self->lastCounts);
else
fDelta = fabs(self->currSteps - self->lastSteps);
if (fDelta > 10) {
SConnection *pDumCon;
MotCallback sCall;
set_lastMotion(self, self->currSteps, self->currCounts);
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
pDumCon = SCCreateDummyConnection(pServ->pSics);
MotorGetSoftPosition(self->pMot,pDumCon,&sCall.fVal);
SCDeleteConnection(pDumCon);
sCall.pName = self->pMot->name;
InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall);
InvokeCallBack(self->pMot->pCall, MOTEND, &sCall);
}
}
return;
}
} while (0);
break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* TODO: handle run command */
self->driver_status = HWBusy;
self->run_flag = 1;
return;
case CMD_HALT:
/* TODO: handle halt command */
return;
}
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event);
self->errorCode = STATEERROR;
}
/** \brief Reads motor position, implements the GetPosition
* method in the MotorDriver interface.
*
@@ -1949,7 +2037,7 @@ static int DMC2280GetPos(void *pData, float *fPos){
self = (pDMC2280Driv)pData;
assert(self != NULL);
#if 0
#if 1
if (self->has_fsm) {
if (self->myState == DMCState_Unknown)
SicsWait(1);
@@ -2019,6 +2107,7 @@ static int DMC2280Run(void *pData,float fValue){
self->fTarget = fValue;
if (self->has_fsm) {
/* TODO: FIXME and handle RUN command everywhere */
while (self->myState == DMCState_AirOff
|| self->myState == DMCState_MotorOff) {
SicsWait(1);
@@ -2598,6 +2687,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
float oldZero, newZero;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
MotorGetPar(self->pMot,"softzero",&oldZero);
newZero = (self->pMot->fPosition - newValue) + oldZero;
MotorSetPar(self->pMot,pCon,"softzero",newZero);
@@ -3305,6 +3395,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
float oldZero, newZero, currPos, newValue;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
MotorGetPar(self->pMot, "softzero", &oldZero);
if (argc > 3) {
sscanf(argv[2], "%f", &currPos);