motor verbose/debug, position_names, hard<->soft and refactor
Make debug mode and verbose mode separate modes Implement position names in the "posit" code as alternate to numbers Refactor to use hard/soft conversion functions
This commit is contained in:
Douglas Clowes
@ -229,6 +229,7 @@ struct __MoDriv {
|
||||
struct timeval time_settle_done; /**< time when settling will be over */
|
||||
bool doSettle; /**< flag to request settle after move, autoreset */
|
||||
int debug;
|
||||
int verbose;
|
||||
int stepCount; /**< number of step operations for this move cycle */
|
||||
float creep_offset; /**< last little bit to creep in units */
|
||||
float creep_precision; /**< how far away we can stop creeping */
|
||||
@ -249,7 +250,9 @@ struct __MoDriv {
|
||||
pNWTimer state_timer; /**< motor state timer */
|
||||
SConnection *trace;
|
||||
int posit_count;
|
||||
int posit_name_count;
|
||||
double* positions;
|
||||
char** position_names;
|
||||
int variables;
|
||||
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
|
||||
int state_trace_idx;
|
||||
@ -397,39 +400,81 @@ static bool parseNumber(const char *str, double *pNumber) {
|
||||
static bool getHardFromSoft(pDMC2280Driv self, SConnection *pCon, double fSoft, double *pHard) {
|
||||
float fLower, fUpper, fZero, fSign, fLim;
|
||||
double fHard;
|
||||
bool result = true;
|
||||
|
||||
MotorGetPar(self->pMot, "softlowerlim", &fLower);
|
||||
MotorGetPar(self->pMot, "softupperlim", &fUpper);
|
||||
MotorGetPar(self->pMot, "softzero", &fZero);
|
||||
MotorGetPar(self->pMot, "sign", &fSign);
|
||||
fHard = fSoft;
|
||||
fHard += fZero;
|
||||
if (fSign != 0.0)
|
||||
fHard *= fSign;
|
||||
if (fSoft > fUpper) {
|
||||
if (pCon)
|
||||
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
|
||||
fSoft, fUpper, self->name);
|
||||
return false;
|
||||
result = false;
|
||||
}
|
||||
if (fSoft < fLower) {
|
||||
if (pCon)
|
||||
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
|
||||
fSoft, fLower, self->name);
|
||||
return false;
|
||||
result = 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;
|
||||
result = 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;
|
||||
result = false;
|
||||
}
|
||||
*pHard = fHard;
|
||||
return true;
|
||||
return result;
|
||||
}
|
||||
|
||||
static bool getSoftFromHard(pDMC2280Driv self, SConnection *pCon, double fHard, double *pSoft) {
|
||||
float fLower, fUpper, fZero, fSign, fLim;
|
||||
double fSoft;
|
||||
bool result = true;
|
||||
|
||||
MotorGetPar(self->pMot, "softlowerlim", &fLower);
|
||||
MotorGetPar(self->pMot, "softupperlim", &fUpper);
|
||||
MotorGetPar(self->pMot, "softzero", &fZero);
|
||||
MotorGetPar(self->pMot, "sign", &fSign);
|
||||
fSoft = fHard;
|
||||
if (fSign != 0.0)
|
||||
fSoft /= fSign;
|
||||
fSoft -= fZero;
|
||||
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);
|
||||
result = 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);
|
||||
result = false;
|
||||
}
|
||||
if (fSoft > fUpper) {
|
||||
if (pCon)
|
||||
SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
|
||||
fSoft, fUpper, self->name);
|
||||
result = false;
|
||||
}
|
||||
if (fSoft < fLower) {
|
||||
if (pCon)
|
||||
SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
|
||||
fSoft, fLower, self->name);
|
||||
result = false;
|
||||
}
|
||||
*pSoft = fSoft;
|
||||
return result;
|
||||
}
|
||||
|
||||
static void TrackingRelease(pDMC2280Driv self) {
|
||||
@ -558,7 +603,7 @@ static long long unit2count(pDMC2280Driv self, double target) {
|
||||
result = (int) (absolute - 0.5);
|
||||
else
|
||||
result = (int) absolute;
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "unit2count Rounding %f to %lld", absolute, result);
|
||||
SICSLogWrite(line, eLog);
|
||||
@ -607,7 +652,19 @@ static double unit2posit(pDMC2280Driv self, double target) {
|
||||
return fPos;
|
||||
}
|
||||
|
||||
static double posit2unit(pDMC2280Driv self, double target) {
|
||||
static double arg2posit(pDMC2280Driv self, const char* arg) {
|
||||
double fPosit = 0.0;
|
||||
int i;
|
||||
if (self->posit_name_count > 0 && self->position_names)
|
||||
for (i = 0; i < self->posit_name_count; ++i)
|
||||
if (strcasecmp(arg, self->position_names[i]) == 0)
|
||||
return (double) i + 1;
|
||||
if (parseNumber(arg, &fPosit))
|
||||
return fPosit;
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
static double posit2hard(pDMC2280Driv self, double target) {
|
||||
double result;
|
||||
int i = ((int) target) - 1;
|
||||
if (i < 0) {
|
||||
@ -623,13 +680,13 @@ static double posit2unit(pDMC2280Driv self, double target) {
|
||||
|
||||
static double posit2soft(pDMC2280Driv self, double target) {
|
||||
double fSoft, fHard;
|
||||
fHard = posit2unit(self, target);
|
||||
fHard = posit2hard(self, target);
|
||||
getSoftFromHard(self, NULL, fHard, &fSoft);
|
||||
return fSoft;
|
||||
}
|
||||
|
||||
static long long posit2count(pDMC2280Driv self, double target) {
|
||||
return unit2count(self, posit2unit(self, target));
|
||||
return unit2count(self, posit2hard(self, target));
|
||||
}
|
||||
|
||||
static double count2posit(pDMC2280Driv self, long long counts) {
|
||||
@ -726,7 +783,7 @@ static int motAbsol(pDMC2280Driv self, double target) {
|
||||
result = (int) (absolute - 0.5);
|
||||
else
|
||||
result = (int) absolute;
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "motAbsol Rounding %f to %d", absolute, result);
|
||||
SICSLogWrite(line, eLog);
|
||||
@ -786,7 +843,7 @@ static int motCreep(pDMC2280Driv self, double target) {
|
||||
* In all repetitive things, there just has to be a limit
|
||||
*/
|
||||
if (abs(self->creep_val) > MAX_CREEP_STEPS) {
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "Motor=%s creep stopped, stepcount = %d",
|
||||
self->name, self->stepCount);
|
||||
@ -805,7 +862,7 @@ static int motCreep(pDMC2280Driv self, double target) {
|
||||
/*
|
||||
* We may still move, calculate if we will and how far
|
||||
*/
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d",
|
||||
self->currSteps, target_steps, offset, self->currSteps + offset);
|
||||
@ -850,7 +907,7 @@ static int motCreep(pDMC2280Driv self, double target) {
|
||||
if (self->creep_val > 0) /* moving down, restore to negative */
|
||||
offset = - offset;
|
||||
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "CREEP: Motor=%s, preseek=%d, creep_val=%d, cur_steps=%d",
|
||||
self->name, self->preseek, self->creep_val, self->currSteps);
|
||||
@ -974,7 +1031,7 @@ static int SendCallback(pAsyncTxn pCmd) {
|
||||
pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
|
||||
|
||||
if (pCmd->txn_status == ATX_TIMEOUT) {
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
SICSLogWrite(pCmd->out_buf, eLog);
|
||||
SICSLogWrite("<TIMEOUT>", eLog);
|
||||
}
|
||||
@ -986,7 +1043,7 @@ static int SendCallback(pAsyncTxn pCmd) {
|
||||
case ':': /* prompt */
|
||||
case ' ': /* leading blank */
|
||||
case '-': /* leading minus sign */
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
SICSLogWrite(cmnd, eLog);
|
||||
SICSLogWrite(resp, eLog);
|
||||
}
|
||||
@ -1002,7 +1059,7 @@ static int SendCallback(pAsyncTxn pCmd) {
|
||||
default:
|
||||
if ((cmnd[0] == 'M' && cmnd[1] == 'G') || (cmnd[0] == 'L' && cmnd[1] == 'V')) {
|
||||
/* MG and LV commands can produce this result */
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
SICSLogWrite(cmnd, eLog);
|
||||
SICSLogWrite(resp, eLog);
|
||||
}
|
||||
@ -1059,7 +1116,7 @@ static int DMC_SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
|
||||
status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len);
|
||||
|
||||
if (status != 1) {
|
||||
if (self->debug)
|
||||
if (self->verbose)
|
||||
SICSLogWrite(cmd, eLog);
|
||||
if (status == -1)
|
||||
self->errorCode = MOTCMDTMO;
|
||||
@ -1070,14 +1127,14 @@ static int DMC_SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
|
||||
|
||||
switch (reply[0]) {
|
||||
case ':': /* prompt */
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
SICSLogWrite(cmd, eLog);
|
||||
SICSLogWrite(reply, eLog);
|
||||
}
|
||||
return SUCCESS;
|
||||
case ' ': /* leading blank */
|
||||
case '-': /* leading minus sign */
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
SICSLogWrite(cmd, eLog);
|
||||
SICSLogWrite(reply, eLog);
|
||||
}
|
||||
@ -1093,7 +1150,7 @@ static int DMC_SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
|
||||
default:
|
||||
if ((cmd[0] == 'M' && cmd[1] == 'G') || (cmd[0] == 'L' && cmd[1] == 'V')) {
|
||||
/* MG and LV commands can produce this result */
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
SICSLogWrite(cmd, eLog);
|
||||
SICSLogWrite(reply, eLog);
|
||||
}
|
||||
@ -1426,7 +1483,7 @@ static int checkMotion(pDMC2280Driv self) {
|
||||
double cmp = ratio_cmp;
|
||||
if (fabs(cmp) > -1.0 && fabs(cmp) < 1.0)
|
||||
cmp = 1.0 / cmp;
|
||||
if (self->debug || bNotYet == false) {
|
||||
if (self->verbose || bNotYet == false) {
|
||||
snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f",
|
||||
self->name, ratio_obs, ratio_exp, cmp);
|
||||
SICSLogWrite(msg, eError);
|
||||
@ -1443,7 +1500,7 @@ static int checkMotion(pDMC2280Driv self) {
|
||||
set_lastMotion(self, self->currSteps, self->currCounts);
|
||||
}
|
||||
} else {
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char msg[132];
|
||||
double cmp = ratio_cmp;
|
||||
if (fabs(cmp) > 0 && fabs(cmp) < 1)
|
||||
@ -1690,7 +1747,7 @@ static int rspStatus(pDMC2280Driv self, const char* text) {
|
||||
else
|
||||
iIOByte = iTIone;
|
||||
|
||||
if ((trace_switches || self->debug) && self->currFlags != iFlags) {
|
||||
if ((trace_switches || self->verbose) && self->currFlags != iFlags) {
|
||||
char line[CMDLEN];
|
||||
char *sw;
|
||||
if ((self->currFlags & (8 + 4)) != (iFlags & (8 + 4))) {
|
||||
@ -2005,7 +2062,7 @@ static void state_trace_ins(pDMC2280Driv self, const char *fmt, ...) {
|
||||
self->state_trace_done[self->state_trace_idx] = 0;
|
||||
if (true)
|
||||
#else
|
||||
if (self->debug || self->trace)
|
||||
if (self->verbose || self->trace)
|
||||
#endif
|
||||
{
|
||||
char format[CMDLEN];
|
||||
@ -2017,7 +2074,7 @@ static void state_trace_ins(pDMC2280Driv self, const char *fmt, ...) {
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(lp, CMDLEN, format, ap);
|
||||
va_end(ap);
|
||||
if (self->debug)
|
||||
if (self->verbose)
|
||||
SICSLogWrite(lp, eLog);
|
||||
if (self->trace)
|
||||
SCWrite(self->trace, lp, eLog);
|
||||
@ -2032,7 +2089,7 @@ static void report_event(pDMC2280Driv self, pEvtEvent event) {
|
||||
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
|
||||
if (true)
|
||||
#else
|
||||
if (self->debug || self->trace)
|
||||
if (self->verbose || self->trace)
|
||||
#endif
|
||||
{
|
||||
char text[CMDLEN];
|
||||
@ -2051,7 +2108,7 @@ static void handle_event(pDMC2280Driv self, pEvtEvent event) {
|
||||
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
|
||||
if (true)
|
||||
#else
|
||||
if (self->debug || self->trace)
|
||||
if (self->verbose || self->trace)
|
||||
#endif
|
||||
{
|
||||
report_event(self, event);
|
||||
@ -2063,7 +2120,7 @@ static void change_state(pDMC2280Driv self, StateFunc func) {
|
||||
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
|
||||
if (true)
|
||||
#else
|
||||
if (self->debug || self->trace)
|
||||
if (self->verbose || self->trace)
|
||||
#endif
|
||||
{
|
||||
state_trace_ins(self, "NewState=%s",
|
||||
@ -2219,7 +2276,7 @@ static int state_snd_callback(pAsyncTxn pCmd) {
|
||||
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
|
||||
if (true)
|
||||
#else
|
||||
if (self->debug || self->trace)
|
||||
if (self->verbose || self->trace)
|
||||
#endif
|
||||
{
|
||||
char text[CMDLEN];
|
||||
@ -2420,7 +2477,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
|
||||
} else {
|
||||
(void) report_motion(self, eReportIdle);
|
||||
}
|
||||
if (trace_switches || self->debug)
|
||||
if (trace_switches || self->verbose)
|
||||
DMC_SetTimer(self, self->motorPollFast);
|
||||
else
|
||||
DMC_SetTimer(self, self->motorPollSlow);
|
||||
@ -2711,7 +2768,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
|
||||
}
|
||||
}
|
||||
}
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n",
|
||||
self->name,
|
||||
@ -2979,7 +3036,7 @@ static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event) {
|
||||
|
||||
if (self->preseek && self->stepCount > 10) {
|
||||
/* limit the maximum number of tries */
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
|
||||
self->name, self->stepCount);
|
||||
@ -3395,13 +3452,13 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
|
||||
DMC_SetTimer(self, self->settle);
|
||||
return;
|
||||
}
|
||||
if (true /*self->debug*/ || true /*self->doStats*/) {
|
||||
if (true /*self->verbose*/ || true /*self->doStats*/) {
|
||||
double units = self->currPosition - self->origPosition;
|
||||
long int steps = self->currSteps - self->origSteps;
|
||||
long int counts = self->currCounts - self->origCounts;
|
||||
double time = DoubleTime() - self->origTime;
|
||||
char line[CMDLEN];
|
||||
if (true /*self->debug*/) {
|
||||
if (true /*self->verbose*/) {
|
||||
snprintf(line, CMDLEN, "Motor=%s stopped: p=%.6f(%.6f-%.6f), u=%.6f,"
|
||||
" s=%ld, c=%ld, s/X=%.6f,"
|
||||
" r=(%.6f-%.6f), t=%.3fS",
|
||||
@ -3594,7 +3651,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
|
||||
|
||||
if (self->preseek && self->stepCount > 10) {
|
||||
/* limit the maximum number of tries */
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
|
||||
self->name, self->stepCount);
|
||||
@ -3605,7 +3662,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
|
||||
}
|
||||
else if (self->preseek) {
|
||||
absolute = motCreep(self, target);
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "Motor=%s unchanged direction: "
|
||||
"preseek=%d, fPreseek=%f, steps=%d, creep_val=%d",
|
||||
@ -3621,7 +3678,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
|
||||
/* change of direction, reset motion check */
|
||||
set_lastMotion(self, self->currSteps, self->currCounts);
|
||||
absolute = motCreep(self, target);
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "Motor=%s changed direction: "
|
||||
"preseek=%d, fPreseek=%f, steps=%d, creep_val=%d",
|
||||
@ -3637,7 +3694,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
|
||||
else
|
||||
absolute = motCreep(self, target);
|
||||
|
||||
if (self->debug && self->preseek) {
|
||||
if (self->verbose && self->preseek) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "Motor=%s, Pos=%f, Preseek=%f, Target=%f\n",
|
||||
self->name,
|
||||
@ -3651,7 +3708,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
|
||||
*/
|
||||
if (self->preseek) {
|
||||
if (absolute == self->currSteps) {
|
||||
if (self->debug) {
|
||||
if (self->verbose) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "Motor=%s motion stopped, absolute = %d",
|
||||
self->name, absolute);
|
||||
@ -3686,7 +3743,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
|
||||
}
|
||||
/* We get here when the motor stops normally
|
||||
*/
|
||||
if (true /*self->debug*/) {
|
||||
if (true /*self->verbose*/) {
|
||||
double units = self->currPosition - self->origPosition;
|
||||
long int steps = self->currSteps - self->origSteps;
|
||||
long int counts = self->currCounts - self->origCounts;
|
||||
@ -4326,6 +4383,10 @@ static int DMC2280GetPar(void *pData, char *name,
|
||||
*fValue = self->debug;
|
||||
return 1;
|
||||
}
|
||||
if(strcasecmp(name,"verbose") == 0) {
|
||||
*fValue = self->verbose;
|
||||
return 1;
|
||||
}
|
||||
if(strcasecmp(name,SETTLE) == 0) {
|
||||
*fValue = self->settle;
|
||||
return 1;
|
||||
@ -4524,6 +4585,16 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
|
||||
}
|
||||
}
|
||||
|
||||
/* Verbose, managers only */
|
||||
if(strcasecmp(name,"verbose") == 0) {
|
||||
if(!SCMatchRights(pCon,usMugger))
|
||||
return 1;
|
||||
else {
|
||||
self->verbose = newValue;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
/* Setttle, managers only */
|
||||
if(strcasecmp(name,SETTLE) == 0) {
|
||||
if(!SCMatchRights(pCon,usMugger))
|
||||
@ -4896,6 +4967,16 @@ static void DMC2280StrList(pDMC2280Driv self, char *name, SConnection *pCon){
|
||||
DMC2280Error(self, &iCode, error, CMDLEN);
|
||||
SCPrintf(pCon, eValue, "%s.error = %d:%s", self->name, iCode, error);
|
||||
}
|
||||
if (self->posit_name_count > 0 && self->position_names) {
|
||||
int i, k = 0;
|
||||
char line[1320];
|
||||
k += snprintf(line, sizeof(line) - k, "%s.position_names =", self->name);
|
||||
for (i = 0; i < self->posit_name_count; ++i) {
|
||||
k += snprintf(&line[k], sizeof(line) - k, " %s",
|
||||
self->position_names[i]);
|
||||
}
|
||||
SCWrite(pCon, line, eValue);
|
||||
}
|
||||
return;
|
||||
}
|
||||
/** \brief List the motor parameters to the client.
|
||||
@ -4919,6 +5000,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
|
||||
SCPrintf(pCon, eValue, "%s.motorPollSlow = %d\n", name, self->motorPollSlow);
|
||||
SCPrintf(pCon, eValue, "%s.airPollTimer = %d\n", name, self->airPollTimer);
|
||||
SCPrintf(pCon, eValue, "%s.Debug = %d\n", name, self->debug);
|
||||
SCPrintf(pCon, eValue, "%s.Verbose = %d\n", name, self->verbose);
|
||||
SCPrintf(pCon, eValue, "%s.Settle = %d\n", name, self->settle);
|
||||
SCPrintf(pCon, eValue, "%s.Blockage_Check_Interval = %f\n", name, self->blockage_ckInterval);
|
||||
SCPrintf(pCon, eValue, "%s.Blockage_Thresh = %f\n", name, self->blockage_thresh);
|
||||
@ -5122,11 +5204,11 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
|
||||
|
||||
/* PARAMETERS: Fetch parameter values */
|
||||
|
||||
/* Debug: this motor driver logs exchanges */
|
||||
if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL)
|
||||
pNew->debug=0;
|
||||
/* Verbose: this motor driver logs exchanges */
|
||||
if ((pPtr=getParam(pCon, interp, params,"verbose",_OPTIONAL)) == NULL)
|
||||
pNew->verbose=0;
|
||||
else {
|
||||
sscanf(pPtr,"%d",&(pNew->debug));
|
||||
sscanf(pPtr,"%d",&(pNew->verbose));
|
||||
}
|
||||
|
||||
pNew->ao_id[0] = '\0';
|
||||
@ -5653,8 +5735,9 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
}
|
||||
else if (strcasecmp("driver", argv[1]) == 0 && strcasecmp("run", argv[2]) == 0) {
|
||||
if (argc > 2) {
|
||||
float fLower, fUpper, fZero, fSign, fFixed, fLim;
|
||||
float fFixed;
|
||||
double fSoft;
|
||||
double fHard;
|
||||
char *endPtr;
|
||||
errno = 0;
|
||||
fSoft = strtod(argv[3], &endPtr);
|
||||
@ -5673,36 +5756,15 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
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 {
|
||||
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;
|
||||
self->doReportMotion = true;
|
||||
state_cmd_execute(self, CMD_RUN);
|
||||
}
|
||||
} else if (getHardFromSoft(self, pCon, fSoft, &fHard)) {
|
||||
self->fTarget = fHard;
|
||||
SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard);
|
||||
self->doOscillate = false;
|
||||
self->doReportMotion = true;
|
||||
state_cmd_execute(self, CMD_RUN);
|
||||
}
|
||||
}
|
||||
else {
|
||||
@ -5955,6 +6017,22 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else if(strcasecmp("hard2soft", argv[1]) == 0) {
|
||||
double fHard, fSoft;
|
||||
if (parseNumber(argv[2], &fHard)) {
|
||||
getSoftFromHard(self, pCon, fHard, &fSoft);
|
||||
SCPrintf(pCon, eValue, "%s.hard2soft = %g", self->name, fSoft);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else if(strcasecmp("soft2hard", argv[1]) == 0) {
|
||||
double fHard, fSoft;
|
||||
if (parseNumber(argv[2], &fSoft)) {
|
||||
getHardFromSoft(self, pCon, fSoft, &fHard);
|
||||
SCPrintf(pCon, eValue, "%s.soft2hard = %g", self->name, fHard);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else if(self->abs_encoder && strcasecmp("unit2count", argv[1]) == 0) {
|
||||
char line[132];
|
||||
if (argc > 2) {
|
||||
@ -5995,7 +6073,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
if (argc == 2) {
|
||||
int i, k = 0;
|
||||
char line[1320];
|
||||
k += snprintf(line, sizeof(line) - k, "%s.positions ", self->name);
|
||||
k += snprintf(line, sizeof(line) - k, "%s.positions =", self->name);
|
||||
for (i = 0; i < self->posit_count; ++i) {
|
||||
k += snprintf(&line[k], sizeof(line) - k, " %f",
|
||||
self->positions[i]);
|
||||
@ -6003,15 +6081,15 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
SCWrite(pCon, line, eValue);
|
||||
return 1;
|
||||
}
|
||||
if (self->posit_count > 0) {
|
||||
self->posit_count = 0;
|
||||
}
|
||||
if (self->positions) {
|
||||
free(self->positions);
|
||||
self->positions = NULL;
|
||||
self->posit_count = 0;
|
||||
}
|
||||
if (argc > 3 && strcasecmp("erase", argv[2]) == 0) {
|
||||
if (self->posit_count > 0) {
|
||||
self->posit_count = 0;
|
||||
}
|
||||
if (argc > 2 && strcasecmp("erase", argv[2]) == 0) {
|
||||
char line[132];
|
||||
snprintf(line, 132, "%s.posit_count = %d", self->name, self->posit_count);
|
||||
SCWrite(pCon, line, eValue);
|
||||
@ -6025,6 +6103,45 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else if(strcasecmp("position_names", argv[1]) == 0) {
|
||||
if (argc == 2) {
|
||||
int i, k = 0;
|
||||
char line[1320];
|
||||
k += snprintf(line, sizeof(line) - k, "%s.position_names =", self->name);
|
||||
for (i = 0; i < self->posit_name_count; ++i) {
|
||||
k += snprintf(&line[k], sizeof(line) - k, " %s",
|
||||
self->position_names[i]);
|
||||
}
|
||||
SCWrite(pCon, line, eValue);
|
||||
return 1;
|
||||
}
|
||||
if (self->position_names) {
|
||||
int i;
|
||||
for (i = 0; i < self->posit_name_count; ++i) {
|
||||
free(self->position_names[i]);
|
||||
self->position_names[i] = NULL;
|
||||
}
|
||||
free(self->position_names);
|
||||
self->position_names = NULL;
|
||||
self->posit_name_count = 0;
|
||||
}
|
||||
if (self->posit_name_count > 0) {
|
||||
self->posit_name_count = 0;
|
||||
}
|
||||
if (argc > 2 && strcasecmp("erase", argv[2]) == 0) {
|
||||
char line[132];
|
||||
snprintf(line, 132, "%s.posit_name_count = %d", self->name, self->posit_name_count);
|
||||
SCWrite(pCon, line, eValue);
|
||||
return 1;
|
||||
}
|
||||
self->posit_name_count = argc - 2;
|
||||
self->position_names = malloc(self->posit_name_count * sizeof(*self->position_names));
|
||||
int i;
|
||||
for (i = 0; i < self->posit_name_count; ++i) {
|
||||
self->position_names[i] = strdup(argv[i + 2]);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
if (self->abs_encoder && strcasecmp("absEncHome", argv[1]) == 0) {
|
||||
if (argc > 2) {
|
||||
/* Debug Managers only */
|
||||
@ -6061,7 +6178,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
char line[132];
|
||||
if (argc > 2) {
|
||||
double target;
|
||||
target = strtod(argv[2], NULL);
|
||||
target = arg2posit(self, argv[2]);
|
||||
snprintf(line, 132, "%s.posit2count = %lld",
|
||||
self->name,
|
||||
posit2count(self, target));
|
||||
@ -6099,11 +6216,25 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
SCWrite(pCon, line, eValue);
|
||||
return 1;
|
||||
}
|
||||
else if(strcasecmp("posit2hard", argv[1]) == 0) {
|
||||
char line[132];
|
||||
if (argc > 2) {
|
||||
double target;
|
||||
target = arg2posit(self, argv[2]);
|
||||
snprintf(line, 132, "%s.posit2hard = %f",
|
||||
self->name,
|
||||
posit2hard(self, target));
|
||||
}
|
||||
else
|
||||
strcpy(line, "missing value");
|
||||
SCWrite(pCon, line, eValue);
|
||||
return 1;
|
||||
}
|
||||
else if(strcasecmp("posit2soft", argv[1]) == 0) {
|
||||
char line[132];
|
||||
if (argc > 2) {
|
||||
double target;
|
||||
target = strtod(argv[2], NULL);
|
||||
target = arg2posit(self, argv[2]);
|
||||
snprintf(line, 132, "%s.posit2soft = %f",
|
||||
self->name,
|
||||
posit2soft(self, target));
|
||||
@ -6113,14 +6244,14 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
SCWrite(pCon, line, eValue);
|
||||
return 1;
|
||||
}
|
||||
else if(strcasecmp("posit2unit", argv[1]) == 0) {
|
||||
else if(strcasecmp("posit2unit", argv[1]) == 0) { /* Legacy */
|
||||
char line[132];
|
||||
if (argc > 2) {
|
||||
double target;
|
||||
target = strtod(argv[2], NULL);
|
||||
target = arg2posit(self, argv[2]);
|
||||
snprintf(line, 132, "%s.posit2unit = %f",
|
||||
self->name,
|
||||
posit2unit(self, target));
|
||||
posit2hard(self, target));
|
||||
}
|
||||
else
|
||||
strcpy(line, "missing value");
|
||||
|
||||
Reference in New Issue
Block a user