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 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 "<unknown_state>";
}
@@ -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 */
/* 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;
}
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:
if (self->run_flag == 0) {
self->driver_status = HWIdle;
if (self->motOffDelay)
if (self->motOffDelay) {
NetWatchRegisterTimer(&self->state_timer,
self->motOffDelay,
state_tmr_callback, self);
else
return;
}
}
NetWatchRegisterTimer(&self->state_timer,
MOTOR_POLL_FAST,
0,
state_tmr_callback, self);
return;
case eTimerEvent:
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,6 +2338,7 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
if (self->state_timer)
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
if (self->driver_status == HWIdle)
self->driver_status = HWBusy;
change_state(self, DMCState_MotorOn);
return;
@@ -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,51 +2409,28 @@ 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);
/* 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;
}
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);
self->errorCode = STATEERROR;
change_state(self, DMCState_Error);
@@ -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);