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

Add motor creep_factor and creep_substep and remove obsolete code

Browse Source
This commit is contained in:
Douglas Clowes
2013-08-02 09:50:46 +10:00
parent ea0a9759e1
commit 88f788e9cc
1 changed files with 76 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
site_ansto/motor_dmc2280.c
View File

@@ -38,8 +38,6 @@
#define TEXTPARLEN 1024
#define CMDLEN 1024
#define STATE_TRACE (200)
#define DECADIC_CREEP (10)
#define SPEED_ON_MOVE 1
extern double DoubleTime(void);
@@ -217,6 +215,8 @@ struct __MoDriv {
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 */
float creep_factor; /**< how much of the offset to creep (0.01 <= creep_factor <= 0.99) */
float creep_substep; /**< how much of the last count to creep (0.01 <= creep_substep <= 0.99) */
int creep_val;
int preseek; /**< Flag = 1 if current move is a backlash preseek */
int run_flag; /**< Flag = 1 if RUN command has been issued and deferred */
@@ -693,14 +693,14 @@ static int motCreep(pDMC2280Driv self, double target) {
} else {
/*
* if the offset is less than or equal to the steps for one encoder count
* we have to "single step" otherwise we want to go half way
* we have to "single step" otherwise we want to go part way
*/
if (offset <= fabs(self->stepsPerX / self->cntsPerX)) {
offset = fabs(self->stepsPerX / self->cntsPerX) / DECADIC_CREEP;
offset = fabs(self->stepsPerX / self->cntsPerX) * self->creep_substep;
if (offset < 1)
offset = 1;
} else {
offset = offset / 2;
offset = offset * self->creep_factor;
}
}
/*
@@ -2101,28 +2101,9 @@ static void DMCState_Init(pDMC2280Driv self, pEvtEvent event) {
/* FIXME: FIXME_DOUG */
return;
}
#ifdef SPEED_ON_MOVE
/* Defer Speed, Accel and Decel to move command */
#else
/* Set speed */
value = motSpeed(self, self->speed);
snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
break;
}
/* Set acceleration */
value = motAccel(self, self->accel);
snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
break;
}
/* Set deceleration */
value = motDecel(self, self->decel);
snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value);
if (FAILURE == DMC_Send(self, cmd)) {
break;
}
#endif
cmdVars(self);
self->subState = 1;
return;
@@ -2460,11 +2441,10 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
return;
}
#endif
#ifdef SPEED_ON_MOVE
cmdSpeed(self); /* No Response */
cmdAccel(self); /* No Response */
cmdDecel(self); /* No Response */
#endif
self->minPosition = self->currPosition;
self->maxPosition = self->currPosition;
self->stepCount = 0;
@@ -4114,6 +4094,14 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->creep_precision;
return 1;
}
if(strcasecmp(name,"creep_factor") == 0) {
*fValue = self->creep_factor;
return 1;
}
if(strcasecmp(name,"creep_substep") == 0) {
*fValue = self->creep_substep;
return 1;
}
}
else {
if (strcasecmp(name,"homerun") == 0) {
@@ -4289,6 +4277,38 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
return 1;
}
}
/* Set creep_factor */
if (strcasecmp(name,"creep_factor") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
self->creep_factor = fabs(newValue);
if (self->creep_factor < 0)
self->creep_factor = -self->creep_factor;
if (self->creep_factor <= 0.01)
self->creep_factor = 0.01;
if (self->creep_factor >= 0.99)
self->creep_factor = 0.99;
return 1;
}
}
/* Set creep_substep */
if (strcasecmp(name,"creep_substep") == 0) {
if(!SCMatchRights(pCon,usMugger)) /* managers only */
return 1;
else {
self->creep_substep = fabs(newValue);
if (self->creep_substep < 0)
self->creep_substep = -self->creep_substep;
if (self->creep_substep <= 0.01)
self->creep_substep = 0.01;
if (self->creep_substep >= 0.99)
self->creep_substep = 0.99;
return 1;
}
}
}
else { /* If we do NOT have an absolute encoder */
@@ -4570,6 +4590,8 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Creep_Precision = %f\n", name, self->creep_precision);
snprintf(buffer, BUFFLEN, "%s.Creep_Factor = %f\n", name, self->creep_factor);
snprintf(buffer, BUFFLEN, "%s.Creep_Substep = %f\n", name, self->creep_substep);
SCWrite(pCon, buffer, eStatus);
}
if (self->posit_count > 0) {
@@ -4934,6 +4956,32 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
pNew->creep_precision = -pNew->creep_precision;
}
/* CREEP_FACTOR: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_factor",_OPTIONAL)) == NULL)
pNew->creep_factor = 0.5;
else {
sscanf(pPtr, "%f", &(pNew->creep_factor));
if (pNew->creep_factor < 0)
pNew->creep_factor = -pNew->creep_factor;
if (pNew->creep_factor <= 0.01)
pNew->creep_factor = 0.01;
if (pNew->creep_factor >= 0.99)
pNew->creep_factor = 0.99;
}
/* CREEP_substep: this controls unidirectional driving */
if ((pPtr=getParam(pCon, interp, params,"creep_substep",_OPTIONAL)) == NULL)
pNew->creep_substep = 0.1;
else {
sscanf(pPtr, "%f", &(pNew->creep_substep));
if (pNew->creep_substep < 0)
pNew->creep_substep = -pNew->creep_substep;
if (pNew->creep_substep <= 0.01)
pNew->creep_substep = 0.01;
if (pNew->creep_substep >= 0.99)
pNew->creep_substep = 0.99;
}
/* BIAS for encoder - add this to value read */
if ((pPtr=getParam(pCon, interp, params,"bias_bias",_OPTIONAL)) == NULL)
pNew->bias_bias = 0;