Impliment motor "oscillate" and "status" commands and prefix "driver run"
This commit is contained in:
Douglas Clowes
@ -271,6 +271,13 @@ struct __MoDriv {
|
||||
double S_r;
|
||||
bool doTracking;
|
||||
TrackEntryBuffer *trackHead, *trackTail;
|
||||
bool doOscillate; /**< flag for motor oscillation */
|
||||
bool oscillate_up; /**< next oscillation move is to high position */
|
||||
bool oscillate_init; /**< initial move is not counted */
|
||||
int oscillate_count; /**< number of moves to make or -1 for continuous */
|
||||
int oscillate_counter; /**< number of moves made - half-oscillations */
|
||||
double oscillate_low; /**< soft position at low end */
|
||||
double oscillate_high; /**< soft position at high end */
|
||||
};
|
||||
|
||||
int DMC2280MotionControl = 1; /* defaults to enabled */
|
||||
@ -361,6 +368,64 @@ static double getMotorParam(pDMC2280Driv self, char *param) {
|
||||
return fParam;
|
||||
}
|
||||
|
||||
static bool parseNumber(const char *str, double *pNumber) {
|
||||
char *endPtr;
|
||||
double fNumber;
|
||||
if (!pNumber)
|
||||
return false;
|
||||
fNumber = strtod(str, &endPtr);
|
||||
if (endPtr == str || errno == ERANGE)
|
||||
return false;
|
||||
/* Reject Infinity and Not a Number (NaN) */
|
||||
switch (fpclassify(fNumber)) {
|
||||
case FP_ZERO:
|
||||
case FP_NORMAL:
|
||||
break;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
*pNumber = fNumber;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool getHardFromSoft(pDMC2280Driv self, SConnection *pCon, double fSoft, double *pHard) {
|
||||
float fLower, fUpper, fZero, fSign, fLim;
|
||||
double fHard;
|
||||
|
||||
MotorGetPar(self->pMot, "softlowerlim", &fLower);
|
||||
MotorGetPar(self->pMot, "softupperlim", &fUpper);
|
||||
MotorGetPar(self->pMot, "softzero", &fZero);
|
||||
MotorGetPar(self->pMot, "sign", &fSign);
|
||||
if (fSoft > fUpper) {
|
||||
if (pCon)
|
||||
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
|
||||
fSoft, fUpper, self->name);
|
||||
return false;
|
||||
}
|
||||
if (fSoft < fLower) {
|
||||
if (pCon)
|
||||
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
|
||||
fSoft, fLower, self->name);
|
||||
return false;
|
||||
}
|
||||
fHard = fSoft;
|
||||
fHard += fZero;
|
||||
fHard *= fSign;
|
||||
if (fHard > self->fUpper) {
|
||||
if (pCon)
|
||||
SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s",
|
||||
fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name);
|
||||
return false;
|
||||
} else if (fHard < self->fLower) {
|
||||
if (pCon)
|
||||
SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s",
|
||||
fSoft, self->fLower * fSign + fZero, self->fLower, self->name);
|
||||
return false;
|
||||
}
|
||||
*pHard = fHard;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void TrackingRelease(pDMC2280Driv self) {
|
||||
while (self->trackHead) {
|
||||
TrackEntryBuffer *victim;
|
||||
@ -1745,6 +1810,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event);
|
||||
static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event);
|
||||
static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event);
|
||||
static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event);
|
||||
static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event);
|
||||
static void DMCState_BacklashStart(pDMC2280Driv self, pEvtEvent event);
|
||||
static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event);
|
||||
static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event);
|
||||
@ -1763,6 +1829,7 @@ static char* state_name(StateFunc func) {
|
||||
if (func == DMCState_Moving) return "DMCState_Moving";
|
||||
if (func == DMCState_StepMove) return "DMCState_StepMove";
|
||||
if (func == DMCState_SimpleMove) return "DMCState_SimpleMove";
|
||||
if (func == DMCState_Oscillate) return "DMCState_Oscillate";
|
||||
if (func == DMCState_BacklashStart) return "DMCState_BacklashStart";
|
||||
if (func == DMCState_BacklashCont) return "DMCState_BacklashCont";
|
||||
if (func == DMCState_CreepStart) return "DMCState_CreepStart";
|
||||
@ -2523,6 +2590,10 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
|
||||
if (self->trackHead)
|
||||
TrackingRelease(self);
|
||||
self->myNextState = NULL;
|
||||
if (self->doOscillate) {
|
||||
change_state(self, DMCState_Oscillate);
|
||||
return;
|
||||
}
|
||||
if (self->backlash_offset != 0) {
|
||||
change_state(self, DMCState_BacklashStart);
|
||||
return;
|
||||
@ -3072,6 +3143,87 @@ static void DMCState_CreepCont(pDMC2280Driv self, pEvtEvent event) {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* This state oscillates the motor between two positions
|
||||
*/
|
||||
static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event) {
|
||||
double target;
|
||||
int absolute;
|
||||
|
||||
switch (event->event_type) {
|
||||
case eStateEvent:
|
||||
self->myNextState = NULL;
|
||||
/* handle termination conditions (e.g. count) */
|
||||
if (self->oscillate_count > 0 && self->oscillate_counter >= self->oscillate_count)
|
||||
self->doOscillate = false;
|
||||
if (!self->doOscillate) {
|
||||
change_state(self, DMCState_OffTimer); /* TODO: check this is OK */
|
||||
return;
|
||||
}
|
||||
if (self->oscillate_up) {
|
||||
double fHard;
|
||||
if (getHardFromSoft(self, NULL, self->oscillate_high, &fHard))
|
||||
self->fTarget = fHard;
|
||||
self->oscillate_up = false;
|
||||
} else {
|
||||
double fHard;
|
||||
if (getHardFromSoft(self, NULL, self->oscillate_low, &fHard))
|
||||
self->fTarget = fHard;
|
||||
self->oscillate_up = true;
|
||||
}
|
||||
/* TODO: set timer for optional pause */
|
||||
/* Note: fallthrough is intentional */
|
||||
case eTimerEvent:
|
||||
target = self->fTarget;
|
||||
self->preseek = 0;
|
||||
absolute = motCreep(self, target);
|
||||
self->doSettle = false;
|
||||
cmdPosition(self, absolute);
|
||||
return;
|
||||
case eMessageEvent:
|
||||
if (self->run_flag != 0) {
|
||||
change_state(self, DMCState_MotorHalt);
|
||||
return;
|
||||
}
|
||||
/* set the next state after this move */
|
||||
self->myNextState = DMCState_Oscillate;
|
||||
if (!self->oscillate_init)
|
||||
self->oscillate_counter++;
|
||||
self->oscillate_init = false;
|
||||
change_state(self, DMCState_StepMove);
|
||||
return;
|
||||
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;
|
||||
if (self->waitResponse == false) {
|
||||
change_state(self, DMCState_MotorHalt);
|
||||
}
|
||||
return;
|
||||
}
|
||||
break;
|
||||
case eTimeoutEvent:
|
||||
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;
|
||||
change_state(self, DMCState_Error);
|
||||
return;
|
||||
}
|
||||
/*
|
||||
* This state moves the motor from its current position to the current destination
|
||||
* set by PA (or PR) by the caller in myNextState.
|
||||
@ -3819,6 +3971,7 @@ static int DMC2280Run(void *pData,float fValue){
|
||||
/* Only do it if it is within our hard limits */
|
||||
if (fValue >= self->fLower && fValue <= self->fUpper) {
|
||||
self->fTarget = fValue;
|
||||
self->doOscillate = false;
|
||||
state_cmd_execute(self, CMD_RUN);
|
||||
return 1;
|
||||
}
|
||||
@ -5274,86 +5427,202 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else if(strcasecmp("run", argv[1]) == 0) {
|
||||
else if(strcasecmp("oscillate_count", argv[1]) == 0) {
|
||||
if (argc > 2)
|
||||
self->oscillate_count = atoi(argv[2]);
|
||||
else
|
||||
SCPrintf(pCon, eValue, "%s.oscillate_count = %d", self->name, self->oscillate_count);
|
||||
return 1;
|
||||
}
|
||||
else if (strcasecmp("oscillate_high", argv[1]) == 0) {
|
||||
double fSoft = 0.0;
|
||||
double fHard = 0.0;
|
||||
if (argc > 2) {
|
||||
float fLower, fUpper, fZero, fSign, fFixed, fLim;
|
||||
double fSoft;
|
||||
char *endPtr;
|
||||
errno = 0;
|
||||
fSoft = strtod(argv[2], &endPtr);
|
||||
if (endPtr == argv[2] || errno == ERANGE) {
|
||||
self->errorCode = BADPAR;
|
||||
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]);
|
||||
if (parseNumber(argv[2], &fSoft) &&
|
||||
getHardFromSoft(self, pCon, fSoft, &fHard))
|
||||
self->oscillate_high = fSoft;
|
||||
}
|
||||
else
|
||||
SCPrintf(pCon, eValue, "%s.oscillate_high = %g", self->name, self->oscillate_high);
|
||||
return 1;
|
||||
}
|
||||
else if (strcasecmp("oscillate_low", argv[1]) == 0) {
|
||||
double fSoft = 0.0;
|
||||
double fHard = 0.0;
|
||||
if (argc > 2) {
|
||||
if (parseNumber(argv[2], &fSoft) &&
|
||||
getHardFromSoft(self, pCon, fSoft, &fHard))
|
||||
self->oscillate_low = fSoft;
|
||||
}
|
||||
else
|
||||
SCPrintf(pCon, eValue, "%s.oscillate_low = %g", self->name, self->oscillate_low);
|
||||
return 1;
|
||||
}
|
||||
else if (strcasecmp("oscillate_counter", argv[1]) == 0) {
|
||||
SCPrintf(pCon, eValue, "%s.oscillate_counter = %d", self->name, self->oscillate_counter);
|
||||
}
|
||||
else if(strcasecmp("osc", argv[1]) == 0 || strcasecmp("oscillate", argv[1]) == 0) {
|
||||
if (argc > 2) {
|
||||
if (strcasecmp("list", argv[2]) == 0) {
|
||||
/* TODO: list the osc params */
|
||||
SCPrintf(pCon, eValue, "%s.oscillate_count = %d", self->name, self->oscillate_count);
|
||||
SCPrintf(pCon, eValue, "%s.oscillate_counter = %d", self->name, self->oscillate_counter);
|
||||
SCPrintf(pCon, eValue, "%s.oscillate_low = %g", self->name, self->oscillate_low);
|
||||
SCPrintf(pCon, eValue, "%s.oscillate_high = %g", self->name, self->oscillate_high);
|
||||
return 1;
|
||||
}
|
||||
/* Reject Infinity and Not a Number (NaN) */
|
||||
switch (fpclassify(fSoft)) {
|
||||
case FP_ZERO:
|
||||
case FP_NORMAL:
|
||||
break;
|
||||
default:
|
||||
self->errorCode = BADPAR;
|
||||
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]);
|
||||
if (strcasecmp("start", argv[2]) == 0) {
|
||||
float fFixed;
|
||||
double fHard;
|
||||
MotorGetPar(self->pMot, "fixed", &fFixed);
|
||||
if (fFixed >= 0) {
|
||||
SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name);
|
||||
return 1;
|
||||
}
|
||||
if (!getHardFromSoft(self, pCon, self->oscillate_low, &fHard)) {
|
||||
SCPrintf(pCon, eWarning, "Oscillate low point is in error");
|
||||
return 1;
|
||||
}
|
||||
if (!getHardFromSoft(self, pCon, self->oscillate_high, &fHard)) {
|
||||
SCPrintf(pCon, eWarning, "Oscillate high point is in error");
|
||||
return 1;
|
||||
}
|
||||
if (fabs(self->oscillate_high - motPosit(self)) < fabs(self->oscillate_low - motPosit(self)))
|
||||
self->oscillate_up = true;
|
||||
else
|
||||
self->oscillate_up = false;
|
||||
self->doOscillate = true;
|
||||
self->oscillate_init = true;
|
||||
self->oscillate_counter = 0;
|
||||
state_cmd_execute(self, CMD_RUN);
|
||||
return 1;
|
||||
}
|
||||
MotorGetPar(self->pMot, "softlowerlim", &fLower);
|
||||
MotorGetPar(self->pMot, "softupperlim", &fUpper);
|
||||
MotorGetPar(self->pMot, "softzero", &fZero);
|
||||
MotorGetPar(self->pMot, "sign", &fSign);
|
||||
MotorGetPar(self->pMot, "fixed", &fFixed);
|
||||
if (fFixed >= 0) {
|
||||
SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name);
|
||||
} else if (fSoft > fUpper) {
|
||||
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
|
||||
fSoft, fUpper, self->name);
|
||||
} else if (fSoft < fLower) {
|
||||
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
|
||||
fSoft, fLower, self->name);
|
||||
} else {
|
||||
double fHard = fSoft;
|
||||
fHard += fZero;
|
||||
fHard *= fSign;
|
||||
if (fHard > self->fUpper) {
|
||||
SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s",
|
||||
fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name);
|
||||
} else if (fHard < self->fLower) {
|
||||
SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s",
|
||||
fSoft, self->fLower * fSign + fZero, self->fLower, self->name);
|
||||
if (strcasecmp("stop", argv[2]) == 0) {
|
||||
self->doOscillate = false;
|
||||
return 1;
|
||||
}
|
||||
if (strcasecmp("halt", argv[2]) == 0) {
|
||||
self->doOscillate = false;
|
||||
state_cmd_execute(self, CMD_HALT);
|
||||
return 1;
|
||||
}
|
||||
} else {
|
||||
SCPrintf(pCon, eStatus, "%s.oscillation = %s", self->name, self->doOscillate ? "on" : "off");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else if(strcasecmp("driver", argv[1]) == 0) {
|
||||
if(strcasecmp("run", argv[2]) == 0) {
|
||||
if (argc > 2) {
|
||||
float fLower, fUpper, fZero, fSign, fFixed, fLim;
|
||||
double fSoft;
|
||||
char *endPtr;
|
||||
errno = 0;
|
||||
fSoft = strtod(argv[3], &endPtr);
|
||||
if (endPtr == argv[3] || errno == ERANGE) {
|
||||
self->errorCode = BADPAR;
|
||||
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]);
|
||||
return 1;
|
||||
}
|
||||
/* Reject Infinity and Not a Number (NaN) */
|
||||
switch (fpclassify(fSoft)) {
|
||||
case FP_ZERO:
|
||||
case FP_NORMAL:
|
||||
break;
|
||||
default:
|
||||
self->errorCode = BADPAR;
|
||||
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]);
|
||||
return 1;
|
||||
}
|
||||
MotorGetPar(self->pMot, "softlowerlim", &fLower);
|
||||
MotorGetPar(self->pMot, "softupperlim", &fUpper);
|
||||
MotorGetPar(self->pMot, "softzero", &fZero);
|
||||
MotorGetPar(self->pMot, "sign", &fSign);
|
||||
MotorGetPar(self->pMot, "fixed", &fFixed);
|
||||
if (fFixed >= 0) {
|
||||
SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name);
|
||||
} else if (fSoft > fUpper) {
|
||||
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
|
||||
fSoft, fUpper, self->name);
|
||||
} else if (fSoft < fLower) {
|
||||
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
|
||||
fSoft, fLower, self->name);
|
||||
} else {
|
||||
self->fTarget = fHard;
|
||||
SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard);
|
||||
state_cmd_execute(self, CMD_RUN);
|
||||
double fHard = fSoft;
|
||||
fHard += fZero;
|
||||
fHard *= fSign;
|
||||
if (fHard > self->fUpper) {
|
||||
SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s",
|
||||
fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name);
|
||||
} else if (fHard < self->fLower) {
|
||||
SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s",
|
||||
fSoft, self->fLower * fSign + fZero, self->fLower, self->name);
|
||||
} else {
|
||||
self->fTarget = fHard;
|
||||
SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard);
|
||||
self->doOscillate = false;
|
||||
state_cmd_execute(self, CMD_RUN);
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
char *txt = NULL;
|
||||
if (OKOK == self->driver_status)
|
||||
txt = "OKOK";
|
||||
else if (HWIdle == self->driver_status)
|
||||
txt = "HWIdle";
|
||||
else if (HWBusy == self->driver_status)
|
||||
txt = "HWBusy";
|
||||
else if (HWFault == self->driver_status)
|
||||
txt = "HWFault";
|
||||
else if (HWPosFault == self->driver_status)
|
||||
txt = "HWPosFault";
|
||||
else if (HWCrash == self->driver_status)
|
||||
txt = "HWCrash";
|
||||
else if (NOMEMORY == self->driver_status)
|
||||
txt = "NOMEMORY";
|
||||
else if (HWNoBeam == self->driver_status)
|
||||
txt = "HWNoBeam";
|
||||
else if (HWPause == self->driver_status)
|
||||
txt = "HWPause";
|
||||
else if (HWWarn == self->driver_status)
|
||||
txt = "HWWarn";
|
||||
else if (HWRedo == self->driver_status)
|
||||
txt = "HWRedo";
|
||||
else
|
||||
txt = "HWUnknown";
|
||||
SCPrintf(pCon, eValue, "%s.run = %s", self->name, txt);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else {
|
||||
char *txt = NULL;
|
||||
if (OKOK == self->driver_status)
|
||||
txt = "OKOK";
|
||||
else if (HWIdle == self->driver_status)
|
||||
txt = "HWIdle";
|
||||
else if (HWBusy == self->driver_status)
|
||||
txt = "HWBusy";
|
||||
else if (HWFault == self->driver_status)
|
||||
txt = "HWFault";
|
||||
else if (HWPosFault == self->driver_status)
|
||||
txt = "HWPosFault";
|
||||
else if (HWCrash == self->driver_status)
|
||||
txt = "HWCrash";
|
||||
else if (NOMEMORY == self->driver_status)
|
||||
txt = "NOMEMORY";
|
||||
else if (HWNoBeam == self->driver_status)
|
||||
txt = "HWNoBeam";
|
||||
else if (HWPause == self->driver_status)
|
||||
txt = "HWPause";
|
||||
else if (HWWarn == self->driver_status)
|
||||
txt = "HWWarn";
|
||||
else if (HWRedo == self->driver_status)
|
||||
txt = "HWRedo";
|
||||
else
|
||||
txt = "HWUnknown";
|
||||
SCPrintf(pCon, eValue, "%s.run = %s", self->name, txt);
|
||||
}
|
||||
}
|
||||
else if(strcasecmp("status", argv[1]) == 0) {
|
||||
char *txt = NULL;
|
||||
if (OKOK == self->driver_status)
|
||||
txt = "OKOK";
|
||||
else if (HWIdle == self->driver_status)
|
||||
txt = "HWIdle";
|
||||
else if (HWBusy == self->driver_status)
|
||||
txt = "HWBusy";
|
||||
else if (HWFault == self->driver_status)
|
||||
txt = "HWFault";
|
||||
else if (HWPosFault == self->driver_status)
|
||||
txt = "HWPosFault";
|
||||
else if (HWCrash == self->driver_status)
|
||||
txt = "HWCrash";
|
||||
else if (NOMEMORY == self->driver_status)
|
||||
txt = "NOMEMORY";
|
||||
else if (HWNoBeam == self->driver_status)
|
||||
txt = "HWNoBeam";
|
||||
else if (HWPause == self->driver_status)
|
||||
txt = "HWPause";
|
||||
else if (HWWarn == self->driver_status)
|
||||
txt = "HWWarn";
|
||||
else if (HWRedo == self->driver_status)
|
||||
txt = "HWRedo";
|
||||
else
|
||||
txt = "HWUnknown";
|
||||
SCPrintf(pCon, eValue, "%s.status = %s", self->name, txt);
|
||||
return 1;
|
||||
}
|
||||
else if(strcasecmp("halt", argv[1]) == 0) {
|
||||
|
||||
Reference in New Issue
Block a user