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

First pass at Universal Unidirectional Motor Driving (UUMD)

Browse Source 
r1849 | dcl | 2007-04-12 16:59:32 +1000 (Thu, 12 Apr 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-04-12 16:59:32 +10:00
parent cff3ac6553
commit a3ec8b98a0
1 changed files with 49 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
site_ansto/motor_dmc2280.c
View File

@@ -100,13 +100,14 @@ typedef struct __MoDriv {
float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */
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 fTarget;
int settle;
struct timeval time_settle_done;
int airpad_state;
int airpad_counter;
pNWTimer airpad_timer;
pNWTimer motor_timer;
float backlash_offset; /**< signed offset to drive from */
float fTarget; /**< target passed from SICS to timer callback */
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 */
int airpad_counter; /**< airpad timer retry counter */
pNWTimer airpad_timer; /**< timer waiting for airpad action to complete */
pNWTimer motor_timer; /**< motor off timer */
int debug;
} DMC2280Driv, *pDMC2280Driv;
@@ -711,6 +712,17 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
snprintf(BGx, CMDLEN, "BG%c", axis);
target = fValue - self->home;
if (self->backlash_offset > FLT_EPSILON && target < self->lastPosition) {
target += self->backlash_offset;
if (target > self->fUpper)
target = self->fUpper;
}
if (self->backlash_offset < -FLT_EPSILON && target > self->lastPosition) {
target += self->backlash_offset;
if (target < self->fLower)
target = self->fLower;
}
if (1 == self->abs_endcoder) {
absEncHome = self->absEncHome;
cntsPerX = self->cntsPerX;
@@ -762,7 +774,7 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
else
break;
snprintf(absPosCmd, CMDLEN,
"PA%c=(((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c",
"PA%c=((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c",
axis,
absEncHome,
axis,
@@ -956,12 +968,11 @@ static int DMC2280Run(void *pData,float fValue){
* Note: this will read the current position which will block
*/
do {
#if 1
self->time_lastPos_set.tv_sec = 0;
#else
float currPos;
DMC2280GetPos(pData, &currPos);
set_lastPos(pData, currPos);
self->lastPosition = currPos;
#if 1
self->time_lastPos_set.tv_sec = 0;
#endif
} while (0);
@@ -1503,6 +1514,10 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->blockage_fail;
return 1;
}
if(strcasecmp(name,"backlash_offset") == 0) {
*fValue = self->backlash_offset;
return 1;
}
if (self->abs_endcoder != 0) {
if (strcasecmp(name,"absenc") == 0) {
if (readAbsEnc(self, fValue) == SUCCESS)
@@ -1678,6 +1693,17 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
}
}
/* Set backlash offset,
* managers only */
if(strcasecmp(name,"backlash_offset") == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->backlash_offset = newValue;
return 1;
}
}
/* Invoke Home Run routine in controller,
* managers only */
if(self->abs_endcoder == 0 && strcasecmp(name,"homerun") == 0) {
@@ -1796,6 +1822,8 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, ((pDMC2280Driv)self)->blockage_fail);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %d\n", name, ((pDMC2280Driv)self)->backlash_offset);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads);
SCWrite(pCon, buffer, eStatus);
snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder);
@@ -1943,6 +1971,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
pNew->blockage_thresh = 0.5;
pNew->blockage_ratio = 2.0;
pNew->blockage_fail = 0;
pNew->backlash_offset = 0.0;
/* PARAMETERS: Fetch parameter values */
if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) {
@@ -2019,6 +2048,13 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
sscanf(pPtr,"%d",&(pNew->settle));
}
/* BACKLASH: this motor need airpads */
if ((pPtr=getParam(pCon, interp, params,"backlash_offset",_OPTIONAL)) == NULL)
pNew->backlash_offset=0.0;
else {
sscanf(pPtr,"%f",&(pNew->backlash_offset));
}
/* AIRPADS: this motor need airpads */
if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL)
pNew->has_airpads=0;
@@ -2036,7 +2072,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
else
sscanf(pPtr,"%d",&(pNew->absEncHome));
if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL)
pNew->cntsPerX=1;
pNew->cntsPerX=1.0;
else
sscanf(pPtr,"%f",&(pNew->cntsPerX));
}