diff --git a/motorApp/MotorSrc/motorRecord.cc b/motorApp/MotorSrc/motorRecord.cc index 80e98f9a..2ba95bbd 100644 --- a/motorApp/MotorSrc/motorRecord.cc +++ b/motorApp/MotorSrc/motorRecord.cc @@ -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); +} + diff --git a/motorApp/MotorSrc/motorRecord.dbd b/motorApp/MotorSrc/motorRecord.dbd index 0083d0b4..4040234b 100644 --- a/motorApp/MotorSrc/motorRecord.dbd +++ b/motorApp/MotorSrc/motorRecord.dbd @@ -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) }