Impliment motor "oscillate" and "status" commands and prefix "driver run"
This commit is contained in:
@ -271,6 +271,13 @@ struct __MoDriv {
|
|||||||
double S_r;
|
double S_r;
|
||||||
bool doTracking;
|
bool doTracking;
|
||||||
TrackEntryBuffer *trackHead, *trackTail;
|
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 */
|
int DMC2280MotionControl = 1; /* defaults to enabled */
|
||||||
@ -361,6 +368,64 @@ static double getMotorParam(pDMC2280Driv self, char *param) {
|
|||||||
return fParam;
|
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) {
|
static void TrackingRelease(pDMC2280Driv self) {
|
||||||
while (self->trackHead) {
|
while (self->trackHead) {
|
||||||
TrackEntryBuffer *victim;
|
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_Moving(pDMC2280Driv self, pEvtEvent event);
|
||||||
static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event);
|
static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event);
|
||||||
static void DMCState_SimpleMove(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_BacklashStart(pDMC2280Driv self, pEvtEvent event);
|
||||||
static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event);
|
static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event);
|
||||||
static void DMCState_CreepStart(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_Moving) return "DMCState_Moving";
|
||||||
if (func == DMCState_StepMove) return "DMCState_StepMove";
|
if (func == DMCState_StepMove) return "DMCState_StepMove";
|
||||||
if (func == DMCState_SimpleMove) return "DMCState_SimpleMove";
|
if (func == DMCState_SimpleMove) return "DMCState_SimpleMove";
|
||||||
|
if (func == DMCState_Oscillate) return "DMCState_Oscillate";
|
||||||
if (func == DMCState_BacklashStart) return "DMCState_BacklashStart";
|
if (func == DMCState_BacklashStart) return "DMCState_BacklashStart";
|
||||||
if (func == DMCState_BacklashCont) return "DMCState_BacklashCont";
|
if (func == DMCState_BacklashCont) return "DMCState_BacklashCont";
|
||||||
if (func == DMCState_CreepStart) return "DMCState_CreepStart";
|
if (func == DMCState_CreepStart) return "DMCState_CreepStart";
|
||||||
@ -2523,6 +2590,10 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
|
|||||||
if (self->trackHead)
|
if (self->trackHead)
|
||||||
TrackingRelease(self);
|
TrackingRelease(self);
|
||||||
self->myNextState = NULL;
|
self->myNextState = NULL;
|
||||||
|
if (self->doOscillate) {
|
||||||
|
change_state(self, DMCState_Oscillate);
|
||||||
|
return;
|
||||||
|
}
|
||||||
if (self->backlash_offset != 0) {
|
if (self->backlash_offset != 0) {
|
||||||
change_state(self, DMCState_BacklashStart);
|
change_state(self, DMCState_BacklashStart);
|
||||||
return;
|
return;
|
||||||
@ -3072,6 +3143,87 @@ static void DMCState_CreepCont(pDMC2280Driv self, pEvtEvent event) {
|
|||||||
return;
|
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
|
* This state moves the motor from its current position to the current destination
|
||||||
* set by PA (or PR) by the caller in myNextState.
|
* 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 */
|
/* Only do it if it is within our hard limits */
|
||||||
if (fValue >= self->fLower && fValue <= self->fUpper) {
|
if (fValue >= self->fLower && fValue <= self->fUpper) {
|
||||||
self->fTarget = fValue;
|
self->fTarget = fValue;
|
||||||
|
self->doOscillate = false;
|
||||||
state_cmd_execute(self, CMD_RUN);
|
state_cmd_execute(self, CMD_RUN);
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
@ -5274,16 +5427,101 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|||||||
}
|
}
|
||||||
return 1;
|
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) {
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
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) {
|
if (argc > 2) {
|
||||||
float fLower, fUpper, fZero, fSign, fFixed, fLim;
|
float fLower, fUpper, fZero, fSign, fFixed, fLim;
|
||||||
double fSoft;
|
double fSoft;
|
||||||
char *endPtr;
|
char *endPtr;
|
||||||
errno = 0;
|
errno = 0;
|
||||||
fSoft = strtod(argv[2], &endPtr);
|
fSoft = strtod(argv[3], &endPtr);
|
||||||
if (endPtr == argv[2] || errno == ERANGE) {
|
if (endPtr == argv[3] || errno == ERANGE) {
|
||||||
self->errorCode = BADPAR;
|
self->errorCode = BADPAR;
|
||||||
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]);
|
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]);
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
/* Reject Infinity and Not a Number (NaN) */
|
/* Reject Infinity and Not a Number (NaN) */
|
||||||
@ -5293,7 +5531,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
self->errorCode = BADPAR;
|
self->errorCode = BADPAR;
|
||||||
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]);
|
SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]);
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
MotorGetPar(self->pMot, "softlowerlim", &fLower);
|
MotorGetPar(self->pMot, "softlowerlim", &fLower);
|
||||||
@ -5322,6 +5560,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|||||||
} else {
|
} else {
|
||||||
self->fTarget = fHard;
|
self->fTarget = fHard;
|
||||||
SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard);
|
SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard);
|
||||||
|
self->doOscillate = false;
|
||||||
state_cmd_execute(self, CMD_RUN);
|
state_cmd_execute(self, CMD_RUN);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -5356,6 +5595,36 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|||||||
}
|
}
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
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) {
|
else if(strcasecmp("halt", argv[1]) == 0) {
|
||||||
state_cmd_execute(self, CMD_HALT);
|
state_cmd_execute(self, CMD_HALT);
|
||||||
return 1;
|
return 1;
|
||||||
|
Reference in New Issue
Block a user