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

Begin migration of motor positions from encoder counts to motor units (mm/degree)

Browse Source 
r2185 | dcl | 2007-10-17 08:36:19 +1000 (Wed, 17 Oct 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-10-17 08:36:19 +10:00
parent 9052167b7f
commit a4d7cb12cc
1 changed files with 204 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

220
site_ansto/motor_dmc2280.c
View File

@@ -183,8 +183,10 @@ struct __MoDriv {
pNWTimer state_timer; /**< motor state timer */
SConnection *trace;
int posit_count;
bool posit_check;
unsigned long long *posit_array;
#ifdef POSIT_COUNTS
long long* posit_array;
#endif
double* positions;
int variables;
};
@@ -251,6 +253,7 @@ static int DMC2280Halt(void *pData);
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue);
#ifdef POSIT_COUNTS
static bool check_posits(pDMC2280Driv self) {
char line[CMDLEN];
int missing = 0;
@@ -308,6 +311,70 @@ static bool check_posits(pDMC2280Driv self) {
i + 1,
self->name,
self->posit_array[i]);
/* TODO log me */
}
return false;
}
#endif
static bool check_positions(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->positions[i] < -9999999)
++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->positions[i] > self->positions[i - 1])
direction = 1;
else if (self->positions[i] < self->positions[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->positions[i] < self->positions[i - 1])) {
direction = 0;
}
break;
case 1:
if (!(self->positions[i] > self->positions[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' : %f",
i + 1,
self->name,
self->positions[i]);
/* TODO log me */
}
return false;
}
@@ -355,6 +422,7 @@ static float count2unit(pDMC2280Driv self, long long counts) {
return fPos;
}
#ifdef POSIT_COUNTS
static long long posit2count(pDMC2280Driv self, float target) {
double absolute;
long long result;
@@ -365,6 +433,9 @@ static long long posit2count(pDMC2280Driv self, float target) {
SICSLogWrite(line, eStatus);
return -1;
}
if (!check_posits(self))
return -1;
int i = ((int) target) - 1;
if (i < 0) {
i = 0;
@@ -431,6 +502,60 @@ static float posit2unit(pDMC2280Driv self, float target) {
return count2unit(self, posit2count(self, target));
}
#else
static float unit2posit(pDMC2280Driv self, float target) {
int i, j;
float fPos;
if (!check_positions(self))
return 0;
if (self->positions[0] < self->positions[1]) {
for (i = 1; i < self->posit_count - 1; ++i) {
if (target < self->positions[i]) {
break;
}
}
}
else {
for (i = 1; i < self->posit_count - 1; ++i) {
if (target > self->positions[i]) {
break;
}
}
}
--i;
j = i + 1;
fPos = (double) j +
((double)target - self->positions[i]) /
((double)self->positions[j] - self->positions[i]);
return fPos;
}
static float posit2unit(pDMC2280Driv self, float target) {
double result;
int i = ((int) target) - 1;
if (i < 0) {
i = 0;
} else if (i > self->posit_count - 2) {
i = self->posit_count - 2;
}
result = (double)self->positions[i]
+ ((double)target - ((double)i + 1)) *
((double)self->positions[i + 1] - self->positions[i]);
return result;
}
static long long posit2count(pDMC2280Driv self, float target) {
return unit2count(self, posit2unit(self, target));
}
static float count2posit(pDMC2280Driv self, long long counts) {
return unit2posit(self, count2unit(self, counts));
}
#endif
/** \brief Convert axis speed in physical units to
* motor speed in steps/sec.
*
@@ -2485,6 +2610,17 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->backlash_offset;
return 1;
}
if (self->posit_count > 0) {
if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) {
int index;
*fValue = -1.0;
index = strtol(&name[6], NULL, 10);
// if (index < 1 || index > self->posit_count)
// return 0;
*fValue = posit2count(self, index);
return 1;
}
}
if (self->abs_encoder != 0) {
if (strcasecmp(name,"absenc") == 0) {
if (readAbsEnc(self, fValue) == SUCCESS)
@@ -2516,18 +2652,6 @@ static int DMC2280GetPar(void *pData, char *name,
}
}
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)
@@ -2798,7 +2922,10 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
index = strtol(&name[6], NULL, 10);
if (index < 1 || index > self->posit_count)
return 0;
#ifdef POSIT_COUNTS
self->posit_array[index - 1] = newValue;
#endif
self->positions[index - 1] = count2unit(self, newValue);
return 1;
}
}
@@ -2886,7 +3013,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
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]);
posit2count(self, i + 1));
SCWrite(pCon, buffer, eStatus);
}
}
@@ -2945,11 +3072,17 @@ static void KillDMC2280(/*@only@*/void *pData){
AsyncUnitDestroy(self->asyncUnit);
self->asyncUnit = NULL;
}
#ifdef POSIT_COUNTS
if (self->posit_array) {
free(self->posit_array);
self->posit_array = NULL;
self->posit_count = 0;
}
#endif
if (self->positions) {
free(self->positions);
self->positions = 0;
}
self->posit_count = 0;
/* Not required as performed in caller
* free(self);
*/
@@ -3238,14 +3371,23 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
else {
int i;
char line[80];
#ifdef POSIT_COUNTS
pNew->posit_array = malloc(pNew->posit_count * sizeof(*pNew->posit_array));
#endif
pNew->positions = malloc(pNew->posit_count * sizeof(*pNew->positions));
for (i = 0; i < pNew->posit_count; ++i) {
snprintf(line, 80, "posit_%d", i + 1);
if ((pPtr=getParam(pCon, interp, params,line,_OPTIONAL)) == NULL) {
#ifdef POSIT_COUNTS
pNew->posit_array[i] = -1;
#endif
pNew->positions[i] = 0.0;
}
else {
#ifdef POSIT_COUNTS
pNew->posit_array[i] = strtol(pPtr, NULL, 10);
#endif
pNew->positions[i] = count2unit(pNew, strtol(pPtr, NULL, 10));
}
}
}
@@ -3456,6 +3598,52 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
SCWrite(pCon, line, eValue);
return 1;
}
else if(strcasecmp("positions", argv[1]) == 0) {
if (argc == 2) {
int i, k = 0;
char line[1320];
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]);
}
SCWrite(pCon, line, eValue);
return 1;
}
if (self->posit_count > 0) {
self->posit_count = 0;
}
#ifdef POSIT_COUNTS
if (self->posit_array) {
free(self->posit_array);
self->posit_array = NULL;
}
#endif
if (self->positions) {
free(self->positions);
self->positions = NULL;
self->posit_count = 0;
}
if (argc > 3 && strcasecmp("erase", argv[2]) == 0) {
char line[132];
snprintf(line, 132, "%s.posit_count = %d", self->name, self->posit_count);
SCWrite(pCon, line, eValue);
return 1;
}
self->posit_count = argc - 2;
self->positions = malloc(self->posit_count * sizeof(*self->positions));
#ifdef POSIT_COUNTS
self->posit_array = malloc(self->posit_count * sizeof(*self->posit_array));
#endif
int i;
for (i = 0; i < self->posit_count; ++i) {
self->positions[i] = strtod(argv[i + 2], NULL);
#ifdef POSIT_COUNTS
self->posit_array[i] = unit2count(self, self->positions[i]);
#endif
}
return 1;
}
if (self->posit_count > 0) {
if(strcasecmp("posit2count", argv[1]) == 0) {
char line[132];