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

POSIT handling code for position-mode motors and a little refactoring

Browse Source 
r2155 | dcl | 2007-09-26 09:50:53 +1000 (Wed, 26 Sep 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-09-26 09:50:53 +10:00
parent bef9e542ae
commit efb0b117f6
1 changed files with 381 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

422
site_ansto/motor_dmc2280.c
View File

@@ -1,3 +1,4 @@
#define POSIT_SHIM
/** \file motor_dmc2280.c
* \brief Driver for Galil DMC2280 motor controller.
*
@@ -125,7 +126,7 @@ struct __MoDriv {
char axisLabel;
char lastCmd[CMDLEN];
char dmc2280Error[CMDLEN];
float home; /**< home position for axis, default=0 */
float 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 */
@@ -138,8 +139,8 @@ struct __MoDriv {
int currCounts;
float currPosition; /**< Position at last position check */
float lastPosition; /**< Position at last position check */
float lastSteps;
float lastCounts;
int lastSteps;
int lastCounts;
int thread0; /**< last read of _XQ0 */
unsigned short int input0; /**< last read of _TI0 */
unsigned short int input1; /**< last read of _TI1 */
@@ -175,6 +176,9 @@ struct __MoDriv {
int subState;
pNWTimer state_timer; /**< motor state timer */
SConnection *trace;
int posit_count;
bool posit_check;
unsigned long long *posit_array;
};
int DMC2280MotionControl = 1; /* defaults to enabled */
@@ -240,6 +244,186 @@ static int DMC2280Halt(void *pData);
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue);
static bool check_posits(pDMC2280Driv self) {
char line[CMDLEN];
int missing = 0;
int direction = 0;
int i;
if (self->posit_count < 2) {
snprintf(line, ERRLEN, "Insufficient positions on motor '%s': %d",
self->name, self->posit_count);
SICSLogWrite(line,eError);
return false;
}
for (i = 0; i < self->posit_count; ++i)
if (self->posit_array[i] < 0)
++missing;
if (missing) {
snprintf(line, ERRLEN, "Missing positions on motor '%s': %d",
self->name, missing);
SICSLogWrite(line,eError);
return false;
}
for (i = 1; i < self->posit_count; ++i) {
if (i == 1) {
if (self->posit_array[i] > self->posit_array[i - 1])
direction = 1;
else if (self->posit_array[i] < self->posit_array[i - 1])
direction = -1;
else
direction = 0;
}
if (direction == 0) {
snprintf(line, ERRLEN, "Position order on motor '%s' : %d",
self->name, i);
SICSLogWrite(line,eError);
}
else {
switch (direction) {
case -1:
if (!(self->posit_array[i] < self->posit_array[i - 1])) {
direction = 0;
}
break;
case 1:
if (!(self->posit_array[i] > self->posit_array[i - 1])) {
direction = 0;
}
break;
}
}
}
if (direction != 0)
return true;
for (i = 0; i < self->posit_count; ++i) {
snprintf(line, ERRLEN, "Position %2d motor '%s' : %lld",
i + 1,
self->name,
self->posit_array[i]);
}
return false;
}
static long long unit2count(pDMC2280Driv self, float target) {
double absolute;
long long result;
if (self->abs_encoder == 0) {
char line[CMDLEN];
snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder",
self->name);
SICSLogWrite(line, eStatus);
return -1;
}
/* distance of target from home in units */
absolute = (double) target - self->fHome;
/* convert to encoder counts */
absolute *= (double) self->cntsPerX;
/* add home position in counts */
absolute += (double) self->absEncHome;
if (absolute > 0)
result = (int) (absolute + 0.5);
else if (absolute < 0)
result = (int) (absolute - 0.5);
else
result = (int) absolute;
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "unit2count Rounding %f to %lld", absolute, result);
SICSLogWrite(line, eStatus);
}
return result;
}
static float count2unit(pDMC2280Driv self, long long counts) {
float fPos;
if (self->abs_encoder == 0) {
char line[CMDLEN];
snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder",
self->name);
SICSLogWrite(line, eStatus);
return -1;
}
fPos = (counts - self->absEncHome) / self->cntsPerX + self->fHome;
return fPos;
}
static long long posit2count(pDMC2280Driv self, float target) {
double absolute;
long long result;
if (self->abs_encoder == 0) {
char line[CMDLEN];
snprintf(line, CMDLEN, "posit2count motor %s has no absolute encoder",
self->name);
SICSLogWrite(line, eStatus);
return -1;
}
int i = ((int) target) - 1;
if (i < 0) {
i = 0;
} else if (i > self->posit_count - 2) {
i = self->posit_count - 2;
}
absolute = (double)self->posit_array[i]
+ ((double)target - ((double)i + 1)) *
((double)self->posit_array[i + 1] - self->posit_array[i]);
if (absolute > 0)
result = (int) (absolute + 0.5);
else if (absolute < 0)
result = (int) (absolute - 0.5);
else
result = (int) absolute;
if (self->debug) {
char line[CMDLEN];
snprintf(line, CMDLEN, "posit2count Rounding %f to %lld", absolute, result);
SICSLogWrite(line, eStatus);
}
return result;
}
static float count2posit(pDMC2280Driv self, long long counts) {
int i, j;
float fPos;
if (self->abs_encoder == 0) {
char line[CMDLEN];
snprintf(line, CMDLEN, "count2posit motor %s has no absolute encoder",
self->name);
SICSLogWrite(line, eStatus);
return 0;
}
if (!check_posits(self))
return 0;
if (self->posit_array[0] < self->posit_array[1]) {
for (i = 1; i < self->posit_count - 1; ++i) {
if (counts < self->posit_array[i]) {
break;
}
}
}
else {
for (i = 1; i < self->posit_count - 1; ++i) {
if (counts > self->posit_array[i]) {
break;
}
}
}
--i;
j = i + 1;
fPos = (double) j +
((double)counts - self->posit_array[i]) /
((double)self->posit_array[j] - self->posit_array[i]);
return fPos;
}
static float unit2posit(pDMC2280Driv self, float target) {
return count2posit(self, unit2count(self, target));
}
static float posit2unit(pDMC2280Driv self, float target) {
return count2unit(self, posit2count(self, target));
}
/** \brief Convert axis speed in physical units to
* motor speed in steps/sec.
*
@@ -288,12 +472,13 @@ static int motDecel(pDMC2280Driv self, float axisDecel) {
static float motPosit(pDMC2280Driv self) {
float fPos;
if (self->abs_encoder)
fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->home;
fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->fHome;
else
fPos = (self->currSteps - self->motorHome) / self->stepsPerX + self->home;
fPos = (self->currSteps - self->motorHome) / self->stepsPerX + self->fHome;
return fPos;
}
/** \brief Convert motor target in physical units to
* motor absolute position in steps
*
@@ -305,24 +490,17 @@ static int motAbsol(pDMC2280Driv self, float target) {
double absolute;
int result;
if (self->abs_encoder) {
#if 1
/* distance of target from home in units */
absolute = target - self->home;
absolute = target - self->fHome;
/* subtract current encoder position in units */
absolute -= (self->currCounts - self->absEncHome) / self->cntsPerX;
/* convert to motor steps */
absolute *= self->stepsPerX;
/* add current position in steps */
absolute += (double) self->currSteps;
#else
absolute = ((self->absEncHome - self->currCounts) +
(self->cntsPerX * (target - self->home))) *
(self->stepsPerX / self->cntsPerX) +
self->currSteps;
#endif
}
else {
absolute = (target - self->home) * self->stepsPerX + self->motorHome;
absolute = (target - self->fHome) * self->stepsPerX + self->motorHome;
}
if (absolute > 0)
result = (int) (absolute + 0.5);
@@ -768,7 +946,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
* - 0 MOTOR BLOCKED, no significant change in position detected.
*/
static int checkMotion(void *pData) {
float steps, counts, ratio_obs, ratio_exp, ratio_cmp;
float ratio_obs, ratio_exp, ratio_cmp;
long int usec_TimeDiff;
struct timeval now;
@@ -780,11 +958,7 @@ static int checkMotion(void *pData) {
return 1;
if (self->time_lastPos_set.tv_sec == 0) {
/* first time - initialise the data */
if (self->has_fsm) {
steps = self->currSteps;
counts = self->currCounts;
}
set_lastMotion(pData, steps, counts);
set_lastMotion(self, self->currSteps, self->currCounts);
return 1;
}
gettimeofday(&now, NULL);
@@ -794,21 +968,17 @@ static int checkMotion(void *pData) {
usec_TimeDiff -= self->time_lastPos_set.tv_usec;
if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
return 1;
if (self->has_fsm) {
steps = self->currSteps;
counts = self->currCounts;
}
/* If not stepping, then not blocked */
if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
/* just update the timestamp */
set_lastMotion(pData, self->lastSteps, self->lastCounts);
set_lastMotion(self, self->lastSteps, self->lastCounts);
return 1;
}
/* calculate observed and expected steps per count ratios */
if (counts == self->lastCounts) /* prevent divide by zero */
ratio_obs = (steps - self->lastSteps);
if (self->currCounts == self->lastCounts) /* prevent divide by zero */
ratio_obs = (self->currSteps - self->lastSteps);
else
ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts);
ratio_obs = (self->currSteps - self->lastSteps) / (self->currCounts - self->lastCounts);
ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX;
ratio_cmp = ratio_obs / ratio_exp;
/* wrong signs, less than half, or more than double is trouble */
@@ -822,13 +992,14 @@ static int checkMotion(void *pData) {
snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f",
self->name, ratio_obs, ratio_exp, cmp);
SICSLogWrite(msg, eError);
snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%f/%f",
steps, self->lastSteps, counts, self->lastCounts,
snprintf(msg, sizeof(msg), "steps=%d-%d, counts=%d-%d, exp=%f/%f",
self->currSteps, self->lastSteps,
self->currCounts, self->lastCounts,
self->stepsPerX, self->cntsPerX);
SICSLogWrite(msg, eError);
if (self->blockage_fail)
return 0;
set_lastMotion(pData, steps, counts);
set_lastMotion(self, self->currSteps, self->currCounts);
return 1;
} else {
if (self->debug) {
@@ -840,7 +1011,7 @@ static int checkMotion(void *pData) {
self->name, ratio_obs, ratio_exp, cmp);
SICSLogWrite(msg, eError);
}
set_lastMotion(pData, steps, counts);
set_lastMotion(self, self->currSteps, self->currCounts);
return 1;
}
return 1;
@@ -1190,6 +1361,11 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
if (iRet == 0)
break;
set_lastMotion(self, self->currSteps, self->currCounts);
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
if (self->pMot) {
self->pMot->fTarget = self->pMot->fPosition = motPosit(self);
}
snprintf(cmd, CMDLEN, "LV");
DMC_SendCmd(self, cmd, state_msg_callback);
self->subState = 2;
@@ -2194,7 +2370,7 @@ static int DMC2280GetPar(void *pData, char *name,
/* XXX Maybe move this to a configuration parameter.*/
if(strcasecmp(name,HOME) == 0) {
*fValue = self->home;
*fValue = self->fHome;
return 1;
}
if(strcasecmp(name,HARDLOWERLIM) == 0) {
@@ -2295,6 +2471,19 @@ static int DMC2280GetPar(void *pData, char *name,
return 0;
}
}
if (self->posit_count > 0) {
int index;
if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) {
*fValue = -1.0;
index = strtol(&name[6], NULL, 10);
if (index < 1 || index > self->posit_count)
return 0;
*fValue = (float) self->posit_array[index - 1];
return 1;
}
}
if(strcasecmp(name,"thread0") == 0)
return readThread(self, 0, fValue);
if(strcasecmp(name,"thread1") == 0)
@@ -2328,7 +2517,7 @@ static int DMC2280GetPar(void *pData, char *name,
* - 0 request failed
* */
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue){
char *name, float newValue) {
pDMC2280Driv self = NULL;
char pError[ERRLEN];
char cmd[CMDLEN];
@@ -2351,7 +2540,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
SCWrite(pCon, pError, eError);
return 1;
}
self->home = newValue;
self->fHome = newValue;
return 1;
}
}
@@ -2559,8 +2748,20 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
return 1;
}
if (self->posit_count > 0) {
int index;
if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) {
index = strtol(&name[6], NULL, 10);
if (index < 1 || index > self->posit_count)
return 0;
self->posit_array[index - 1] = newValue;
return 1;
}
}
return 0;
}
/** \brief List the motor parameters to the client.
* \param self (r) provides access to the motor's data structure
* \param *name (r) name of motor.
@@ -2588,7 +2789,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
pDMC2280Driv self = (pDMC2280Driv) pData;
char buffer[BUFFLEN];
snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->home);
snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->fHome);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, self->speed);
SCWrite(pCon, buffer, eStatus);
@@ -2634,6 +2835,17 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
SCWrite(pCon, buffer, eStatus);
}
}
if (self->posit_count > 0) {
int i;
snprintf(buffer, BUFFLEN, "%s.posit_count = %d\n", name,
self->posit_count);
SCWrite(pCon, buffer, eStatus);
for (i = 0; i < self->posit_count; ++i) {
snprintf(buffer, BUFFLEN, "%s.posit_%d = %lld\n", name, i + 1,
self->posit_array[i]);
SCWrite(pCon, buffer, eStatus);
}
}
snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX);
SCWrite(pCon, buffer, eStatus);
return;
@@ -2689,6 +2901,11 @@ static void KillDMC2280(/*@only@*/void *pData){
AsyncUnitDestroy(self->asyncUnit);
self->asyncUnit = NULL;
}
if (self->posit_array) {
free(self->posit_array);
self->posit_array = NULL;
self->posit_count = 0;
}
/* Not required as performed in caller
* free(self);
*/
@@ -2793,9 +3010,9 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
pNew->pMot = NULL;
strcpy(pNew->name, motor);
pNew->home = 0.0;
pNew->fLower = 0.0;//(float)atof(argv[2]);
pNew->fUpper = 0.0;//(float)atof(argv[3]);
pNew->fHome = 0.0;
pNew->fLower = 0.0;
pNew->fUpper = 0.0;
pNew->GetPosition = DMC2280GetPos;
pNew->RunTo = DMC2280Run;
pNew->GetStatus = DMC2280Status;
@@ -2963,6 +3180,32 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
pNew->creep_precision = -pNew->creep_precision;
}
}
if ((pPtr=getParam(pCon, interp, params,"posit_count",_OPTIONAL)) == NULL)
pNew->posit_count = 0;
else {
sscanf(pPtr,"%d",&(pNew->posit_count));
if (pNew->posit_count < 1 || pNew->posit_count > 99) {
snprintf(pError, ERRLEN, "Invalid posit_count on motor '%s': %d", motor,
pNew->posit_count);
SCWrite(pCon,pError,eError);
pNew->posit_count = 0;
}
else {
int i;
char line[80];
pNew->posit_array = malloc(pNew->posit_count * sizeof(*pNew->posit_array));
for (i = 0; i < pNew->posit_count; ++i) {
snprintf(line, 80, "posit_%d", i + 1);
if ((pPtr=getParam(pCon, interp, params,line,_OPTIONAL)) == NULL) {
pNew->posit_array[i] = -1;
}
else {
pNew->posit_array[i] = strtol(pPtr, NULL, 10);
}
}
}
}
}
change_state(pNew, DMCState_Unknown);
@@ -3130,10 +3373,107 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
}
else {
self->trace = 0;
SCWrite(pCon, "TRACE OFF", eValue);
SCWrite(pCon, "TRACE OFF", eValue);
}
return 1;
}
else if(strcasecmp("unit2count", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = strtod(argv[2], NULL);
snprintf(line, 132, "%s.unit2count = %lld",
self->name,
unit2count(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("count2unit", argv[1]) == 0) {
char line[132];
if (argc > 2) {
long long target;
target = strtoll(argv[2], NULL, 10);
snprintf(line, 132, "%s.count2unit = %f",
self->name,
count2unit(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
if (self->posit_count > 0) {
if(strcasecmp("posit2count", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = strtod(argv[2], NULL);
snprintf(line, 132, "%s.posit2count = %lld",
self->name,
posit2count(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("count2posit", argv[1]) == 0) {
char line[132];
if (argc > 2) {
long long target;
target = strtoll(argv[2], NULL, 10);
snprintf(line, 132, "%s.count2posit = %f",
self->name,
count2posit(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("unit2posit", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = strtod(argv[2], NULL);
snprintf(line, 132, "%s.unit2posit = %f",
self->name,
unit2posit(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("posit2unit", argv[1]) == 0) {
char line[132];
if (argc > 2) {
double target;
target = strtod(argv[2], NULL);
snprintf(line, 132, "%s.posit2unit = %f",
self->name,
posit2unit(self, target));
}
else
strcpy(line, "missing value");
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("posit", argv[1]) == 0) {
char line[132];
int target = self->currCounts;
if (argc > 2)
target = strtol(argv[2], NULL, 10);
snprintf(line, 132, "%s.posit = %f",
self->name,
count2posit(self, target));
SCWrite(pCon, line, eValue);
return 1;
}
}
}
return MotorAction(pCon, pSics, pData, argc, argv);
}