From 52ec125feb74e55585f3cbe31355541fd3e40c2c Mon Sep 17 00:00:00 2001 From: timmmooney Date: Wed, 29 Jan 2014 18:20:58 +0000 Subject: [PATCH] Significant changes to support adding accel/decel segments to trajectory, post motor-start positions after build, forbid hybrid mode (not supported), in TIME_MODE_TOTAL, timeTrajectory had extra point at end, buildTrajectory was using npulses instead of npoints, don't change MAXv's update freq at run time --- motorApp/OmsSrc/MAX_trajectoryScan.st | 208 ++++++++++++++++++++++---- 1 file changed, 175 insertions(+), 33 deletions(-) diff --git a/motorApp/OmsSrc/MAX_trajectoryScan.st b/motorApp/OmsSrc/MAX_trajectoryScan.st index 0b76496b..a2ce2d30 100644 --- a/motorApp/OmsSrc/MAX_trajectoryScan.st +++ b/motorApp/OmsSrc/MAX_trajectoryScan.st @@ -39,8 +39,10 @@ option +r; * uses MAX_AXES*MAX_ELEMENTS*8 bytes in this SNL program (up to 128KB). * Similar memory will be required for the records in the database. * (Note that currently MAX_AXES is fixed at 8, in trajectoryScan.h.) + * MAX_ELEMENTS_P2 = MAX_ELEMENTS+2 is needed for accel/decel */ #define MAX_ELEMENTS 1000 +#define MAX_ELEMENTS_P2 1002 /* Maximum # of output pulses. For now, we emit a pulse at the beginning of * every trajectory element. @@ -121,6 +123,10 @@ int nextra; int npoints; double dtime; double dpos; +double accelDist[MAX_AXES]; +double decelDist[MAX_AXES]; +%%double aDist; +%%double dDist; double posActual; double posTheory; double expectedTime; @@ -158,19 +164,24 @@ unsigned long startTime; %% static int getMotorMoving(SS_ID ssId, struct UserVar *pVar, int movingMask); %% static int getEpicsMotorMoving(SS_ID ssId, struct UserVar *pVar); %% static int waitEpicsMotors(SS_ID ssId, struct UserVar *pVar); -%% static int buildTrajectory(SS_ID ssId, struct UserVar *pVar, double *timeTrajectory, -%% double *motorTrajectory, int epicsMotorDir, int moveMode, int npoints, int npulses, double motorOffset, -%% double motorResolution, double motorVmin, int *position, int *velocity, int *acceleration); +%% static int buildTrajectory(SS_ID ssId, struct UserVar *pVar, double *realTimeTrajectory, +%% double *motorTrajectory, int epicsMotorDir, int moveMode, int npoints, double motorOffset, +%% double motorResolution, double motorVmin, int *position, int *velocity, int *acceleration, +%% double *accelDist, double *decelDist); %% static int loadTrajectory(SS_ID ssId, struct UserVar *pVar, int simMode); %% static int getStarted(SS_ID ssId, struct UserVar *pVar); %% static int userToRaw(double user, double off, int dir, double res); %% static double rawToUser(int raw, double off, int dir, double res); -int position[MAX_AXES][MAX_ELEMENTS]; -int velocity[MAX_AXES][MAX_ELEMENTS]; -int acceleration[MAX_AXES][MAX_ELEMENTS]; -double motorStart[MAX_AXES]; +int position[MAX_AXES][MAX_ELEMENTS_P2]; +int velocity[MAX_AXES][MAX_ELEMENTS_P2]; +int acceleration[MAX_AXES][MAX_ELEMENTS_P2]; +/* if we're adding accel/decel points, we need a copy of realTimeTrajectory we can modify. */ +double realTimeTrajectoryAccelDecel[MAX_ELEMENTS_P2]; +double *rttraj; + int motorStartRaw[MAX_AXES]; +double motorEnd[MAX_AXES]; double dbuf[MAX_PULSES]; /* variables for constructing trajectory commands */ @@ -181,6 +192,7 @@ int waitingForTrigger; double vmax; double amax; + ss maxTrajectoryScan { /* Initialize things when first starting */ @@ -270,6 +282,9 @@ ss maxTrajectoryScan { efClear(readbackMon); efClear(nelementsMon); efClear(motorMDVSMon); /* we don't use this */ + efClear(moveModeMon); + + moveModePrev = moveMode; if (initStatus == STATUS_UNDEFINED) initStatus = STATUS_SUCCESS; } state monitor_inputs } @@ -296,6 +311,18 @@ ss maxTrajectoryScan { when(efTestAndClear(motorMDVSMon)) { /* We don't use this. */ } state monitor_inputs + + when (efTestAndClear(moveModeMon)) { + /* We don't support hybrid mode (yet?). */ + if (moveMode == MOVE_MODE_HYBRID) { + moveMode = moveModePrev; + pvPut(moveMode); + } else { + moveModePrev = moveMode; + } + buildStatus = STATUS_UNDEFINED; + pvPut(buildStatus); + } state monitor_inputs } /* Build trajectory */ @@ -313,9 +340,11 @@ ss maxTrajectoryScan { buildStatus = STATUS_SUCCESS; /* presume we'll be successful */ /* Initialize new trajectory */ /* If time mode is TIME_MODE_TOTAL then construct timeTrajectory and post it */ + /* Note that timeTrajectory[i] is the time to go from motorTrajectory[i] to motorTrajectory[i+1] */ if (timeMode == TIME_MODE_TOTAL) { dtime = time/(nelements-1); - for (i=0; irealTimeTrajectory, pVar->motorTrajectory[pVar->j], - %% pVar->epicsMotorDir[pVar->j], pVar->moveMode, pVar->npoints, pVar->npulses, + rttraj = realTimeTrajectoryAccelDecel; + if (addAccelDecel) rttraj = realTimeTrajectoryAccelDecel; + %%buildTrajectory(ssId, pVar, pVar->rttraj, pVar->motorTrajectory[pVar->j], + %% pVar->epicsMotorDir[pVar->j], pVar->moveMode, pVar->npoints, %% pVar->epicsMotorOff[pVar->j], pVar->epicsMotorMres[pVar->j], pVar->epicsMotorVMIN[pVar->j], - %% pVar->position[pVar->j], pVar->velocity[pVar->j], pVar->acceleration[pVar->j]); + %% pVar->position[pVar->j], pVar->velocity[pVar->j], pVar->acceleration[pVar->j], + %% &aDist, &dDist); + %%pVar->accelDist[pVar->j] = aDist; + %%pVar->decelDist[pVar->j] = dDist; } } /* Compute expected time for trajectory. This includes timeScale factor. */ - expectedTime = realTimeTrajectory[npoints-1]; + if (addAccelDecel) { + /* Accel/decel segments are not scaled with timeScale factor. */ + expectedTime = realTimeTrajectoryAccelDecel[npoints+1]; + } else { + expectedTime = realTimeTrajectory[npoints-1]; + } /* Check trajectories against motor soft limits */ limitViolation = 0; @@ -381,6 +424,33 @@ ss maxTrajectoryScan { if (limitViolation) { buildStatus = STATUS_FAILURE; } + + /* Post start positions. For relative scans, positions will be recalculated at exec time. */ + for (j=0; jmotorCurrent, pVar->motorCurrentRaw, &(pVar->dtime)); + for (j=0; j0.) && (dtime > realTimeTrajectory[i]); i++); + /* bracket dtime in the interval [realTimeTrajectoryAccelDecel[i], realTimeTrajectoryAccelDecel[i+1]] */ + for (i=lastRealTimePoint; (i0.) && (dtime > realTimeTrajectoryAccelDecel[i]); i++); i--; if (i<0) i = 0; if (doPoll && (i > 2) && (i < npoints-2) && (overrideFactor >= .01) && (currPulse < MAX_PULSES-1)) { - if (debugLevel >= 10) printf("wait_execute: time=%f, i=%d, realTimeTrajectory[i]=%f\n", - dtime, i, realTimeTrajectory[i]); - frac = (dtime - realTimeTrajectory[i]) / (realTimeTrajectory[i+1] - realTimeTrajectory[i]); + if (debugLevel >= 10) printf("wait_execute: time=%f, i=%d, realTimeTrajectoryAccelDecel[i]=%f\n", + dtime, i, realTimeTrajectoryAccelDecel[i]); + frac = (dtime - realTimeTrajectoryAccelDecel[i]) / (realTimeTrajectoryAccelDecel[i+1] - realTimeTrajectoryAccelDecel[i]); posTheory = motorTrajectory[j][i] + frac * (motorTrajectory[j][i+1] - motorTrajectory[j][i]); if (moveMode != MOVE_MODE_ABSOLUTE) { posTheory += motorStart[j]; @@ -545,7 +634,7 @@ ss maxTrajectoryScan { (motorError[j][currPulse] - motorError[j][currPulse-1]); /* change in speed needed to make up the position in a time equal to the length of the current * segment */ - deltaV = dpos / (realTimeTrajectory[i+1] - realTimeTrajectory[i]); + deltaV = dpos / (realTimeTrajectoryAccelDecel[i+1] - realTimeTrajectoryAccelDecel[i]); vO = (1-(deltaV/v)*overrideFactor) * 100; %%pVar->vOverride = NINT(pVar->vO); if (vOverride<80) vOverride=80; @@ -608,10 +697,13 @@ ss maxTrajectoryScan { pvPut(execMessage); /* Get the current motor positions, post them */ %%getMotorPositions(ssId, pVar, pVar->motorCurrent, pVar->motorCurrentRaw, &(pVar->dtime)); - for (j=0; jaddAccelDecel */ + *decelDist = 0; + + if (pVar->addAccelDecel) { + dp = motorTrajectory[1] - motorTrajectory[0]; + dt = realTimeTrajectory[2] - realTimeTrajectory[1]; + *accelDist = (dp/dt * pVar->accel) / 2; + /* Average speed over last trajectory segment is the speed from which we want to decelerate to 0 speed. */ + dp = motorTrajectory[npoints-1] - motorTrajectory[npoints-2]; + dt = realTimeTrajectory[npoints] - realTimeTrajectory[npoints-1]; + /* Note that accel_time == decel_time, but the distances are different. */ + *decelDist = (dp/dt * pVar->accel) / 2; + + /* Shift motorTrajectory forward by one element; prepend accel, append decel; restore before returning. + * realTimeTrajectory was modified for us by caller. */ + for (i=npoints; i>0; i--) { + motorTrajectory[i] = motorTrajectory[i-1]; + } + motorTrajectory[0] = motorTrajectory[1] - *accelDist; + motorTrajectory[npoints+1] = motorTrajectory[npoints] + *decelDist; + npoints += 2; + } + calcMotorTrajectory[0] = motorTrajectory[0]; v_out[0] = 0; if (pVar->debugLevel >= 7) { - printf("###:%8s %8s %7s %8s %8s %8s %8s %8s\n", + printf("\n###:%8s %8s %7s %8s %8s %8s %8s %8s\n", "pos", "calcPos", "dp", "t", "v_ideal", "accel_p", "accel_v", "accel_s"); } for (i=1; idebugLevel >= 1) printf("%10.2f %10d %10d %10d %10d\n", time, position[i], NINT(x0), velocity[i], acceleration[i]); } + if (pVar->addAccelDecel) { + /* Shift motorTrajectory backward by one element, and clear decel element. */ + npoints -= 2; + for (i=0; istartPulses; + endPulses = pVar->endPulses; + npoints = pVar->npoints; + + if (pVar->addAccelDecel) { + /* We assume the arrays pVar->position, pVar->acceleration, pVar->velocity, and pVar->realTimeTrajectoryAccelDecel + * have had accel/decel trajectory points added. */ + startPulses++; + endPulses++; + npoints+=2; + } sprintf(stringOut, "AM;"); /* multitasking mode, flush queue */ writeOnly(ssId, pVar, stringOut); @@ -1058,7 +1197,8 @@ static int loadTrajectory(SS_ID ssId, struct UserVar *pVar, int simMode) { epicsTimeGetCurrent(&eStartTime); /* not actually the start time, getMotorPositions just needs a value */ getMotorPositions(ssId, pVar, pVar->motorCurrent, pVar->motorCurrentRaw, &(pVar->dtime)); -#if 1 +#if 0 + /* It's a bad idea to change updateFreq at run time. Better to set it at init time. */ /* Set update rate. */ i = 1024 * pVar->updateFreq; if (i != currUpdateRate) { @@ -1078,7 +1218,7 @@ static int loadTrajectory(SS_ID ssId, struct UserVar *pVar, int simMode) { #endif /* we may need current raw positions to mock up relative mode */ - epicsTimeGetCurrent(&eStartTime); /* not actually the start time, getMotorPositions just needs a value */ + epicsTimeGetCurrent(&eStartTime); /* not actually the start time, getMotorPositions just needs eStartTime to have a value. */ getMotorPositions(ssId, pVar, pVar->motorCurrent, pVar->motorCurrentRaw, &(pVar->dtime)); for (j=0, firstTask=1; jnpoints; i++) { + for (i=0; iacceleration[j][i] > 0) { segment_accel = pVar->acceleration[j][i]; segment_decel = pVar->acceleration[j][i]; @@ -1141,7 +1281,7 @@ static int loadTrajectory(SS_ID ssId, struct UserVar *pVar, int simMode) { /* time from the beginning of the trajectory segment at which the velocity reaches zero.*/ t_v0 = (double)(-segment_v_start) / pVar->acceleration[j][i]; - if ((t_v0 < .005) || (((pVar->realTimeTrajectory[i] - pVar->realTimeTrajectory[i-1]) - t_v0) < .005)) { + if ((t_v0 < .005) || (((pVar->realTimeTrajectoryAccelDecel[i] - pVar->realTimeTrajectoryAccelDecel[i-1]) - t_v0) < .005)) { /* Don't split very near either end of segment. */ if (pVar->debugLevel > 0) printf("declined to split segment at t=%f\n", t_v0); do_split = 0; @@ -1209,7 +1349,7 @@ static int loadTrajectory(SS_ID ssId, struct UserVar *pVar, int simMode) { } else { /* enable pulses if we're within user specified range */ if (firstTask && ((pVar->outBitNum >= 0) && (pVar->outBitNum <= 15))) { - if (i >= pVar->startPulses && i <= pVar->endPulses) { + if (i >= startPulses && i <= endPulses) { /* Enable output pulses */ if (!pulsesEnabled) { sprintf(stringOut, "AM; VIO[%d]%04x,%04x,%04x;", j+1, onMask, offMask, outMask); @@ -1251,3 +1391,5 @@ static int loadTrajectory(SS_ID ssId, struct UserVar *pVar, int simMode) { }% + +