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

motor verbose/debug, position_names, hard<->soft and refactor

Browse Source 
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
2014-05-30 17:32:47 +10:00
parent c7a2d05832
commit 1a1f00c49a
1 changed files with 216 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

301
site_ansto/motor_dmc2280.c
View File

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