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

A bit more rationalisation

Browse Source 
r2183 | dcl | 2007-10-11 18:00:02 +1000 (Thu, 11 Oct 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-10-11 18:00:02 +10:00
parent 34d8abac97
commit 5d311bf61f
1 changed files with 73 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
site_ansto/motor_dmc2280.c
View File

@@ -78,6 +78,13 @@ struct EvtEvent_s {
} event;
};
#define VAR_REQ 1
#define VAR_RSP 2
#define VAR_RUN 4
#define VAR_DST 8
#define VAR_POS 16
#define VAR_HLT 32
static pAsyncProtocol DMC2280_Protocol = NULL;
/*-----------------------------------------------------------------------
@@ -178,6 +185,7 @@ struct __MoDriv {
int posit_count;
bool posit_check;
unsigned long long *posit_array;
int variables;
};
int DMC2280MotionControl = 1; /* defaults to enabled */
@@ -1068,6 +1076,12 @@ static int cmdStatus(pDMC2280Driv self) {
return DMC_SendCmd(self, cmd, state_msg_callback);
}
static int cmdVars(pDMC2280Driv self) {
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "LV");
return DMC_SendCmd(self, cmd, state_msg_callback);
}
static int cmdOn(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->has_airpads == 1) {
@@ -1102,6 +1116,26 @@ static int cmdBegin(pDMC2280Driv self)
return DMC_SendCmd(self, cmd, state_msg_callback);
}
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);
}
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);
}
static int cmdOff(pDMC2280Driv self) {
char cmd[CMDLEN];
if (self->has_airpads == 1) {
@@ -1142,6 +1176,32 @@ static int rspStatus(pDMC2280Driv self, const char* text) {
return 1;
}
static int rspVars(pDMC2280Driv self, const char* text) {
self->variables = 0;
if (has_var_x(self, text, "REQ"))
self->variables |= VAR_REQ;
if (has_var_x(self, text, "RSP"))
self->variables |= VAR_RSP;
if (has_var_x(self, text, "RUN"))
self->variables |= VAR_RUN;
if (has_var_x(self, text, "DST"))
self->variables |= VAR_DST;
if (has_var_x(self, text, "POS"))
self->variables |= VAR_POS;
if (has_var_x(self, text, "HLT"))
self->variables |= VAR_HLT;
if ((self->variables & VAR_REQ) &&
(self->variables & VAR_RSP)) {
self->has_airpads = 2;
if ((self->variables & VAR_RUN) &&
(self->variables & VAR_DST) &&
(self->variables & VAR_POS)) {
self->has_airpads = 3;
}
}
return 1;
}
/* State Functions */
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event);
@@ -1365,21 +1425,12 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
if (self->pMot) {
self->pMot->fTarget = self->pMot->fPosition = motPosit(self);
}
snprintf(cmd, CMDLEN, "LV");
DMC_SendCmd(self, cmd, state_msg_callback);
cmdVars(self);
self->subState = 2;
return;
}
if (self->subState == 2) { /* LV */
if (has_var_x(self, pCmd->inp_buf, "REQ"))
if (has_var_x(self, pCmd->inp_buf, "RSP")) {
self->has_airpads = 2;
if (has_var_x(self, pCmd->inp_buf, "RUN"))
if (has_var_x(self, pCmd->inp_buf, "DST"))
if (has_var_x(self, pCmd->inp_buf, "POS")) {
self->has_airpads = 3;
}
}
if (self->subState == 2) { /* Vars */
rspVars(self, pCmd->inp_buf);
change_state(self, DMCState_Idle);
return;
}
@@ -1512,11 +1563,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
self->subState = 1;
return;
case eTimerEvent:
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");
DMC_SendCmd(self, cmd, state_msg_callback);
cmdPoll(self);
self->subState = 2;
return;
case eMessageEvent:
@@ -1535,7 +1582,7 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
return;
}
}
else if (self->subState == 2) { /* MG APDONE/RSPx */
else if (self->subState == 2) { /* Poll */
float fReply;
fReply = (float) atof(pCmd->inp_buf);
if (fReply > 0) {
@@ -1582,6 +1629,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
/* TODO AMPERR */
set_lastMotion(self, self->currSteps, self->currCounts);
/* compute position for PA command */
target = self->fTarget;
@@ -1604,7 +1652,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
}
else if (self->backlash_offset < 0) {
/*
* We want to be moving from to low high ,
* We want to be moving from low to high,
* if the target is lower than current
* we must pre-seek to the lower side
*/
@@ -1681,6 +1729,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
iRet = rspStatus(self, pCmd->inp_buf);
if (iRet == 0)
break;
/* TODO AMPERR */
iFlags = self->currFlags;
moving = (iFlags & STATUSMOVING);
if (moving) {
@@ -1765,8 +1814,8 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
}
}
/* limit the maximum number of tries */
if (self->preseek && self->stepCount > 10) {
/* limit the maximum number of tries */
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
@@ -1774,6 +1823,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
SICSLogWrite(line, eStatus);
}
self->preseek = 0;
absolute = motCreep(self, target);
}
else if (self->preseek) {
absolute = motCreep(self, target);
@@ -1876,8 +1926,7 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
if (self->state_timer)
NetWatchRemoveTimer(self->state_timer);
self->state_timer = 0;
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
DMC_SendCmd(self, cmd, state_msg_callback);
cmdHalt(self);
return;
case eTimerEvent:
cmdStatus(self);
@@ -1959,11 +2008,7 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
cmdOff(self);
return;
case eTimerEvent:
if (self->has_airpads == 2 || self->has_airpads == 3)
snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
else
snprintf(cmd, CMDLEN, "MG APDONE");
DMC_SendCmd(self, cmd, state_msg_callback);
cmdPoll(self);
self->subState = 1;
return;
case eMessageEvent:
@@ -1975,7 +2020,7 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
return;
}
}
else if (self->subState == 1) { /* MG APDONE/RSPx */
else if (self->subState == 1) { /* Poll */
float fReply;
fReply = (float) atof(pCmd->inp_buf);
if (fReply == 0) {