From e3a9c183229db301894ab33447576bdb72e82636 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Wed, 30 Jan 2008 13:38:29 +1100 Subject: [PATCH] Accumulated changes including RUN and HALT command everywhere r2298 | dcl | 2008-01-30 13:38:29 +1100 (Wed, 30 Jan 2008) | 2 lines --- site_ansto/motor_dmc2280.c | 442 ++++++++++++++++++++++--------------- 1 file changed, 266 insertions(+), 176 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index f4913d44..8e152e54 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -180,10 +180,12 @@ struct __MoDriv { 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 */ + /**< Flag = -1 if HALT command has been issued and deferred */ int driver_status; StateFunc myState; StateFunc myPrevState; int subState; + bool waitResponse; /**< true is a message sent and we wait for response */ pNWTimer state_timer; /**< motor state timer */ SConnection *trace; int posit_count; @@ -255,6 +257,12 @@ int DMC2280MotionControl = 1; /* defaults to enabled */ #define LONG_NAME "long_name" #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 DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); @@ -738,8 +746,7 @@ static int motCreep(pDMC2280Driv self, double target) { return target_steps; } -static int DMC_Tx(pAsyncProtocol p, pAsyncTxn ctx) -{ +static int DMC_Tx(pAsyncProtocol p, pAsyncTxn ctx) { int iRet = 1; pAsyncTxn myCmd = (pAsyncTxn) ctx; @@ -826,20 +833,17 @@ static int DMC_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) { return AQU_POP_CMD; } -static int DMC_SendCmd(pDMC2280Driv self, - char* command, - AsyncTxnHandler callback) -{ +static int DMC_SendReq(pDMC2280Driv self, char* command) { + self->waitResponse = true; return AsyncUnitSendTxn(self->asyncUnit, command, strlen(command), - callback, self, CMDLEN); + state_msg_callback, self, CMDLEN); } /** * \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* resp = pCmd->inp_buf; pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; @@ -879,17 +883,12 @@ static int SendCallback(pAsyncTxn pCmd) 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. * - * If the command fails it displays the DMC2280 error message to the client - * and writes it to the log file, also sets errorCode field in motor's - * data structure. + * This function does not expect or care about the response. + * + * 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 *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 a response (':' follows) */ -static int DMC2280Send(pDMC2280Driv self, char *command) { - return DMC2280Queue(self, command, SendCallback); +static int DMC_Send(pDMC2280Driv self, char *command) { + 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 * \return */ -static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) { +static int DMC_SendReceive(pDMC2280Driv self, char *cmd, char* reply) { int status; int cmd_len = CMDLEN; @@ -1000,7 +1001,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "TP%c", self->axisLabel); - if (FAILURE == DMC2280SendReceive(self, cmd, reply)) + if (FAILURE == DMC_SendReceive(self, cmd, reply)) return FAILURE; *pos = (double) atol(reply); @@ -1019,7 +1020,7 @@ static int readThread(pDMC2280Driv self, int thread, float *pos) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG _XQ%d", thread); - if (FAILURE == DMC2280SendReceive(self, cmd, reply)) + if (FAILURE == DMC_SendReceive(self, cmd, reply)) return 0; *pos = (float) atoi(reply); @@ -1038,7 +1039,7 @@ static int readHomeRun(pDMC2280Driv self, float *pos) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "MG HOMERUN"); - if (FAILURE == DMC2280SendReceive(self, cmd, reply)) + if (FAILURE == DMC_SendReceive(self, cmd, reply)) return FAILURE; *pos = (float) atoi(reply); @@ -1062,12 +1063,12 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { /* 0 => reset homerun */ if (newValue < 0.5) { snprintf(cmd, CMDLEN, "HOMERUN=0"); - if (FAILURE == DMC2280Send(self, cmd)) + if (FAILURE == DMC_Send(self, cmd)) return FAILURE; return SUCCESS; } snprintf(cmd, CMDLEN, "XQ #HOME,7"); - if (FAILURE == DMC2280Send(self, cmd)) + if (FAILURE == DMC_Send(self, cmd)) return FAILURE; 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); } - -/* 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 * @@ -1214,13 +1208,13 @@ static int cmdStatus(pDMC2280Driv self) { (self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel, (self->variables & VAR_SWI) ? "SWI" : "_TS", 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) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "LV"); - return DMC_SendCmd(self, cmd, state_msg_callback); + return DMC_SendReq(self, cmd); } static int cmdOn(pDMC2280Driv self) { @@ -1234,47 +1228,43 @@ static int cmdOn(pDMC2280Driv self) { else { 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]; if (self->has_airpads == 3) snprintf(cmd, CMDLEN, "DST%c=%d", self->axisLabel, target); else 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]; if (self->has_airpads == 3) snprintf(cmd, CMDLEN, "RUN%c=1", self->axisLabel); else 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]; if (self->has_airpads == 2 || self->has_airpads == 3) snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel); else if (self->has_airpads == 1) 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]; if (self->variables & VAR_HLT) snprintf(cmd, CMDLEN, "HLT%c=1", self->axisLabel); else 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) { @@ -1288,7 +1278,7 @@ static int cmdOff(pDMC2280Driv self) { else { 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) { @@ -1374,7 +1364,6 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event); static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event); static void DMCState_OffTimer(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 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_OffTimer) return "DMCState_OffTimer"; if (func == DMCState_MotorStop) return "DMCState_MotorStop"; - if (func == DMCState_MotorOff) return "DMCState_MotorOff"; if (func == DMCState_Error) return "DMCState_Error"; return ""; } @@ -1501,8 +1489,7 @@ static void handle_event(pDMC2280Driv self, pEvtEvent event) { self->myState(self, event); } -static int state_msg_callback(pAsyncTxn pCmd) -{ +static int state_msg_callback(pAsyncTxn pCmd) { pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx; EvtEvent event; @@ -1522,12 +1509,12 @@ static int state_msg_callback(pAsyncTxn pCmd) event.event_type = eMessageEvent; event.event.msg.cmd = pCmd; } + self->waitResponse = false; handle_event(self, &event); return 0; } -static int state_tmr_callback(void* ctx, int mode) -{ +static int state_tmr_callback(void* ctx, int mode) { pDMC2280Driv self = (pDMC2280Driv) ctx; EvtEvent event; self->state_timer = 0; @@ -1549,57 +1536,80 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { int value; switch (event->event_type) { - case eTimerEvent: - return; case eStateEvent: + self->run_flag = 0; + self->driver_status = HWIdle; /* Set speed */ value = motSpeed(self, self->speed); snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value); - if (FAILURE == DMC2280Send(self, cmd)) { + if (FAILURE == DMC_Send(self, cmd)) { break; } /* Set acceleration */ value = motAccel(self, self->accel); snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value); - if (FAILURE == DMC2280Send(self, cmd)) { + if (FAILURE == DMC_Send(self, cmd)) { break; } /* Set deceleration */ value = motDecel(self, self->decel); snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value); - if (FAILURE == DMC2280Send(self, cmd)) { + if (FAILURE == DMC_Send(self, cmd)) { break; } - cmdStatus(self); + cmdVars(self); self->subState = 1; 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: do { 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; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; set_lastMotion(self, self->currSteps, self->currCounts); - if (self->pMot == NULL) - self->pMot = FindMotor(pServ->pSics, self->name); - if (self->pMot) { - self->pMot->fTarget = self->pMot->fPosition = motPosit(self); - } - cmdVars(self); - self->subState = 2; - return; - } - if (self->subState == 2) { /* Vars */ - rspVars(self, pCmd->inp_buf); - change_state(self, DMCState_Idle); + NetWatchRegisterTimer(&self->state_timer, + 0, + state_tmr_callback, self); + self->subState = 0; return; } } while (0); 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: - /* TODO: handle timeout */ + /* handle timeout */ + change_state(self, DMCState_Unknown); break; } unhandled_event(self, event); @@ -1613,13 +1623,16 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { case eStateEvent: if (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) { char cmd[CMDLEN]; snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel); - DMC2280Send(self, cmd); + DMC_Send(self, cmd); } NetWatchRegisterTimer(&self->state_timer, - MOTOR_POLL_SLOW, + 0, state_tmr_callback, self); self->subState = 0; return; @@ -1637,7 +1650,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { if (iRet == 0) break; /* action deferred RUN command */ - if (self->run_flag) { + if (self->run_flag == 1) { self->run_flag = 0; change_state(self, DMCState_MotorStart); return; @@ -1662,11 +1675,9 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { case eCommandEvent: 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; - if (self->subState == 0) { + if (self->waitResponse == false) { + self->run_flag = 0; change_state(self, DMCState_MotorStart); } else { @@ -1674,17 +1685,20 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { } return; case CMD_HALT: - /* handle halt command */ + /* we are already halted so just reset run_flag*/ self->run_flag = 0; return; } break; 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); self->errorCode = MOTCMDTMO; self->driver_status = HWFault; - change_state(self, DMCState_MotorHalt); + NetWatchRegisterTimer(&self->state_timer, + MOTOR_POLL_SLOW, + state_tmr_callback, self); + self->subState = 0; return; } unhandled_event(self, event); @@ -1696,6 +1710,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: + self->run_flag = 0; if (self->state_timer) { NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; @@ -1751,6 +1766,16 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { self->subState = 2; return; 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 { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { @@ -1777,7 +1802,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { /* Handle error return */ self->errorCode = RUNERROR; self->driver_status = HWFault; - change_state(self, DMCState_MotorOff); + change_state(self, DMCState_MotorStop); return; } /* TODO handle airpad timeout */ @@ -1789,10 +1814,37 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) { } } while (0); break; - /* TODO: handle halt command, can we get a HALT between this and MotorOn? ferdi */ - case eTimeoutEvent: - /* TODO handle message timeout */ + case eCommandEvent: + 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: + /* handle halt command */ + if (self->waitResponse == false) { + self->run_flag = 0; + change_state(self, DMCState_MotorStop); + } + else { + self->run_flag = -1; + } + return; + } 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); self->errorCode = STATEERROR; @@ -1812,6 +1864,16 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { self->subState = 1; return; 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 { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* Status Response */ @@ -1896,18 +1958,36 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) { } } while (0); break; - /* TODO: handle halt command, Hey Doug is this right? ferdi */ case eCommandEvent: 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: - /* handle halt command, send message */ - change_state(self, DMCState_MotorHalt); + /* handle halt command */ + if (self->waitResponse == false) { + self->run_flag = 0; + change_state(self, DMCState_MotorHalt); + } + else + self->run_flag = -1; return; } break; case eTimeoutEvent: - /* TODO handle message timeout */ - break; + /* 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); self->errorCode = STATEERROR; @@ -1927,6 +2007,10 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { self->subState = 2; return; case eMessageEvent: + if (self->run_flag != 0) { + change_state(self, DMCState_MotorHalt); + return; + } do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* BG */ @@ -2127,8 +2211,18 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) { break; case eCommandEvent: 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: /* handle halt command, send message */ + self->run_flag = -1; change_state(self, DMCState_MotorHalt); return; } @@ -2178,14 +2272,33 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) { state_tmr_callback, self); return; } - change_state(self, DMCState_MotorStop); + if (self->run_flag == 1) + change_state(self, DMCState_MotorStart); + else + change_state(self, DMCState_MotorStop); return; } } while (0); break; - case eTimeoutEvent: - /* TODO handle message timeout */ + case eCommandEvent: + 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; + 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); self->errorCode = STATEERROR; @@ -2196,19 +2309,28 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) { static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: - self->driver_status = HWIdle; - if (self->motOffDelay) - NetWatchRegisterTimer(&self->state_timer, - self->motOffDelay, - state_tmr_callback, self); - else - NetWatchRegisterTimer(&self->state_timer, - MOTOR_POLL_FAST, - state_tmr_callback, self); + if (self->run_flag == 0) { + self->driver_status = HWIdle; + if (self->motOffDelay) { + NetWatchRegisterTimer(&self->state_timer, + self->motOffDelay, + state_tmr_callback, self); + return; + } + } + NetWatchRegisterTimer(&self->state_timer, + 0, + state_tmr_callback, self); return; case eTimerEvent: - change_state(self, DMCState_MotorStop); + if (self->run_flag <= 0) + change_state(self, DMCState_MotorStop); + else + change_state(self, DMCState_MotorOn); return; + case eMessageEvent: + /* no need to handle message event */ + break; case eCommandEvent: switch (event->event.cmd.cmd_type) { case CMD_RUN: @@ -2216,7 +2338,8 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; - self->driver_status = HWBusy; + if (self->driver_status == HWIdle) + self->driver_status = HWBusy; change_state(self, DMCState_MotorOn); return; case CMD_HALT: @@ -2228,6 +2351,9 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { return; } return; + case eTimeoutEvent: + /* no need to handle message timeout */ + break; } unhandled_event(self, event); self->errorCode = STATEERROR; @@ -2238,6 +2364,7 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) { static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { switch (event->event_type) { case eStateEvent: + self->run_flag = 0; cmdOff(self); return; case eTimerEvent: @@ -2245,11 +2372,16 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { self->subState = 1; return; case eMessageEvent: + if (self->run_flag == 1) { + self->run_flag = 0; + change_state(self, DMCState_MotorStart); + return; + } do { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 0) { /* Off command */ if (self->has_airpads == 0) { - change_state(self, DMCState_MotorOff); + change_state(self, DMCState_Idle); return; } } @@ -2257,14 +2389,14 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { int iRet; iRet = rspPoll(self, pCmd->inp_buf); if (iRet == 0) { /* state has changed to OFF */ - change_state(self, DMCState_MotorOff); + change_state(self, DMCState_Idle); return; } else if (iRet < 0) { /* Handle error return */ self->errorCode = RUNERROR; self->driver_status = HWFault; - change_state(self, DMCState_MotorOff); + change_state(self, DMCState_Idle); return; } /* TODO handle airpad timeout */ @@ -2277,50 +2409,27 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) { case eCommandEvent: switch (event->event.cmd.cmd_type) { 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; 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; } break; case eTimeoutEvent: - /* TODO handle message timeout */ - break; - } - unhandled_event(self, event); - self->errorCode = STATEERROR; - change_state(self, DMCState_Error); - 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); + /* 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; - 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); self->errorCode = STATEERROR; @@ -2339,10 +2448,10 @@ static void DMCState_Error(pDMC2280Driv self, pEvtEvent event) { switch (event->event.cmd.cmd_type) { case CMD_RUN: /* TODO: handle run command */ - return; + break; case CMD_HALT: /* TODO: handle halt command */ - return; + break; } break; case eTimeoutEvent: @@ -2421,16 +2530,10 @@ static int DMC2280Run(void *pData,float 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); return 1; } - /** \brief Returns the motor status while it's moving, * 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); self->state_timer = 0; change_state(self, DMCState_Unknown); - /* Schedule a timer event as soon as possible */ - NetWatchRegisterTimer(&self->state_timer, - 0, - state_tmr_callback, self); } return MOTFAIL; default: @@ -3009,7 +3108,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, } self->speed = newValue; 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 1; } @@ -3028,7 +3127,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, } self->accel = newValue; 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 1; } @@ -3047,7 +3146,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, } self->decel = newValue; 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 1; } @@ -3158,8 +3257,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){ return; } -static void DMC_Notify(void* context, int event) -{ +static void DMC_Notify(void* context, int event) { pDMC2280Driv self = (pDMC2280Driv) context; char line[132]; @@ -3179,10 +3277,6 @@ static void DMC_Notify(void* context, int event) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; change_state(self, DMCState_Unknown); - /* Schedule a timer event as soon as possible */ - NetWatchRegisterTimer(&self->state_timer, - 0, - state_tmr_callback, self); } break; } @@ -3536,8 +3630,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { } int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, - int argc, char *argv[]) -{ + int argc, char *argv[]) { pMotor pm = (pMotor) pData; pDMC2280Driv self = (pDMC2280Driv) pm->pDriver; @@ -3566,7 +3659,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, } idx += j; } - DMC2280SendReceive(self, cmd, rsp); + DMC_SendReceive(self, cmd, rsp); SCWrite(pCon, rsp, eValue); return 1; } @@ -3679,11 +3772,8 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, if (self->state_timer) NetWatchRemoveTimer(self->state_timer); self->state_timer = 0; + self->driver_status = HWIdle; 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 */ return MotorAction(pCon, pSics, pData, argc, argv);