Change floats to doubles, controller virtual motor variables, a little refactoring
r2264 | dcl | 2007-12-07 10:38:34 +1100 (Fri, 07 Dec 2007) | 2 lines
This commit is contained in:
Douglas Clowes
@@ -85,6 +85,8 @@ struct EvtEvent_s {
|
||||
#define VAR_POS (1<<4)
|
||||
#define VAR_HLT (1<<5)
|
||||
#define VAR_ENC (1<<6)
|
||||
#define VAR_SWI (1<<7)
|
||||
#define VAR_HOM (1<<8)
|
||||
|
||||
static pAsyncProtocol DMC2280_Protocol = NULL;
|
||||
|
||||
@@ -133,19 +135,19 @@ struct __MoDriv {
|
||||
char axisLabel;
|
||||
char lastCmd[CMDLEN];
|
||||
char dmc2280Error[CMDLEN];
|
||||
float fHome; /**< home position for axis, default=0 */
|
||||
double fHome; /**< home position for axis, default=0 */
|
||||
int motorHome; /**< motor home position in steps */
|
||||
int noPowerSave; /**< Flag = 1 to leave motors on after a move */
|
||||
float stepsPerX; /**< steps per physical unit */
|
||||
double stepsPerX; /**< steps per physical unit */
|
||||
int abs_encoder; /**< Flag = 1 if there is an abs enc */
|
||||
int absEncHome; /**< Home position in counts for abs enc */
|
||||
float cntsPerX; /**< absolute encoder counts per physical unit */
|
||||
double cntsPerX; /**< absolute encoder counts per physical unit */
|
||||
int motOffDelay; /**< msec to wait before switching motor off, default=0 */
|
||||
int currFlags;
|
||||
int currSteps;
|
||||
int currCounts;
|
||||
float currPosition; /**< Position at last position check */
|
||||
float lastPosition; /**< Position at last position check */
|
||||
double currPosition; /**< Position at last position check */
|
||||
double lastPosition; /**< Position at last position check */
|
||||
int lastSteps;
|
||||
int lastCounts;
|
||||
int thread0; /**< last read of _XQ0 */
|
||||
@@ -162,8 +164,8 @@ struct __MoDriv {
|
||||
int blockage_fail; /**< flag =1 if we should fail the motor */
|
||||
int has_airpads; /**< Flag = 1 if there is are airpads for this motor */
|
||||
float backlash_offset; /**< signed offset to drive from */
|
||||
float fTarget; /**< target passed from SICS to timer callback */
|
||||
float fPreseek; /**< preseek target when preseek is active */
|
||||
double fTarget; /**< target passed from SICS to timer callback */
|
||||
double fPreseek; /**< preseek target when preseek is active */
|
||||
int settle; /**< motor settling time in seconds */
|
||||
struct timeval time_settle_done; /**< time when settling will be over */
|
||||
int airpad_state; /**< state of the airpads finite state machine */
|
||||
@@ -383,7 +385,7 @@ static bool check_positions(pDMC2280Driv self) {
|
||||
return false;
|
||||
}
|
||||
|
||||
static long long unit2count(pDMC2280Driv self, float target) {
|
||||
static long long unit2count(pDMC2280Driv self, double target) {
|
||||
double absolute;
|
||||
long long result;
|
||||
if (self->abs_encoder == 0) {
|
||||
@@ -413,8 +415,8 @@ static long long unit2count(pDMC2280Driv self, float target) {
|
||||
return result;
|
||||
}
|
||||
|
||||
static float count2unit(pDMC2280Driv self, long long counts) {
|
||||
float fPos;
|
||||
static double count2unit(pDMC2280Driv self, long long counts) {
|
||||
double fPos;
|
||||
if (self->abs_encoder == 0) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder",
|
||||
@@ -427,7 +429,7 @@ static float count2unit(pDMC2280Driv self, long long counts) {
|
||||
}
|
||||
|
||||
#ifdef POSIT_COUNTS
|
||||
static long long posit2count(pDMC2280Driv self, float target) {
|
||||
static long long posit2count(pDMC2280Driv self, double target) {
|
||||
double absolute;
|
||||
long long result;
|
||||
if (self->abs_encoder == 0) {
|
||||
@@ -463,9 +465,9 @@ static long long posit2count(pDMC2280Driv self, float target) {
|
||||
return result;
|
||||
}
|
||||
|
||||
static float count2posit(pDMC2280Driv self, long long counts) {
|
||||
static double count2posit(pDMC2280Driv self, long long counts) {
|
||||
int i, j;
|
||||
float fPos;
|
||||
double fPos;
|
||||
if (self->abs_encoder == 0) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "count2posit motor %s has no absolute encoder",
|
||||
@@ -498,19 +500,19 @@ static float count2posit(pDMC2280Driv self, long long counts) {
|
||||
return fPos;
|
||||
}
|
||||
|
||||
static float unit2posit(pDMC2280Driv self, float target) {
|
||||
static double unit2posit(pDMC2280Driv self, double target) {
|
||||
return count2posit(self, unit2count(self, target));
|
||||
}
|
||||
|
||||
static float posit2unit(pDMC2280Driv self, float target) {
|
||||
static double posit2unit(pDMC2280Driv self, double target) {
|
||||
return count2unit(self, posit2count(self, target));
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
static float unit2posit(pDMC2280Driv self, float target) {
|
||||
static double unit2posit(pDMC2280Driv self, double target) {
|
||||
int i, j;
|
||||
float fPos;
|
||||
double fPos;
|
||||
if (!check_positions(self))
|
||||
return 0;
|
||||
|
||||
@@ -536,7 +538,7 @@ static float unit2posit(pDMC2280Driv self, float target) {
|
||||
return fPos;
|
||||
}
|
||||
|
||||
static float posit2unit(pDMC2280Driv self, float target) {
|
||||
static double posit2unit(pDMC2280Driv self, double target) {
|
||||
double result;
|
||||
int i = ((int) target) - 1;
|
||||
if (i < 0) {
|
||||
@@ -550,11 +552,11 @@ static float posit2unit(pDMC2280Driv self, float target) {
|
||||
return result;
|
||||
}
|
||||
|
||||
static long long posit2count(pDMC2280Driv self, float target) {
|
||||
static long long posit2count(pDMC2280Driv self, double target) {
|
||||
return unit2count(self, posit2unit(self, target));
|
||||
}
|
||||
|
||||
static float count2posit(pDMC2280Driv self, long long counts) {
|
||||
static double count2posit(pDMC2280Driv self, long long counts) {
|
||||
return unit2posit(self, count2unit(self, counts));
|
||||
}
|
||||
|
||||
@@ -567,7 +569,7 @@ static float count2posit(pDMC2280Driv self, long long counts) {
|
||||
* \param speed in physical units, eg mm/sec degrees/sec
|
||||
* \return the speed in motor steps/sec
|
||||
*/
|
||||
static int motSpeed(pDMC2280Driv self, float axisSpeed) {
|
||||
static int motSpeed(pDMC2280Driv self, double axisSpeed) {
|
||||
int speed;
|
||||
speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5);
|
||||
return speed;
|
||||
@@ -580,7 +582,7 @@ static int motSpeed(pDMC2280Driv self, float axisSpeed) {
|
||||
* \param acceleration in physical units, eg mm/sec^2 degrees/sec^2
|
||||
* \return the acceleration in motor steps/sec^2
|
||||
*/
|
||||
static int motAccel(pDMC2280Driv self, float axisAccel) {
|
||||
static int motAccel(pDMC2280Driv self, double axisAccel) {
|
||||
int accel;
|
||||
accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5);
|
||||
return accel;
|
||||
@@ -593,7 +595,7 @@ static int motAccel(pDMC2280Driv self, float axisAccel) {
|
||||
* \param deceleration in physical units, eg mm/sec^2 degrees/sec^2
|
||||
* \return the deceleration in motor steps/sec^2
|
||||
*/
|
||||
static int motDecel(pDMC2280Driv self, float axisDecel) {
|
||||
static int motDecel(pDMC2280Driv self, double axisDecel) {
|
||||
int decel;
|
||||
decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5);
|
||||
return decel;
|
||||
@@ -605,8 +607,8 @@ static int motDecel(pDMC2280Driv self, float axisDecel) {
|
||||
* \param self (r) provides access to the motor's data structure
|
||||
* \return the motor position in physical units
|
||||
*/
|
||||
static float motPosit(pDMC2280Driv self) {
|
||||
float fPos;
|
||||
static double motPosit(pDMC2280Driv self) {
|
||||
double fPos;
|
||||
if (self->abs_encoder)
|
||||
fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->fHome;
|
||||
else
|
||||
@@ -622,7 +624,7 @@ static float motPosit(pDMC2280Driv self) {
|
||||
* \param target in physical units, eg mm, degrees
|
||||
* \return the absolute position in motor steps
|
||||
*/
|
||||
static int motAbsol(pDMC2280Driv self, float target) {
|
||||
static int motAbsol(pDMC2280Driv self, double target) {
|
||||
double absolute;
|
||||
int result;
|
||||
if (self->abs_encoder) {
|
||||
@@ -659,7 +661,7 @@ static int motAbsol(pDMC2280Driv self, float target) {
|
||||
* \param target in physical units, eg mm, degrees
|
||||
* \return the absolute position in motor steps
|
||||
*/
|
||||
static int motCreep(pDMC2280Driv self, float target) {
|
||||
static int motCreep(pDMC2280Driv self, double target) {
|
||||
int target_steps = motAbsol(self, target);
|
||||
if (!self->abs_encoder || /* no absolute encoder */
|
||||
self->preseek || /* backlash preseek active */
|
||||
@@ -1000,7 +1002,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) {
|
||||
if (FAILURE == DMC2280SendReceive(self, cmd, reply))
|
||||
return FAILURE;
|
||||
|
||||
*pos = (float) atoi(reply);
|
||||
*pos = (double) atol(reply);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
@@ -1063,7 +1065,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
|
||||
return FAILURE;
|
||||
return SUCCESS;
|
||||
}
|
||||
snprintf(cmd, CMDLEN, "XQ #HOME,1");
|
||||
snprintf(cmd, CMDLEN, "XQ #HOME,7");
|
||||
if (FAILURE == DMC2280Send(self, cmd))
|
||||
return FAILURE;
|
||||
|
||||
@@ -1082,7 +1084,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
|
||||
* - 0 MOTOR BLOCKED, no significant change in position detected.
|
||||
*/
|
||||
static int checkMotion(void *pData) {
|
||||
float ratio_obs, ratio_exp, ratio_cmp;
|
||||
double ratio_obs, ratio_exp, ratio_cmp;
|
||||
long int usec_TimeDiff;
|
||||
struct timeval now;
|
||||
|
||||
@@ -1114,17 +1116,17 @@ static int checkMotion(void *pData) {
|
||||
if (self->currCounts == self->lastCounts) /* prevent divide by zero */
|
||||
ratio_obs = (self->currSteps - self->lastSteps);
|
||||
else {
|
||||
ratio_obs = (float) (self->currSteps - self->lastSteps);
|
||||
ratio_obs /= (float) (self->currCounts - self->lastCounts);
|
||||
ratio_obs = (double) (self->currSteps - self->lastSteps);
|
||||
ratio_obs /= (double) (self->currCounts - self->lastCounts);
|
||||
}
|
||||
ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX;
|
||||
ratio_exp = (double) self->stepsPerX / (double) self->cntsPerX;
|
||||
ratio_cmp = ratio_obs / ratio_exp;
|
||||
/* wrong signs, less than half, or more than double is trouble */
|
||||
if (ratio_cmp < 0.0 ||
|
||||
ratio_cmp > self->blockage_ratio ||
|
||||
(1.0 / ratio_cmp) > self->blockage_ratio) {
|
||||
char msg[132];
|
||||
float cmp = ratio_cmp;
|
||||
double cmp = ratio_cmp;
|
||||
if (fabs(cmp) > 0 && fabs(cmp) < 1)
|
||||
cmp = 1.0 / cmp;
|
||||
snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f",
|
||||
@@ -1142,7 +1144,7 @@ static int checkMotion(void *pData) {
|
||||
} else {
|
||||
if (self->debug) {
|
||||
char msg[132];
|
||||
float cmp = ratio_cmp;
|
||||
double cmp = ratio_cmp;
|
||||
if (fabs(cmp) > 0 && fabs(cmp) < 1)
|
||||
cmp = 1.0 / cmp;
|
||||
snprintf(msg, sizeof(msg), "Motor %s pass: obs=%f, exp=%f, cmp=%f",
|
||||
@@ -1170,10 +1172,10 @@ static int checkMotion(void *pData) {
|
||||
*
|
||||
* \return true if found else false
|
||||
*/
|
||||
static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
|
||||
char tmp[20];
|
||||
static bool has_var(pDMC2280Driv self, const char* vars, const char* name) {
|
||||
char* p;
|
||||
snprintf(tmp, 20, "%s%c=", name, self->axisLabel);
|
||||
char tmp[20];
|
||||
snprintf(tmp, 20, "%s=", name);
|
||||
p = strstr(vars, tmp);
|
||||
if (p == NULL) /* not found at all */
|
||||
return false;
|
||||
@@ -1184,6 +1186,13 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
|
||||
return false; /* found partial (ZVVVx=) only */
|
||||
}
|
||||
|
||||
static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
|
||||
char tmp[20];
|
||||
snprintf(tmp, 20, "%s%c", name, self->axisLabel);
|
||||
return has_var(self, vars, tmp);
|
||||
}
|
||||
|
||||
|
||||
/* State Machine Events */
|
||||
|
||||
static int state_msg_callback(pAsyncTxn pCmd);
|
||||
@@ -1195,31 +1204,17 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd);
|
||||
*
|
||||
* \param self access to the drive data
|
||||
*/
|
||||
#if 1
|
||||
static int cmdStatus(pDMC2280Driv self) {
|
||||
char cmd[CMDLEN];
|
||||
|
||||
/* TODO: Use POSx, ENCx, RUNx if it has these variables */
|
||||
snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,_TS%c,%s%c,_TI0,_TI1,_XQ0",
|
||||
/* TODO: Use POSx, ENCx, RUNx, SWIx if it has these variables */
|
||||
snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,%s%c,%s%c,_TI0,_TI1,_XQ0",
|
||||
(self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel,
|
||||
(self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel,
|
||||
self->axisLabel,
|
||||
(self->variables & VAR_SWI) ? "SWI" : "_TS", self->axisLabel,
|
||||
(self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel);
|
||||
return DMC_SendCmd(self, cmd, state_msg_callback);
|
||||
}
|
||||
#else
|
||||
static int cmdStatus(pDMC2280Driv self) {
|
||||
char cmd[CMDLEN];
|
||||
|
||||
if (self->has_airpads == 3)
|
||||
snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,RUN%c,_TI0,_TI1,_XQ0",
|
||||
self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel);
|
||||
else
|
||||
snprintf(cmd, CMDLEN, "MG {F10.0} _TD%c,_TP%c,_TS%c,_BG%c,_TI0,_TI1,_XQ0",
|
||||
self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel);
|
||||
return DMC_SendCmd(self, cmd, state_msg_callback);
|
||||
}
|
||||
#endif
|
||||
|
||||
static int cmdVars(pDMC2280Driv self) {
|
||||
char cmd[CMDLEN];
|
||||
@@ -1330,6 +1325,8 @@ static int rspStatus(pDMC2280Driv self, const char* text) {
|
||||
|
||||
static int rspVars(pDMC2280Driv self, const char* text) {
|
||||
self->variables = 0;
|
||||
if (has_var(self, text, "HOMERUN"))
|
||||
self->variables |= VAR_HOM;
|
||||
if (has_var_x(self, text, "REQ"))
|
||||
self->variables |= VAR_REQ;
|
||||
if (has_var_x(self, text, "RSP"))
|
||||
@@ -1344,6 +1341,8 @@ static int rspVars(pDMC2280Driv self, const char* text) {
|
||||
self->variables |= VAR_HLT;
|
||||
if (has_var_x(self, text, "ENC"))
|
||||
self->variables |= VAR_ENC;
|
||||
if (has_var_x(self, text, "SWI"))
|
||||
self->variables |= VAR_SWI;
|
||||
if ((self->variables & VAR_REQ) &&
|
||||
(self->variables & VAR_RSP)) {
|
||||
self->has_airpads = 2;
|
||||
@@ -1356,6 +1355,14 @@ static int rspVars(pDMC2280Driv self, const char* text) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int rspPoll(pDMC2280Driv self, const char* text) {
|
||||
int iReply = atoi(text);
|
||||
if (iReply < 0)
|
||||
snprintf(self->dmc2280Error, CMDLEN,
|
||||
"MOTOR CONTROLLER REQ ERROR: %d", iReply);
|
||||
return iReply;
|
||||
}
|
||||
|
||||
/* State Functions */
|
||||
|
||||
static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event);
|
||||
@@ -1482,7 +1489,7 @@ static void unhandled_event(pDMC2280Driv self, pEvtEvent event) {
|
||||
state_name(self->myState),
|
||||
self->subState,
|
||||
event_name(event, text, CMDLEN));
|
||||
SICSLogWrite(line, eStatus);
|
||||
SICSLogWrite(line, eError);
|
||||
}
|
||||
|
||||
static void handle_event(pDMC2280Driv self, pEvtEvent event) {
|
||||
@@ -1624,7 +1631,7 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
|
||||
pAsyncTxn pCmd = event->event.msg.cmd;
|
||||
if (self->subState == 1) { /* Status Response */
|
||||
int iRet;
|
||||
float fDelta;
|
||||
long lDelta;
|
||||
iRet = rspStatus(self, pCmd->inp_buf);
|
||||
if (iRet == 0)
|
||||
break;
|
||||
@@ -1636,10 +1643,10 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
|
||||
}
|
||||
/* if the motor moved, update any observers */
|
||||
if (self->abs_encoder)
|
||||
fDelta = fabs(self->currCounts - self->lastCounts);
|
||||
lDelta = fabs(self->currCounts - self->lastCounts);
|
||||
else
|
||||
fDelta = fabs(self->currSteps - self->lastSteps);
|
||||
if (fDelta > 10) {
|
||||
lDelta = fabs(self->currSteps - self->lastSteps);
|
||||
if (lDelta > 10) {
|
||||
set_lastMotion(self, self->currSteps, self->currCounts);
|
||||
report_motion(self);
|
||||
}
|
||||
@@ -1759,12 +1766,19 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
|
||||
}
|
||||
}
|
||||
else if (self->subState == 2) { /* Poll */
|
||||
float fReply;
|
||||
fReply = (float) atof(pCmd->inp_buf);
|
||||
if (fReply > 0) {
|
||||
int iRet;
|
||||
iRet = rspPoll(self, pCmd->inp_buf);
|
||||
if (iRet > 0) { /* state has changed to ON */
|
||||
change_state(self, DMCState_MotorOn);
|
||||
return;
|
||||
}
|
||||
else if (iRet < 0) {
|
||||
/* Handle error return */
|
||||
self->errorCode = RUNERROR;
|
||||
self->driver_status = HWFault;
|
||||
change_state(self, DMCState_MotorOff);
|
||||
return;
|
||||
}
|
||||
/* TODO handle airpad timeout */
|
||||
NetWatchRegisterTimer(&self->state_timer,
|
||||
AIR_POLL_TIMER,
|
||||
@@ -1801,7 +1815,7 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
|
||||
pAsyncTxn pCmd = event->event.msg.cmd;
|
||||
if (self->subState == 1) { /* Status Response */
|
||||
int iRet, absolute;
|
||||
float target;
|
||||
double target;
|
||||
iRet = rspStatus(self, pCmd->inp_buf);
|
||||
if (iRet == 0)
|
||||
break;
|
||||
@@ -2004,7 +2018,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
|
||||
* If this was a pre-seek then compute the next iteration
|
||||
*/
|
||||
if (self->preseek) {
|
||||
float target;
|
||||
double target;
|
||||
int absolute;
|
||||
self->preseek = 0;
|
||||
target = self->fTarget;
|
||||
@@ -2239,12 +2253,20 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
|
||||
}
|
||||
}
|
||||
else if (self->subState == 1) { /* Poll */
|
||||
float fReply;
|
||||
fReply = (float) atof(pCmd->inp_buf);
|
||||
if (fReply == 0) {
|
||||
int iRet;
|
||||
iRet = rspPoll(self, pCmd->inp_buf);
|
||||
if (iRet == 0) { /* state has changed to OFF */
|
||||
change_state(self, DMCState_MotorOff);
|
||||
return;
|
||||
}
|
||||
else if (iRet < 0) {
|
||||
/* Handle error return */
|
||||
self->errorCode = RUNERROR;
|
||||
self->driver_status = HWFault;
|
||||
change_state(self, DMCState_MotorOff);
|
||||
return;
|
||||
}
|
||||
/* TODO handle airpad timeout */
|
||||
}
|
||||
NetWatchRegisterTimer(&self->state_timer,
|
||||
AIR_POLL_TIMER,
|
||||
@@ -2647,6 +2669,10 @@ static int DMC2280GetPar(void *pData, char *name,
|
||||
*fValue = self->fHome;
|
||||
return 1;
|
||||
}
|
||||
if(strcasecmp(name, "stepsPerX") == 0) {
|
||||
*fValue = self->stepsPerX;
|
||||
return 1;
|
||||
}
|
||||
if(strcasecmp(name,HARDLOWERLIM) == 0) {
|
||||
*fValue = self->fLower;
|
||||
return 1;
|
||||
@@ -2737,6 +2763,10 @@ static int DMC2280GetPar(void *pData, char *name,
|
||||
*fValue = self->absEncHome;
|
||||
return 1;
|
||||
}
|
||||
if (strcasecmp(name,"cntsperx") == 0) {
|
||||
*fValue = self->cntsPerX;
|
||||
return 1;
|
||||
}
|
||||
if (self->has_fsm) {
|
||||
if(strcasecmp(name,"creep_offset") == 0) {
|
||||
*fValue = self->creep_offset;
|
||||
@@ -3395,7 +3425,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
|
||||
free(pNew);
|
||||
return NULL;
|
||||
}
|
||||
sscanf(pPtr,"%f",&(pNew->stepsPerX));
|
||||
sscanf(pPtr,"%lg",&(pNew->stepsPerX));
|
||||
if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL)
|
||||
pNew->motorHome=0;
|
||||
else
|
||||
@@ -3442,7 +3472,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
|
||||
if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL)
|
||||
pNew->cntsPerX = 1.0;
|
||||
else
|
||||
sscanf(pPtr,"%f",&(pNew->cntsPerX));
|
||||
sscanf(pPtr,"%lg",&(pNew->cntsPerX));
|
||||
if (pNew->has_fsm) {
|
||||
/* CREEP_OFFSET: this controls unidirectional driving */
|
||||
if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL)
|
||||
@@ -3538,14 +3568,14 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
}
|
||||
else if(strcasecmp("data", argv[1]) == 0) {
|
||||
char line[132];
|
||||
snprintf(line, 132, "%s.motor_step = %f %s", self->name,
|
||||
snprintf(line, 132, "%s.motor_step = %10g %s", self->name,
|
||||
1.0 / self->stepsPerX, self->units);
|
||||
SCWrite(pCon, line, eValue);
|
||||
if (self->abs_encoder) {
|
||||
snprintf(line, 132, "%s.encoder_count = %f %s", self->name,
|
||||
snprintf(line, 132, "%s.encoder_count = %10g %s", self->name,
|
||||
1.0 / self->cntsPerX, self->units);
|
||||
SCWrite(pCon, line, eValue);
|
||||
snprintf(line, 132, "%s.steps_per_count = %f steps", self->name,
|
||||
snprintf(line, 132, "%s.steps_per_count = %10g steps", self->name,
|
||||
self->stepsPerX / self->cntsPerX);
|
||||
SCWrite(pCon, line, eValue);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user