lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

- Ignore RDBD on 1st move.

Browse Source 
- Toggle DMOV on tweaks (TWF/TWR)that are < MRES.
- Remove soft travel-limit error checks from home search request.
- Moved synch'ing target position with readback to subroutine.
- Allow moving out of invalid soft limit travel range toward valid soft limit travel range.
- In-position retry mode for servos.
This commit is contained in:
Ron Sluiter
2013-06-17 14:31:34 +00:00
parent 8dfcc340fe
commit 85511023e9
2 changed files with 173 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/MotorSrc/motorRecord.cc
+169 -92
View File
@@ -161,6 +161,13 @@ HeadURL: $URL$
* alarms, events and the forward scan link in the same order
* as specified in the "EPICS Application Developer's Guide".
* .66 09-06-12 rls - Refix of DLY problem (see 64 above). Hold DMOV false until DLY times out.
* .67 06-12-13 rls - Ignore RDBD on 1st move.
* - Toggle DMOV on tweaks (TWF/TWR).
* - Remove soft travel-limit error checks from home search request.
* - Moved synch'ing target position with readback to subroutine.
* - Allow moving (new target position, jog or home search) out of invalid
* soft limit travel range toward valid soft limit travel range.
* - Added In-position retry mode for servos.
*
*/
@@ -186,6 +193,7 @@ HeadURL: $URL$
#include "motor.h"
#include "epicsExport.h"
#include "errlog.h"
volatile int motorRecordDebug = 0;
extern "C" {epicsExportAddress(int, motorRecordDebug);}
@@ -216,6 +224,7 @@ static void set_dial_lowlimit(motorRecord *, struct motor_dset *);
static void set_userlimits(motorRecord *);
static void range_check(motorRecord *, double *, double, double);
static void clear_buttons(motorRecord *);
static void syncTargetPosition(motorRecord *);
/*** Record Support Entry Table (RSET) functions. ***/
@@ -975,7 +984,7 @@ static void maybeRetry(motorRecord * pmr)
else
{
pmr->dmov = FALSE;
MARK(M_DMOV);
UNMARK(M_DMOV);
pmr->mip = MIP_RETRY;
}
MARK(M_RCNT);
@@ -1084,7 +1093,9 @@ LOGIC:
Mark MIP as changed.
ENDIF
ELSE
Set the Done Moving field (DMOV) TRUE and mark DMOV as changed.
IF Done Moving field is FALSE.
Set Done Moving field (DMOV) TRUE and mark DMOV as changed.
ENDIF
IF the High or Low limit switch is TRUE.
Set the Post Process field to TRUE.
ENDIF
@@ -1116,18 +1127,25 @@ LOGIC:
Clear Limit violation field.
ELSE
IF Jog indicator is true in MIP field.
Update Limit violation (LVIO) based on Jog direction (JOGF/JOGR) and VELO.
Update Limit violation (LVIO) based on Jog direction (JOGF/JOGR) and velocity (JVEL).
ELSE IF Homing indicator is true in MIP field.
Update Limit violation (LVIO) based on Home direction (HOMF/HOMR) and VELO.
ELSE
Update Limit violation (LVIO).
Set Limit violation (LVIO) FALSE.
ELSE IF Move indicator is true in MIP field.
Set Limit violation (LVIO) based on whether the dial target position (DVAL) is within
the dial high/low limit range (+/- retry deadband).
ENDIF
ENDIF
IF Limit violation (LVIO) has changed.
Mark LVIO as changed.
IF Limit violation (LVIO) is TRUE, AND, SET is false (i.e., Use/Set is Set).
Mark LVIO as changed. Set stopmove indicator FALSE.
IF Move indicator is true in MIP field.
IF Limit violation (LVIO) is TRUE, AND, DVAL > DHLM, AND, command direction is Negative (cdir == 0).
Set stopmove indicator TRUE.
ElSE IF Limit violation (LVIO) is TRUE, AND, DVAL < DLLM, AND, command direction is Positive (cdir == 1).
Set stopmove indicator TRUE.
ENDIF
IF stopmove indicator is TRUE, AND, Limit violation (LVIO) is TRUE, AND, SET is false (i.e., Use/Set is Set).
Set STOP field true.
Clear JOGF and JOGR fields.
Clear jog and home requests.
ENDIF
ENDIF
IF STOP field is true, OR,
@@ -1246,7 +1264,7 @@ static long process(dbCommon *arg)
/* Motor has stopped. */
/* Assume we're done moving until we find out otherwise. */
if (pmr->dmov != TRUE)
if (pmr->dmov == FALSE)
{
pmr->dmov = TRUE;
MARK(M_DMOV);
@@ -1305,23 +1323,30 @@ static long process(dbCommon *arg)
pmr->mip &= ~MIP_DELAY;
MARK(M_MIP); /* done delaying */
maybeRetry(pmr);
if (pmr->mip == MIP_RETRY && pmr->rmod == motorRMOD_I)
{
pmr->mip |= MIP_DELAY_REQ;
MARK(M_MIP);
Debug(3, "callbackRequestDelayed() called\n");
callbackRequestDelayed(&pcallback->dly_callback, pmr->dly);
}
}
}
else if (MARKED(M_DMOV))
{
if (!(pmr->mip & MIP_DELAY_REQ))
{
pmr->mip |= MIP_DELAY_REQ;
MARK(M_MIP);
else if (MARKED(M_DMOV))
{
if (!(pmr->mip & MIP_DELAY_REQ))
{
pmr->mip |= MIP_DELAY_REQ;
MARK(M_MIP);
Debug(3, "callbackRequestDelayed() called\n");
callbackRequestDelayed(&pcallback->dly_callback, pmr->dly);
}
callbackRequestDelayed(&pcallback->dly_callback, pmr->dly);
}
/* Restore DMOV to false and UNMARK it so it is not posted. */
pmr->dmov = FALSE;
UNMARK(M_DMOV);
goto process_exit;
}
/* Restore DMOV to false and UNMARK it so it is not posted. */
pmr->dmov = FALSE;
UNMARK(M_DMOV);
goto process_exit;
}
}
}
} /* END of (process_reason == CALLBACK_DATA). */
@@ -1334,25 +1359,37 @@ enter_do_work:
else
{
if (pmr->mip & MIP_JOG)
pmr->lvio = (pmr->jogf && (pmr->drbv > pmr->dhlm - pmr->velo)) ||
(pmr->jogr && (pmr->drbv < pmr->dllm + pmr->velo));
pmr->lvio = (pmr->jogf && (pmr->drbv > pmr->dhlm - pmr->jvel)) ||
(pmr->jogr && (pmr->drbv < pmr->dllm + pmr->jvel));
else if (pmr->mip & MIP_HOME)
pmr->lvio = false; /* Disable soft-limit error check during home search. */
else
pmr->lvio = (pmr->drbv > pmr->dhlm + fabs(pmr->mres)) ||
(pmr->drbv < pmr->dllm - fabs(pmr->mres));
else if (pmr->mip & MIP_MOVE)
pmr->lvio = (pmr->dval > pmr->dhlm + fabs(pmr->rdbd)) ||
(pmr->dval < pmr->dllm - fabs(pmr->rdbd));
}
if (pmr->lvio != old_lvio)
{
bool stopmove = true;
Debug(4, "process: lvio changed to %d\n", pmr->lvio);
MARK(M_LVIO);
if (pmr->lvio && !pmr->set)
if (pmr->mip & MIP_MOVE)
{
if (pmr->lvio && ((pmr->dval > pmr->dhlm) && (pmr->cdir == 0)))
stopmove = false;
else if (pmr->lvio && ((pmr->dval < pmr->dhlm) && (pmr->cdir == 1)))
stopmove = false;
}
if (stopmove == true && pmr->lvio && !pmr->set)
{
pmr->stop = 1;
MARK(M_STOP);
clear_buttons(pmr);
}
}
/* Do we need to examine the record to figure out what work to perform? */
if (pmr->stop || (pmr->spmg == motorSPMG_Stop) ||
(pmr->spmg == motorSPMG_Pause) ||
@@ -1480,25 +1517,29 @@ LOGIC:
ENDIF
Set Undefined Link indicator (UDF) FALSE.
ELSE
IF No Limit violation, AND, (Homing forward/OR/reverse request, AND,
NOT processing Homing forward/OR/reverse, AND, NOT At
High/OR/Low Limit Switch)
IF Homing forward/OR/reverse request, AND, NOT processing Homing
forward/OR/reverse, AND, NOT At High/OR/Low Limit Switch)
IF (STOPPED, OR, PAUSED)
Set DMOV FALSE (Home command will be processed from
postProcess() when SPMG is set to GO).
ENDIF
IF (Software Travel limits are NOT disabled), AND,
(Home Forward, AND, (DVAL > DHLM - VELO)), OR,
(Home Reverse, AND, (DVAL < DLLM + VELO)))
Set Limit violation field true.
NORMAL RETURN.
ENDIF
...
...
Set MIP based on HOMF or HOMR and MARK it.
Set post process TRUE.
IF Motor is moving (MOVN).
Set MIP for STOP and MARK it.
Send STOP command.
ELSE
Send Home velocity and acceleration commands.
Send Home command.
Set DMOV false and MARK it.
Clear retry count (RCNT) and MARK it.
Set commanded direction (CDIR) based on home direction MRES polarity.
ENDIF
NORMAL RETURN.
ENDIF
IF NOT currently jogging, AND, NOT (STOPPED, OR, PAUSED), AND,
No Limit violation, AND, Jog Request is true.
IF NOT currently jogging, AND, NOT (STOPPED, OR, PAUSED), AND, Jog Request is true.
IF (Forward jog, AND, DVAL > [DHLM - VELO]), OR,
(Reverse jog, AND, DVAL > [DLLM + VELO])
Set limit violation (LVIO) true.
@@ -1565,8 +1606,17 @@ LOGIC:
ENDIF
Set VAL and RVAL based on DVAL; mark VAL and RVAL for posting.
IF new raw commanded position = current raw feedback position.
Default too_small to FALSE.
IF this is not a retry.
IF the change in raw position < 1
Set too_small to TRUE.
ENDIF
ELSE IF the change in raw position < RDBD
Set too_small to TRUE.
ENDIF
IF too_small is TRUE.
IF not done moving, AND, [either no motion-in-progress, OR,
retry-in-progress].
Set done moving TRUE.
@@ -1574,9 +1624,9 @@ LOGIC:
NOTE: maybeRetry() can send control here even though the
move is to the same raw position.
ENDIF
Restore previous target positions.
ENDIF
IF this is not a retry.
Reset retry counter and mark RCNT for dbposting.
ENDIF
@@ -1739,14 +1789,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
clear_buttons(pmr);
}
else
{
pmr->val = pmr->lval = pmr->rbv;
MARK(M_VAL);
pmr->dval = pmr->ldvl = pmr->drbv;
MARK(M_DVAL);
pmr->rval = pmr->lrvl = NINT(pmr->dval / pmr->mres);
MARK(M_RVAL);
}
syncTargetPosition(pmr); /* Synchronize target positions with readbacks. */
}
/* Cancel any operations. */
if (pmr->mip & MIP_HOME)
@@ -1859,8 +1902,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
/** Check out all the buttons and other sources of motion **/
/* Send motor to home switch in forward direction. */
if (!pmr->lvio &&
((pmr->homf && !(pmr->mip & MIP_HOMF) && !((pmr->dir == motorDIR_Pos) ? pmr->hls : pmr->lls)) ||
if (((pmr->homf && !(pmr->mip & MIP_HOMF) && !((pmr->dir == motorDIR_Pos) ? pmr->hls : pmr->lls)) ||
(pmr->homr && !(pmr->mip & MIP_HOMR) && !((pmr->dir == motorDIR_Pos) ? pmr->lls : pmr->hls))))
{
if (stop_or_pause == true)
@@ -1869,26 +1911,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
MARK(M_DMOV);
return(OK);
}
/* check for limit violation */
if ((pmr->dhlm == pmr->dllm) && (pmr->dllm == 0.0))
;
else if ((pmr->homf && (pmr->dval > pmr->dhlm - pmr->velo)) ||
(pmr->homr && (pmr->dval < pmr->dllm + pmr->velo)))
{
pmr->lvio = 1; /* Set limit violation ON. */
MARK(M_LVIO);
if (pmr->homf)
{
pmr->homf = 0; /* Clear Home Forward request. */
MARK_AUX(M_HOMF);
}
if (pmr->homr)
{
pmr->homr = 0; /* Clear Home Reverse request. */
MARK_AUX(M_HOMR);
}
return(OK);
}
pmr->mip = pmr->homf ? MIP_HOMF : MIP_HOMR;
MARK(M_MIP);
pmr->pp = TRUE;
@@ -1947,14 +1970,14 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
* Jog motor. Move continuously until we hit a software limit or a
* limit switch, or until user releases button.
*/
if (!(pmr->mip & MIP_JOG) && stop_or_pause == false && !pmr->lvio &&
if (!(pmr->mip & MIP_JOG) && stop_or_pause == false &&
(pmr->mip & MIP_JOG_REQ))
{
/* check for limit violation */
if ((pmr->dhlm == pmr->dllm) && (pmr->dllm == 0.0))
;
else if ((pmr->jogf && (pmr->dval > pmr->dhlm - pmr->velo)) ||
(pmr->jogr && (pmr->dval < pmr->dllm + pmr->velo)))
else if ((pmr->jogf && (pmr->dval > pmr->dhlm - pmr->jvel)) ||
(pmr->jogr && (pmr->dval < pmr->dllm + pmr->jvel)))
{
pmr->lvio = 1;
MARK(M_LVIO);
@@ -2094,6 +2117,10 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
/* Record limit violation */
if ((pmr->dhlm == pmr->dllm) && (pmr->dllm == 0.0))
pmr->lvio = false;
/* LVIO = TRUE, AND, Move request towards valid travel limit range. */
else if (pmr->lvio && (((pmr->dval > pmr->dhlm) && (pmr->dval < pmr->ldvl)) ||
((pmr->dval < pmr->dllm) && (pmr->dval > pmr->ldvl))))
pmr->lvio = false;
else
pmr->lvio = (pmr->dval > pmr->dhlm) ||
(pmr->dval > pmr->dhlm + pmr->bdst) ||
@@ -2110,7 +2137,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
MARK(M_DVAL);
pmr->rval = pmr->lrvl;
MARK(M_RVAL);
if (pmr->mip == MIP_DONE)
if (pmr->mip == MIP_DONE && pmr->dmov == FALSE)
{
pmr->dmov = TRUE;
MARK(M_DMOV);
@@ -2124,10 +2151,18 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
/* IF DVAL field has changed, OR, NOT done moving. */
if (pmr->dval != pmr->ldvl || !pmr->dmov)
{
epicsFloat64 localDiff;
if (pmr->dval != pmr->ldvl)
MARK(M_DVAL);
pmr->diff = pmr->dval - pmr->drbv;
MARK(M_DIFF);
localDiff = pmr->dval - pmr->drbv;
if (pmr->diff != localDiff)
{
pmr->diff = localDiff;
MARK(M_DIFF);
}
pmr->rdif = NINT(pmr->diff / pmr->mres);
MARK(M_RDIF);
if (set)
@@ -2152,7 +2187,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
double bpos = (pmr->dval - pmr->bdst) / pmr->mres;
double bvel = pmr->bvel / fabs(pmr->mres); /* backlash speed */
double bacc = (bvel - vbase) / pmr->bacc; /* backlash accel. */
bool use_rel, preferred_dir;
bool use_rel, preferred_dir, too_small;
double relpos = pmr->diff / pmr->mres;
double relbpos = ((pmr->dval - pmr->bdst) - pmr->drbv) / pmr->mres;
double rbdst1 = 1.0 + (fabs(pmr->bdst) / fabs(pmr->mres));
@@ -2162,7 +2197,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
msta.All = pmr->msta;
/*** Use if encoder or ReadbackLink is in use. ***/
if (pmr->rtry != 0 && ((msta.Bits.EA_PRESENT && pmr->ueip) || pmr->urip))
if (pmr->rtry != 0 && pmr->rmod != motorRMOD_I && ((msta.Bits.EA_PRESENT && pmr->ueip) || pmr->urip))
use_rel = true;
else
use_rel = false;
@@ -2184,7 +2219,16 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
rpos = NINT(rbvpos);
npos = NINT(newpos);
if (abs(npos - rpos) < rdbdpos)
too_small = false;
if ((pmr->mip & MIP_RETRY) == 0)
{
if (abs(npos - rpos) < 1)
too_small = true;
}
else if (abs(npos - rpos) < rdbdpos)
too_small = true;
if (too_small == true)
{
if (pmr->dmov == FALSE && (pmr->mip == MIP_DONE || pmr->mip == MIP_RETRY))
{
@@ -2236,7 +2280,12 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
if (fabs(relbpos) < 1.0)
relbpos = (relbpos > 0.0) ? 1.0 : -1.0;
}
else if (pmr->rmod == motorRMOD_I) /* DC motor like In-position retries. */
return(OK);
else if (pmr->rmod == motorRMOD_D) /* Do default, linear, retries. */
;
else
errPrintf(-1, __FILE__, __LINE__, "Invalid RMOD field value: = %d", pmr->rmod);
if (((use_rel == false) && ((pmr->dval > pmr->ldvl) == (pmr->bdst > 0))) ||
((use_rel == true) && ((pmr->diff > 0) == (pmr->bdst > 0))))
@@ -2331,13 +2380,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
}
else if (pmr->sync != 0 && pmr->mip == MIP_DONE)
{
/* Sync target positions with readbacks. */
pmr->val = pmr->lval = pmr->rbv;
MARK(M_VAL);
pmr->dval = pmr->ldvl = pmr->drbv;
MARK(M_DVAL);
pmr->rval = pmr->lrvl = NINT(pmr->dval / pmr->mres);
MARK(M_RVAL);
syncTargetPosition(pmr); /* Sync target positions with readbacks. */
pmr->sync = 0;
db_post_events(pmr, &pmr->sync, DBE_VAL_LOG);
}
@@ -2394,6 +2437,8 @@ static long special(DBADDR *paddr, int after)
case motorRecordDVAL:
case motorRecordRVAL:
case motorRecordRLV:
case motorRecordTWF:
case motorRecordTWR:
if (pmr->disa == pmr->disv || pmr->disp)
return(OK);
if (pmr->dmov == TRUE)
@@ -3979,3 +4024,35 @@ static void clear_buttons(motorRecord *pmr)
}
}
/*
FUNCTION... void syncTargetPosition(motorRecord *)
USAGE... Synchronize target positions with readbacks.
*/
static void syncTargetPosition(motorRecord *pmr)
{
int dir = (pmr->dir == motorDIR_Pos) ? 1 : -1;
msta_field msta;
msta.All = pmr->msta;
if (msta.Bits.EA_PRESENT && pmr->ueip)
{
/* An encoder is present and the user wants us to use it. */
pmr->rrbv = pmr->rep;
pmr->drbv = pmr->rrbv * pmr->eres;
}
else
{
pmr->rrbv = pmr->rmp;
pmr->drbv = pmr->rrbv * pmr->mres;
}
pmr->rbv = pmr->drbv * dir + pmr->off;
pmr->val = pmr->lval = pmr->rbv ;
MARK(M_VAL);
pmr->dval = pmr->ldvl = pmr->drbv;
MARK(M_DVAL);
pmr->rval = pmr->lrvl = NINT(pmr->dval / pmr->mres);
MARK(M_RVAL);
}
motorApp/MotorSrc/motorRecord.dbd
+4 -1
View File
@@ -3,7 +3,7 @@
# Version: $Revision$
# Modified By: $Author$
# Last Modified: $Date$
# HeadURL: $URL $
# HeadURL: $URL$
# Experimental Physics and Industrial Control System (EPICS)
@@ -70,6 +70,7 @@ menu(motorRMOD) {
choice(motorRMOD_D,"Default")
choice(motorRMOD_A,"Arithmetic")
choice(motorRMOD_G,"Geometric")
choice(motorRMOD_I,"In-Position")
}
include "menuOmsl.dbd"
@@ -483,12 +484,14 @@ recordtype(motor) {
field(TWF,DBF_SHORT) {
asl(ASL0)
prompt("Tweak motor Forward")
special(SPC_MOD)
pp(TRUE)
interest(1)
}
field(TWR,DBF_SHORT) {
asl(ASL0)
prompt("Tweak motor Reverse")
special(SPC_MOD)
pp(TRUE)
interest(1)
}