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

Accumulated changes including RUN and HALT command everywhere

Browse Source 
r2298 | dcl | 2008-01-30 13:38:29 +1100 (Wed, 30 Jan 2008) | 2 lines
This commit is contained in:
Douglas Clowes
2008-01-30 13:38:29 +11:00
parent 0fc5f6b303
commit e3a9c18322
1 changed files with 266 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files

422
site_ansto/motor_dmc2280.c
View File

@@ -180,10 +180,12 @@ struct __MoDriv {
int preseek; /**< Flag = 1 if current move is a backlash preseek */ 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 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 run_flag; /**< Flag = 1 if RUN command has been issued and deferred */
/**< Flag = -1 if HALT command has been issued and deferred */
int driver_status; int driver_status;
StateFunc myState; StateFunc myState;
StateFunc myPrevState; StateFunc myPrevState;
int subState; int subState;
bool waitResponse; /**< true is a message sent and we wait for response */
pNWTimer state_timer; /**< motor state timer */ pNWTimer state_timer; /**< motor state timer */
SConnection *trace; SConnection *trace;
int posit_count; int posit_count;
@@ -255,6 +257,12 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
#define LONG_NAME "long_name" #define LONG_NAME "long_name"
#define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval" #define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval"
/* State Machine Events */
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 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);
@@ -738,8 +746,7 @@ static int motCreep(pDMC2280Driv self, double target) {
return target_steps; return target_steps;
} }
static int DMC_Tx(pAsyncProtocol p, pAsyncTxn ctx) static int DMC_Tx(pAsyncProtocol p, pAsyncTxn ctx) {
{
int iRet = 1; int iRet = 1;
pAsyncTxn myCmd = (pAsyncTxn) ctx; pAsyncTxn myCmd = (pAsyncTxn) ctx;
@@ -826,20 +833,17 @@ static int DMC_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
return AQU_POP_CMD; return AQU_POP_CMD;
} }
static int DMC_SendCmd(pDMC2280Driv self, static int DMC_SendReq(pDMC2280Driv self, char* command) {
char* command, self->waitResponse = true;
AsyncTxnHandler callback)
{
return AsyncUnitSendTxn(self->asyncUnit, return AsyncUnitSendTxn(self->asyncUnit,
command, strlen(command), command, strlen(command),
callback, self, CMDLEN); state_msg_callback, self, CMDLEN);
} }
/** /**
* \brief SendCallback is the callback for the general command. * \brief SendCallback is the callback for the general command.
*/ */
static int SendCallback(pAsyncTxn pCmd) static int SendCallback(pAsyncTxn pCmd) {
{
char* cmnd = pCmd->out_buf; char* cmnd = pCmd->out_buf;
char* resp = pCmd->inp_buf; char* resp = pCmd->inp_buf;
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
@@ -879,17 +883,12 @@ static int SendCallback(pAsyncTxn pCmd)
return 0; return 0;
} }
static int DMC2280Queue(pDMC2280Driv self, char *cmd, AsyncTxnHandler cb) {
if (cb == NULL)
cb = SendCallback;
return DMC_SendCmd(self, cmd, cb);
}
/** \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 * This function does not expect or care about the response.
* and writes it to the log file, also sets errorCode field in motor's *
* data structure. * If the command fails it writes the DMC2280 error message 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 self (rw) provides access to the motor's data structure
* \param *command (r) DMC2280 command * \param *command (r) DMC2280 command
@@ -903,8 +902,10 @@ static int DMC2280Queue(pDMC2280Driv self, char *cmd, AsyncTxnHandler cb) {
':' for a valid command with no response ':' for a valid command with no response
' ' for a valid command with a response (':' follows) ' ' for a valid command with a response (':' follows)
*/ */
static int DMC2280Send(pDMC2280Driv self, char *command) { static int DMC_Send(pDMC2280Driv self, char *command) {
return DMC2280Queue(self, command, SendCallback); return AsyncUnitSendTxn(self->asyncUnit,
command, strlen(command),
SendCallback, self, CMDLEN);
} }
/** /**
@@ -915,7 +916,7 @@ static int DMC2280Send(pDMC2280Driv self, char *command) {
* \param reply space to return response * \param reply space to return response
* \return * \return
*/ */
static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) { static int DMC_SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
int status; int status;
int cmd_len = CMDLEN; int cmd_len = CMDLEN;
@@ -1000,7 +1001,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) {
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "TP%c", self->axisLabel); snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
if (FAILURE == DMC2280SendReceive(self, cmd, reply)) if (FAILURE == DMC_SendReceive(self, cmd, reply))
return FAILURE; return FAILURE;
*pos = (double) atol(reply); *pos = (double) atol(reply);
@@ -1019,7 +1020,7 @@ static int readThread(pDMC2280Driv self, int thread, float *pos) {
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG _XQ%d", thread); snprintf(cmd, CMDLEN, "MG _XQ%d", thread);
if (FAILURE == DMC2280SendReceive(self, cmd, reply)) if (FAILURE == DMC_SendReceive(self, cmd, reply))
return 0; return 0;
*pos = (float) atoi(reply); *pos = (float) atoi(reply);
@@ -1038,7 +1039,7 @@ static int readHomeRun(pDMC2280Driv self, float *pos) {
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "MG HOMERUN"); snprintf(cmd, CMDLEN, "MG HOMERUN");
if (FAILURE == DMC2280SendReceive(self, cmd, reply)) if (FAILURE == DMC_SendReceive(self, cmd, reply))
return FAILURE; return FAILURE;
*pos = (float) atoi(reply); *pos = (float) atoi(reply);
@@ -1062,12 +1063,12 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
/* 0 => reset homerun */ /* 0 => reset homerun */
if (newValue < 0.5) { if (newValue < 0.5) {
snprintf(cmd, CMDLEN, "HOMERUN=0"); snprintf(cmd, CMDLEN, "HOMERUN=0");
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC_Send(self, cmd))
return FAILURE; return FAILURE;
return SUCCESS; return SUCCESS;
} }
snprintf(cmd, CMDLEN, "XQ #HOME,7"); snprintf(cmd, CMDLEN, "XQ #HOME,7");
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC_Send(self, cmd))
return FAILURE; return FAILURE;
return SUCCESS; return SUCCESS;
@@ -1193,13 +1194,6 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
return has_var(self, vars, tmp); return has_var(self, vars, tmp);
} }
/* State Machine Events */
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);
/** /**
* \brief request standard state information for the motor and controller * \brief request standard state information for the motor and controller
* *
@@ -1214,13 +1208,13 @@ static int cmdStatus(pDMC2280Driv self) {
(self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel, (self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel,
(self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel, (self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel,
(self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel); (self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel);
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendReq(self, cmd);
} }
static int cmdVars(pDMC2280Driv self) { static int cmdVars(pDMC2280Driv self) {
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "LV"); snprintf(cmd, CMDLEN, "LV");
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendReq(self, cmd);
} }
static int cmdOn(pDMC2280Driv self) { static int cmdOn(pDMC2280Driv self) {
@@ -1234,47 +1228,43 @@ static int cmdOn(pDMC2280Driv self) {
else { else {
snprintf(cmd, CMDLEN, "SH%c", self->axisLabel); snprintf(cmd, CMDLEN, "SH%c", self->axisLabel);
} }
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendReq(self, cmd);
} }
static int cmdPosition(pDMC2280Driv self, int target) static int cmdPosition(pDMC2280Driv self, int target) {
{
char cmd[CMDLEN]; char cmd[CMDLEN];
if (self->has_airpads == 3) if (self->has_airpads == 3)
snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target); snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target);
else else
snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, target); snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, target);
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendReq(self, cmd);
} }
static int cmdBegin(pDMC2280Driv self) static int cmdBegin(pDMC2280Driv self) {
{
char cmd[CMDLEN]; char cmd[CMDLEN];
if (self->has_airpads == 3) if (self->has_airpads == 3)
snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel); snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel);
else else
snprintf(cmd, CMDLEN, "BG%c", self->axisLabel); snprintf(cmd, CMDLEN, "BG%c", self->axisLabel);
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendReq(self, cmd);
} }
static int cmdPoll(pDMC2280Driv self) static int cmdPoll(pDMC2280Driv self) {
{
char cmd[CMDLEN]; char cmd[CMDLEN];
if (self->has_airpads == 2 || self->has_airpads == 3) if (self->has_airpads == 2 || self->has_airpads == 3)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else if (self->has_airpads == 1) else if (self->has_airpads == 1)
snprintf(cmd, CMDLEN, "MG APDONE"); snprintf(cmd, CMDLEN, "MG APDONE");
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendReq(self, cmd);
} }
static int cmdHalt(pDMC2280Driv self) static int cmdHalt(pDMC2280Driv self) {
{
char cmd[CMDLEN]; char cmd[CMDLEN];
if (self->variables & VAR_HLT) if (self->variables & VAR_HLT)
snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel); snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel);
else else
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel); snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendReq(self, cmd);
} }
static int cmdOff(pDMC2280Driv self) { static int cmdOff(pDMC2280Driv self) {
@@ -1288,7 +1278,7 @@ static int cmdOff(pDMC2280Driv self) {
else { else {
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel); snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
} }
return DMC_SendCmd(self, cmd, state_msg_callback); return DMC_SendReq(self, cmd);
} }
static int rspStatus(pDMC2280Driv self, const char* text) { static int rspStatus(pDMC2280Driv self, const char* text) {
@@ -1374,7 +1364,6 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event);
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event); static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Error(pDMC2280Driv self, pEvtEvent event); static void DMCState_Error(pDMC2280Driv self, pEvtEvent event);
static char* state_name(StateFunc func) { static char* state_name(StateFunc func) {
@@ -1386,7 +1375,6 @@ static char* state_name(StateFunc func) {
if (func == DMCState_MotorHalt) return "DMCState_MotorHalt"; if (func == DMCState_MotorHalt) return "DMCState_MotorHalt";
if (func == DMCState_OffTimer) return "DMCState_OffTimer"; if (func == DMCState_OffTimer) return "DMCState_OffTimer";
if (func == DMCState_MotorStop) return "DMCState_MotorStop"; if (func == DMCState_MotorStop) return "DMCState_MotorStop";
if (func == DMCState_MotorOff) return "DMCState_MotorOff";
if (func == DMCState_Error) return "DMCState_Error"; if (func == DMCState_Error) return "DMCState_Error";
return "<unknown_state>"; return "<unknown_state>";
} }
@@ -1501,8 +1489,7 @@ static void handle_event(pDMC2280Driv self, pEvtEvent event) {
self->myState(self, event); self->myState(self, event);
} }
static int state_msg_callback(pAsyncTxn pCmd) static int state_msg_callback(pAsyncTxn pCmd) {
{
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
EvtEvent event; EvtEvent event;
@@ -1522,12 +1509,12 @@ static int state_msg_callback(pAsyncTxn pCmd)
event.event_type = eMessageEvent; event.event_type = eMessageEvent;
event.event.msg.cmd = pCmd; event.event.msg.cmd = pCmd;
} }
self->waitResponse = false;
handle_event(self, &event); handle_event(self, &event);
return 0; return 0;
} }
static int state_tmr_callback(void* ctx, int mode) static int state_tmr_callback(void* ctx, int mode) {
{
pDMC2280Driv self = (pDMC2280Driv) ctx; pDMC2280Driv self = (pDMC2280Driv) ctx;
EvtEvent event; EvtEvent event;
self->state_timer = 0; self->state_timer = 0;
@@ -1549,57 +1536,80 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
int value; int value;
switch (event->event_type) { switch (event->event_type) {
case eTimerEvent:
return;
case eStateEvent: case eStateEvent:
self->run_flag = 0;
self->driver_status = HWIdle;
/* Set speed */ /* Set speed */
value = motSpeed(self, self->speed); value = motSpeed(self, self->speed);
snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value); snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
if (FAILURE == DMC2280Send(self, cmd)) { if (FAILURE == DMC_Send(self, cmd)) {
break; break;
} }
/* Set acceleration */ /* Set acceleration */
value = motAccel(self, self->accel); value = motAccel(self, self->accel);
snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value); snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value);
if (FAILURE == DMC2280Send(self, cmd)) { if (FAILURE == DMC_Send(self, cmd)) {
break; break;
} }
/* Set deceleration */ /* Set deceleration */
value = motDecel(self, self->decel); value = motDecel(self, self->decel);
snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value); snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value);
if (FAILURE == DMC2280Send(self, cmd)) { if (FAILURE == DMC_Send(self, cmd)) {
break; break;
} }
cmdStatus(self); cmdVars(self);
self->subState = 1; self->subState = 1;
return; return;
case eTimerEvent:
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
if (self->pMot) {
self->pMot->fTarget = self->pMot->fPosition = motPosit(self);
change_state(self, DMCState_Idle);
return;
}
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_SLOW,
state_tmr_callback, self);
return;
case eMessageEvent: case eMessageEvent:
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* Status Request */ if (self->subState == 1) { /* Vars */
rspVars(self, pCmd->inp_buf);
cmdStatus(self);
self->subState = 2;
return;
}
if (self->subState == 2) { /* Status Request */
int iRet; int iRet;
iRet = rspStatus(self, pCmd->inp_buf); iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0) if (iRet == 0)
break; break;
set_lastMotion(self, self->currSteps, self->currCounts); set_lastMotion(self, self->currSteps, self->currCounts);
if (self->pMot == NULL) NetWatchRegisterTimer(&self->state_timer,
self->pMot = FindMotor(pServ->pSics, self->name); 0,
if (self->pMot) { state_tmr_callback, self);
self->pMot->fTarget = self->pMot->fPosition = motPosit(self); self->subState = 0;
}
cmdVars(self);
self->subState = 2;
return;
}
if (self->subState == 2) { /* Vars */
rspVars(self, pCmd->inp_buf);
change_state(self, DMCState_Idle);
return; return;
} }
} while (0); } while (0);
break; break;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
self->driver_status = HWBusy;
self->run_flag = 1;
return;
case CMD_HALT:
/* handle halt command */
self->run_flag = -1;
return;
}
break;
case eTimeoutEvent: case eTimeoutEvent:
/* TODO: handle timeout */ /* handle timeout */
change_state(self, DMCState_Unknown);
break; break;
} }
unhandled_event(self, event); unhandled_event(self, event);
@@ -1613,13 +1623,16 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
case eStateEvent: case eStateEvent:
if (self->state_timer) if (self->state_timer)
NetWatchRemoveTimer(self->state_timer); NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
if (self->driver_status == HWBusy)
self->driver_status = HWIdle;
if (self->variables & VAR_RUN) { if (self->variables & VAR_RUN) {
char cmd[CMDLEN]; char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel); snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel);
DMC2280Send(self, cmd); DMC_Send(self, cmd);
} }
NetWatchRegisterTimer(&self->state_timer, NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_SLOW, 0,
state_tmr_callback, self); state_tmr_callback, self);
self->subState = 0; self->subState = 0;
return; return;
@@ -1637,7 +1650,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
if (iRet == 0) if (iRet == 0)
break; break;
/* action deferred RUN command */ /* action deferred RUN command */
if (self->run_flag) { if (self->run_flag == 1) {
self->run_flag = 0; self->run_flag = 0;
change_state(self, DMCState_MotorStart); change_state(self, DMCState_MotorStart);
return; return;
@@ -1662,11 +1675,9 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
case eCommandEvent: case eCommandEvent:
switch (event->event.cmd.cmd_type) { switch (event->event.cmd.cmd_type) {
case CMD_RUN: case CMD_RUN:
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
assert(self->pMot);
self->driver_status = HWBusy; self->driver_status = HWBusy;
if (self->subState == 0) { if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart); change_state(self, DMCState_MotorStart);
} }
else { else {
@@ -1674,17 +1685,20 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
} }
return; return;
case CMD_HALT: case CMD_HALT:
/* handle halt command */ /* we are already halted so just reset run_flag*/
self->run_flag = 0; self->run_flag = 0;
return; return;
} }
break; break;
case eTimeoutEvent: case eTimeoutEvent:
/* TODO handle message timeout, I don't know what to do here, maybe this'll work. ferdi */ /* handle message timeout */
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN); strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO; self->errorCode = MOTCMDTMO;
self->driver_status = HWFault; self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt); NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_SLOW,
state_tmr_callback, self);
self->subState = 0;
return; return;
} }
unhandled_event(self, event); unhandled_event(self, event);
@@ -1696,6 +1710,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) { switch (event->event_type) {
case eStateEvent: case eStateEvent:
self->run_flag = 0;
if (self->state_timer) { if (self->state_timer) {
NetWatchRemoveTimer(self->state_timer); NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0; self->state_timer = 0;
@@ -1751,6 +1766,16 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
self->subState = 2; self->subState = 2;
return; return;
case eMessageEvent: case eMessageEvent:
if (self->run_flag > 0) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
else if (self->run_flag > 0) {
self->run_flag = 0;
change_state(self, DMCState_MotorHalt);
return;
}
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { if (self->subState == 1) {
@@ -1777,7 +1802,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
/* Handle error return */ /* Handle error return */
self->errorCode = RUNERROR; self->errorCode = RUNERROR;
self->driver_status = HWFault; self->driver_status = HWFault;
change_state(self, DMCState_MotorOff); change_state(self, DMCState_MotorStop);
return; return;
} }
/* TODO handle airpad timeout */ /* TODO handle airpad timeout */
@@ -1789,10 +1814,37 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
} }
} while (0); } while (0);
break; break;
/* TODO: handle halt command, can we get a HALT between this and MotorOn? ferdi */ case eCommandEvent:
case eTimeoutEvent: switch (event->event.cmd.cmd_type) {
/* TODO handle message timeout */ case CMD_RUN:
self->driver_status = HWBusy;
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
}
else {
self->run_flag = 1;
}
return;
case CMD_HALT:
/* handle halt command */
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStop);
}
else {
self->run_flag = -1;
}
return;
}
break; break;
case eTimeoutEvent:
/* handle message timeout */
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
change_state(self, DMCState_MotorStop);
return;
} }
unhandled_event(self, event); unhandled_event(self, event);
self->errorCode = STATEERROR; self->errorCode = STATEERROR;
@@ -1812,6 +1864,16 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
self->subState = 1; self->subState = 1;
return; return;
case eMessageEvent: case eMessageEvent:
if (self->run_flag > 0) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
else if (self->run_flag > 0) {
self->run_flag = 0;
change_state(self, DMCState_MotorHalt);
return;
}
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* Status Response */ if (self->subState == 1) { /* Status Response */
@@ -1896,18 +1958,36 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
} }
} while (0); } while (0);
break; break;
/* TODO: handle halt command, Hey Doug is this right? ferdi */
case eCommandEvent: case eCommandEvent:
switch (event->event.cmd.cmd_type) { switch (event->event.cmd.cmd_type) {
case CMD_RUN:
self->driver_status = HWBusy;
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
}
else {
self->run_flag = 1;
}
return;
case CMD_HALT: case CMD_HALT:
/* handle halt command, send message */ /* handle halt command */
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorHalt); change_state(self, DMCState_MotorHalt);
}
else
self->run_flag = -1;
return; return;
} }
break; break;
case eTimeoutEvent: case eTimeoutEvent:
/* TODO handle message timeout */ /* handle message timeout */
break; strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return;
} }
unhandled_event(self, event); unhandled_event(self, event);
self->errorCode = STATEERROR; self->errorCode = STATEERROR;
@@ -1927,6 +2007,10 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
self->subState = 2; self->subState = 2;
return; return;
case eMessageEvent: case eMessageEvent:
if (self->run_flag != 0) {
change_state(self, DMCState_MotorHalt);
return;
}
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 1) { /* BG */ if (self->subState == 1) { /* BG */
@@ -2127,8 +2211,18 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
break; break;
case eCommandEvent: case eCommandEvent:
switch (event->event.cmd.cmd_type) { switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* TODO: FIXME RUN command while running */
if (self->driver_status == HWIdle)
self->driver_status = HWBusy;
self->run_flag = 1;
if (self->waitResponse == false) {
change_state(self, DMCState_MotorHalt);
}
return;
case CMD_HALT: case CMD_HALT:
/* handle halt command, send message */ /* handle halt command, send message */
self->run_flag = -1;
change_state(self, DMCState_MotorHalt); change_state(self, DMCState_MotorHalt);
return; return;
} }
@@ -2178,14 +2272,33 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
state_tmr_callback, self); state_tmr_callback, self);
return; return;
} }
if (self->run_flag == 1)
change_state(self, DMCState_MotorStart);
else
change_state(self, DMCState_MotorStop); change_state(self, DMCState_MotorStop);
return; return;
} }
} while (0); } while (0);
break; break;
case eTimeoutEvent: case eCommandEvent:
/* TODO handle message timeout */ switch (event->event.cmd.cmd_type) {
case CMD_RUN: /* RUN command deferred*/
if (self->driver_status == HWIdle)
self->driver_status = HWBusy;
self->run_flag = 1;
return;
case CMD_HALT: /* HALT command ignored */
self->run_flag = 0;
return;
}
break; break;
case eTimeoutEvent:
/* handle message timeout */
strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
self->driver_status = HWFault;
change_state(self, DMCState_MotorHalt);
return;
} }
unhandled_event(self, event); unhandled_event(self, event);
self->errorCode = STATEERROR; self->errorCode = STATEERROR;
@@ -2196,19 +2309,28 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) { switch (event->event_type) {
case eStateEvent: case eStateEvent:
if (self->run_flag == 0) {
self->driver_status = HWIdle; self->driver_status = HWIdle;
if (self->motOffDelay) if (self->motOffDelay) {
NetWatchRegisterTimer(&self->state_timer, NetWatchRegisterTimer(&self->state_timer,
self->motOffDelay, self->motOffDelay,
state_tmr_callback, self); state_tmr_callback, self);
else return;
}
}
NetWatchRegisterTimer(&self->state_timer, NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST, 0,
state_tmr_callback, self); state_tmr_callback, self);
return; return;
case eTimerEvent: case eTimerEvent:
if (self->run_flag <= 0)
change_state(self, DMCState_MotorStop); change_state(self, DMCState_MotorStop);
else
change_state(self, DMCState_MotorOn);
return; return;
case eMessageEvent:
/* no need to handle message event */
break;
case eCommandEvent: case eCommandEvent:
switch (event->event.cmd.cmd_type) { switch (event->event.cmd.cmd_type) {
case CMD_RUN: case CMD_RUN:
@@ -2216,6 +2338,7 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
if (self->state_timer) if (self->state_timer)
NetWatchRemoveTimer(self->state_timer); NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0; self->state_timer = 0;
if (self->driver_status == HWIdle)
self->driver_status = HWBusy; self->driver_status = HWBusy;
change_state(self, DMCState_MotorOn); change_state(self, DMCState_MotorOn);
return; return;
@@ -2228,6 +2351,9 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
return; return;
} }
return; return;
case eTimeoutEvent:
/* no need to handle message timeout */
break;
} }
unhandled_event(self, event); unhandled_event(self, event);
self->errorCode = STATEERROR; self->errorCode = STATEERROR;
@@ -2238,6 +2364,7 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) { switch (event->event_type) {
case eStateEvent: case eStateEvent:
self->run_flag = 0;
cmdOff(self); cmdOff(self);
return; return;
case eTimerEvent: case eTimerEvent:
@@ -2245,11 +2372,16 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
self->subState = 1; self->subState = 1;
return; return;
case eMessageEvent: case eMessageEvent:
if (self->run_flag == 1) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
do { do {
pAsyncTxn pCmd = event->event.msg.cmd; pAsyncTxn pCmd = event->event.msg.cmd;
if (self->subState == 0) { /* Off command */ if (self->subState == 0) { /* Off command */
if (self->has_airpads == 0) { if (self->has_airpads == 0) {
change_state(self, DMCState_MotorOff); change_state(self, DMCState_Idle);
return; return;
} }
} }
@@ -2257,14 +2389,14 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
int iRet; int iRet;
iRet = rspPoll(self, pCmd->inp_buf); iRet = rspPoll(self, pCmd->inp_buf);
if (iRet == 0) { /* state has changed to OFF */ if (iRet == 0) { /* state has changed to OFF */
change_state(self, DMCState_MotorOff); change_state(self, DMCState_Idle);
return; return;
} }
else if (iRet < 0) { else if (iRet < 0) {
/* Handle error return */ /* Handle error return */
self->errorCode = RUNERROR; self->errorCode = RUNERROR;
self->driver_status = HWFault; self->driver_status = HWFault;
change_state(self, DMCState_MotorOff); change_state(self, DMCState_Idle);
return; return;
} }
/* TODO handle airpad timeout */ /* TODO handle airpad timeout */
@@ -2277,51 +2409,28 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
case eCommandEvent: case eCommandEvent:
switch (event->event.cmd.cmd_type) { switch (event->event.cmd.cmd_type) {
case CMD_RUN: case CMD_RUN:
/* TODO: handle run command */ /* handle run command */
if (self->waitResponse == false) {
self->run_flag = 0;
change_state(self, DMCState_MotorStart);
return;
}
self->run_flag = 1;
return; return;
case CMD_HALT: case CMD_HALT:
/* TODO: handle halt command, can we get a HALT between this and MotorOff? ferdi */ /* ignore it as we are stopped */
self->run_flag = 0;
return; return;
} }
break; break;
case eTimeoutEvent: case eTimeoutEvent:
/* TODO handle message timeout */ /* handle message timeout */
break; strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
} self->errorCode = MOTCMDTMO;
unhandled_event(self, event); self->driver_status = HWFault;
self->errorCode = STATEERROR; change_state(self, DMCState_MotorStop);
change_state(self, DMCState_Error);
return; return;
} }
static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event) {
switch (event->event_type) {
case eStateEvent:
NetWatchRegisterTimer(&self->state_timer,
ON_SETTLE_TIMER,
state_tmr_callback, self);
return;
case eTimerEvent:
case eMessageEvent:
/* progress to IDLE */
if (self->driver_status == HWBusy)
self->driver_status = HWIdle;
change_state(self, DMCState_Idle);
return;
case eCommandEvent:
switch (event->event.cmd.cmd_type) {
case CMD_RUN:
/* TODO: handle run command */
return;
case CMD_HALT:
/* TODO: handle halt command, can we get a HALT between this and Idle? ferdi */
return;
}
break;
case eTimeoutEvent:
/* TODO handle message timeout */
break;
}
unhandled_event(self, event); unhandled_event(self, event);
self->errorCode = STATEERROR; self->errorCode = STATEERROR;
change_state(self, DMCState_Error); change_state(self, DMCState_Error);
@@ -2339,10 +2448,10 @@ static void DMCState_Error(pDMC2280Driv self, pEvtEvent event) {
switch (event->event.cmd.cmd_type) { switch (event->event.cmd.cmd_type) {
case CMD_RUN: case CMD_RUN:
/* TODO: handle run command */ /* TODO: handle run command */
return; break;
case CMD_HALT: case CMD_HALT:
/* TODO: handle halt command */ /* TODO: handle halt command */
return; break;
} }
break; break;
case eTimeoutEvent: case eTimeoutEvent:
@@ -2421,16 +2530,10 @@ static int DMC2280Run(void *pData,float fValue){
self->fTarget = fValue; self->fTarget = fValue;
/* TODO: FIXME and handle RUN command everywhere */
while (self->myState == DMCState_MotorStop
|| self->myState == DMCState_MotorOff) {
SicsWait(1);
}
state_cmd_execute(self, CMD_RUN); state_cmd_execute(self, CMD_RUN);
return 1; return 1;
} }
/** \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.
* *
@@ -2608,10 +2711,6 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
NetWatchRemoveTimer(self->state_timer); NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0; self->state_timer = 0;
change_state(self, DMCState_Unknown); change_state(self, DMCState_Unknown);
/* Schedule a timer event as soon as possible */
NetWatchRegisterTimer(&self->state_timer,
0,
state_tmr_callback, self);
} }
return MOTFAIL; return MOTFAIL;
default: default:
@@ -3009,7 +3108,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
} }
self->speed = newValue; self->speed = newValue;
snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self, self->speed)); snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self, self->speed));
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC_Send(self, cmd))
return 0; /* FIXME should signal a HWFault */ return 0; /* FIXME should signal a HWFault */
return 1; return 1;
} }
@@ -3028,7 +3127,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
} }
self->accel = newValue; self->accel = newValue;
snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel)); snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel));
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC_Send(self, cmd))
return 0; /* FIXME should signal a HWFault */ return 0; /* FIXME should signal a HWFault */
return 1; return 1;
} }
@@ -3047,7 +3146,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
} }
self->decel = newValue; self->decel = newValue;
snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel)); snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel));
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC_Send(self, cmd))
return 0; /* FIXME should signal a HWFault */ return 0; /* FIXME should signal a HWFault */
return 1; return 1;
} }
@@ -3158,8 +3257,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
return; return;
} }
static void DMC_Notify(void* context, int event) static void DMC_Notify(void* context, int event) {
{
pDMC2280Driv self = (pDMC2280Driv) context; pDMC2280Driv self = (pDMC2280Driv) context;
char line[132]; char line[132];
@@ -3179,10 +3277,6 @@ static void DMC_Notify(void* context, int event)
NetWatchRemoveTimer(self->state_timer); NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0; self->state_timer = 0;
change_state(self, DMCState_Unknown); change_state(self, DMCState_Unknown);
/* Schedule a timer event as soon as possible */
NetWatchRegisterTimer(&self->state_timer,
0,
state_tmr_callback, self);
} }
break; break;
} }
@@ -3536,8 +3630,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
} }
int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[]) int argc, char *argv[]) {
{
pMotor pm = (pMotor) pData; pMotor pm = (pMotor) pData;
pDMC2280Driv self = (pDMC2280Driv) pm->pDriver; pDMC2280Driv self = (pDMC2280Driv) pm->pDriver;
@@ -3566,7 +3659,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
} }
idx += j; idx += j;
} }
DMC2280SendReceive(self, cmd, rsp); DMC_SendReceive(self, cmd, rsp);
SCWrite(pCon, rsp, eValue); SCWrite(pCon, rsp, eValue);
return 1; return 1;
} }
@@ -3679,11 +3772,8 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
if (self->state_timer) if (self->state_timer)
NetWatchRemoveTimer(self->state_timer); NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0; self->state_timer = 0;
self->driver_status = HWIdle;
change_state(self, DMCState_Unknown); change_state(self, DMCState_Unknown);
/* Schedule a timer event as soon as possible */
NetWatchRegisterTimer(&self->state_timer,
0,
state_tmr_callback, self);
} }
/* Handle further in generic motor driver */ /* Handle further in generic motor driver */
return MotorAction(pCon, pSics, pData, argc, argv); return MotorAction(pCon, pSics, pData, argc, argv);