diff --git a/motorApp/OmsSrc/MAX_trajectoryScan.h b/motorApp/OmsSrc/MAX_trajectoryScan.h
new file mode 100644
index 00000000..1638d1e4
--- /dev/null
+++ b/motorApp/OmsSrc/MAX_trajectoryScan.h
@@ -0,0 +1,260 @@
+/* MAX_trajectoryScan.h
+ * 
+ * This file is included in MAX_trajectoryScan.st.
+ */
+
+
+/* State codes for Build, Read and Execute. Careful, these must match the
+ * corresponding MBBI records, but there is no way to check this */
+#define BUILD_STATE_DONE            0
+#define BUILD_STATE_BUSY            1
+#define READ_STATE_DONE             0
+#define READ_STATE_BUSY             1
+#define EXECUTE_STATE_DONE          0
+#define EXECUTE_STATE_MOVE_START    1
+#define EXECUTE_STATE_EXECUTING     2
+#define EXECUTE_STATE_FLYBACK       3
+
+/* Status codes for Build, Execute and Read */
+#define STATUS_UNDEFINED 0
+#define STATUS_SUCCESS   1
+#define STATUS_FAILURE   2
+#define STATUS_ABORT     3
+#define STATUS_TIMEOUT   4
+
+/* Time modes */
+#define TIME_MODE_TOTAL         0
+#define TIME_MODE_PER_ELEMENT   1
+
+/* Move modes */
+#define MOVE_MODE_RELATIVE   0
+#define MOVE_MODE_ABSOLUTE   1
+#define MOVE_MODE_HYBRID     2
+
+/* The maximum number of axes per controller.  If this is changed from 8
+ * then many assign statements in this file must be changed */
+#define MAX_AXES 8
+
+/* Define PVs */
+int     debugLevel;  assign debugLevel   to "{P}{R}DebugLevel.VAL"; 
+monitor debugLevel;
+int     numAxes;     assign numAxes      to "{P}{R}NumAxes.VAL"; 
+monitor numAxes;
+int     nelements;   assign nelements    to "{P}{R}Nelements.VAL"; 
+monitor nelements;
+int     npulses;     assign npulses      to "{P}{R}Npulses.VAL"; 
+monitor npulses;
+int     startPulses; assign startPulses  to "{P}{R}StartPulses.VAL"; 
+monitor startPulses;
+int     endPulses;   assign endPulses    to "{P}{R}EndPulses.VAL"; 
+monitor endPulses;
+int     nactual;     assign nactual      to "{P}{R}Nactual.VAL"; 
+int     moveMode;    assign moveMode     to "{P}{R}MoveMode.VAL";    
+monitor moveMode;
+double  time;        assign time         to "{P}{R}Time.VAL";      
+monitor time;
+double  timeScale;   assign timeScale    to "{P}{R}TimeScale.VAL"; 
+monitor timeScale;
+int     timeMode;    assign timeMode     to "{P}{R}TimeMode.VAL";    
+monitor timeMode;
+double  accel;       assign accel        to "{P}{R}Accel.VAL";     
+monitor accel;
+int     initStatus; /* assign buildStatus  to "{P}{R}InitStatus.VAL"; (pv not in database)*/
+int     build;       assign build        to "{P}{R}Build.VAL";     
+monitor build;
+int     buildState;  assign buildState   to "{P}{R}BuildState.VAL"; 
+int     buildStatus; assign buildStatus  to "{P}{R}BuildStatus.VAL"; 
+string  buildMessage;assign buildMessage to "{P}{R}BuildMessage.VAL";
+int     simMode;     assign simMode      to "{P}{R}SimMode.VAL";   
+monitor simMode;
+int     execute;     assign execute      to "{P}{R}Execute.VAL";   
+monitor execute;
+int     execState;   assign execState    to "{P}{R}ExecState.VAL";  
+monitor execState; 
+int     execStatus;  assign execStatus   to "{P}{R}ExecStatus.VAL";   
+string  execMessage; assign execMessage  to "{P}{R}ExecMessage.VAL";
+int     abort;       assign abort        to "{P}{R}Abort.VAL";   
+monitor abort;
+int     readback;    assign readback     to "{P}{R}Readback.VAL";   
+monitor readback;
+int     readState;   assign readState    to "{P}{R}ReadState.VAL";   
+int     readStatus;  assign readStatus   to "{P}{R}ReadStatus.VAL";   
+string  readMessage; assign readMessage  to "{P}{R}ReadMessage.VAL";
+double  timeTrajectory[MAX_ELEMENTS];
+assign  timeTrajectory to  "{P}{R}TimeTraj.VAL"; 
+monitor timeTrajectory;
+string  trajectoryFile; assign trajectoryFile to "{P}{R}TrajectoryFile.VAL";
+monitor trajectoryFile;
+
+int     moveAxis[MAX_AXES]; 
+assign  moveAxis     to
+        {"{P}{R}M1Move.VAL",
+         "{P}{R}M2Move.VAL",
+         "{P}{R}M3Move.VAL",
+         "{P}{R}M4Move.VAL",
+         "{P}{R}M5Move.VAL",
+         "{P}{R}M6Move.VAL",
+         "{P}{R}M7Move.VAL",
+         "{P}{R}M8Move.VAL"};
+monitor moveAxis;
+
+double  motorTrajectory[MAX_AXES][MAX_ELEMENTS]; 
+assign  motorTrajectory to
+        {"{P}{R}M1Traj.VAL",
+         "{P}{R}M2Traj.VAL",
+         "{P}{R}M3Traj.VAL",
+         "{P}{R}M4Traj.VAL",
+         "{P}{R}M5Traj.VAL",
+         "{P}{R}M6Traj.VAL",
+         "{P}{R}M7Traj.VAL",
+         "{P}{R}M8Traj.VAL"};
+monitor motorTrajectory;
+
+double  motorReadbacks[MAX_AXES][MAX_PULSES]; 
+assign  motorReadbacks to
+        {"{P}{R}M1Actual.VAL",
+         "{P}{R}M2Actual.VAL",
+         "{P}{R}M3Actual.VAL",
+         "{P}{R}M4Actual.VAL",
+         "{P}{R}M5Actual.VAL",
+         "{P}{R}M6Actual.VAL",
+         "{P}{R}M7Actual.VAL",
+         "{P}{R}M8Actual.VAL"};
+
+double  motorError[MAX_AXES][MAX_PULSES]; 
+assign  motorError  to
+        {"{P}{R}M1Error.VAL",
+         "{P}{R}M2Error.VAL",
+         "{P}{R}M3Error.VAL",
+         "{P}{R}M4Error.VAL",
+         "{P}{R}M5Error.VAL",
+         "{P}{R}M6Error.VAL",
+         "{P}{R}M7Error.VAL",
+         "{P}{R}M8Error.VAL"};
+
+double  motorCurrent[MAX_AXES]; 
+assign  motorCurrent to
+        {"{P}{R}M1Current.VAL",
+         "{P}{R}M2Current.VAL",
+         "{P}{R}M3Current.VAL",
+         "{P}{R}M4Current.VAL",
+         "{P}{R}M5Current.VAL",
+         "{P}{R}M6Current.VAL",
+         "{P}{R}M7Current.VAL",
+         "{P}{R}M8Current.VAL"};
+
+int  motorCurrentRaw[MAX_AXES];
+
+double  motorMDVS[MAX_AXES]; 
+assign  motorMDVS   to
+        {"{P}{R}M1MDVS.VAL",
+         "{P}{R}M2MDVS.VAL",
+         "{P}{R}M3MDVS.VAL",
+         "{P}{R}M4MDVS.VAL",
+         "{P}{R}M5MDVS.VAL",
+         "{P}{R}M6MDVS.VAL",
+         "{P}{R}M7MDVS.VAL",
+         "{P}{R}M8MDVS.VAL"};
+monitor motorMDVS;
+
+double  motorMDVA[MAX_AXES]; 
+assign  motorMDVA   to
+        {"{P}{R}M1MDVA.VAL",
+         "{P}{R}M2MDVA.VAL",
+         "{P}{R}M3MDVA.VAL",
+         "{P}{R}M4MDVA.VAL",
+         "{P}{R}M5MDVA.VAL",
+         "{P}{R}M6MDVA.VAL",
+         "{P}{R}M7MDVA.VAL",
+         "{P}{R}M8MDVA.VAL"};
+
+int     motorMDVE[MAX_AXES]; 
+assign  motorMDVE   to
+        {"{P}{R}M1MDVE.VAL",
+         "{P}{R}M2MDVE.VAL",
+         "{P}{R}M3MDVE.VAL",
+         "{P}{R}M4MDVE.VAL",
+         "{P}{R}M5MDVE.VAL",
+         "{P}{R}M6MDVE.VAL",
+         "{P}{R}M7MDVE.VAL",
+         "{P}{R}M8MDVE.VAL"};
+
+double  motorMVA[MAX_AXES]; 
+assign  motorMVA    to
+        {"{P}{R}M1MVA.VAL",
+         "{P}{R}M2MVA.VAL",
+         "{P}{R}M3MVA.VAL",
+         "{P}{R}M4MVA.VAL",
+         "{P}{R}M5MVA.VAL",
+         "{P}{R}M6MVA.VAL",
+         "{P}{R}M7MVA.VAL",
+         "{P}{R}M8MVA.VAL"};
+
+int     motorMVE[MAX_AXES]; 
+assign  motorMVE    to
+        {"{P}{R}M1MVE.VAL",
+         "{P}{R}M2MVE.VAL",
+         "{P}{R}M3MVE.VAL",
+         "{P}{R}M4MVE.VAL",
+         "{P}{R}M5MVE.VAL",
+         "{P}{R}M6MVE.VAL",
+         "{P}{R}M7MVE.VAL",
+         "{P}{R}M8MVE.VAL"};
+
+double  motorMAA[MAX_AXES]; 
+assign  motorMAA    to
+        {"{P}{R}M1MAA.VAL",
+         "{P}{R}M2MAA.VAL",
+         "{P}{R}M3MAA.VAL",
+         "{P}{R}M4MAA.VAL",
+         "{P}{R}M5MAA.VAL",
+         "{P}{R}M6MAA.VAL",
+         "{P}{R}M7MAA.VAL",
+         "{P}{R}M8MAA.VAL"};
+
+int     motorMAE[MAX_AXES]; 
+assign  motorMAE    to
+        {"{P}{R}M1MAE.VAL",
+         "{P}{R}M2MAE.VAL",
+         "{P}{R}M3MAE.VAL",
+         "{P}{R}M4MAE.VAL",
+         "{P}{R}M5MAE.VAL",
+         "{P}{R}M6MAE.VAL",
+         "{P}{R}M7MAE.VAL",
+         "{P}{R}M8MAE.VAL"};
+
+/* We don't assign the EPICS motors here because there may be fewer than 
+ * MAX_AXES actually in use. */
+double  epicsMotorPos[MAX_AXES]; 
+assign  epicsMotorPos to {"","","","","","","",""};
+monitor epicsMotorPos;
+
+double  epicsMotorDir[MAX_AXES]; 
+assign  epicsMotorDir to {"","","","","","","",""};
+monitor epicsMotorDir;
+
+double  epicsMotorOff[MAX_AXES]; 
+assign  epicsMotorOff to {"","","","","","","",""};
+monitor epicsMotorOff;
+
+double  epicsMotorDone[MAX_AXES]; 
+assign  epicsMotorDone to {"","","","","","","",""};
+monitor epicsMotorDone;
+
+double  epicsMotorMres[MAX_AXES]; 
+assign  epicsMotorMres to {"","","","","","","",""};
+monitor epicsMotorMres;
+
+int  epicsMotorCard[MAX_AXES]; 
+assign  epicsMotorCard to {"","","","","","","",""};
+monitor epicsMotorCard;
+
+
+evflag buildMon;        sync build      buildMon;
+evflag executeMon;      sync execute    executeMon;
+evflag execStateMon;    sync execState  execStateMon;
+evflag abortMon;        sync abort      abortMon;
+evflag readbackMon;     sync readback   readbackMon;
+evflag nelementsMon;    sync nelements  nelementsMon;
+evflag motorMDVSMon;    sync motorMDVS  motorMDVSMon;
+
diff --git a/motorApp/OmsSrc/MAX_trajectoryScan.st b/motorApp/OmsSrc/MAX_trajectoryScan.st
new file mode 100644
index 00000000..6572dd40
--- /dev/null
+++ b/motorApp/OmsSrc/MAX_trajectoryScan.st
@@ -0,0 +1,983 @@
+program MAX_trajectoryScan("P=13IDC:,R=traj1,M1=M1,M2=M2,M3=M3,M4=M4,M5=M5,M6=M6,M7=M7,M8=M8,PORT=serial1")
+
+/* sample program invocation:
+ * dbLoadRecords("$(MOTOR)/motorApp/Db/trajectoryScan.db","P=xxx:,R=traj1:,NAXES=2,NELM=100,NPULSE=100")
+ * ...
+ * iocInit()
+ * ...
+ * seq &MAX_trajectoryScan, "P=xxx:,R=traj1:,M1=m1,M2=m2,M3=m3,M4=m4,M5=m5,M6=m6,M7=m7,M8=m8,PORT=none"
+ */
+
+/*  This sequencer program works with trajectoryScan.db.  It implements
+ *  coordinated trajectory motion with an OMS (Prodex) MAXV motor controller.
+ *  Eventually I hope to generalize to a MAXnet controller.  (More precisely,
+ *  I hope to generalize to an asyn-based solution which could work for either.)
+ *
+ *  Tim Mooney -- based on MM4000_trajectoryScan.st by Mark Rivers.
+ */
+
+%% #include <string.h>
+%% #include <stdio.h>
+%% #include <math.h>
+%% #include <epicsString.h>
+%% #include <asynOctetSyncIO.h>
+
+/* This program must be compiled with the recursive option */
+option +r;
+
+/* in progress: split trajectory segments if they go through velocity=0 */
+#define SPLIT_SEGMENT 0
+
+/* Until I get an asyn driver I can use, I'll test by writing/reading
+ * directly to/from drvMaxv.cc's send_mess()/recv_mess() functions.
+ */
+#define USE_ASYN 0
+
+#if USE_ASYN
+#else
+int cardNumber;
+
+/* send_mess:
+ *		If name is an axis name, command is prefixed by single-axis command, such as "AX ".
+ *		If name is null, command is sent without modification.
+ *	return value: {OK = 0, ERROR = 1}
+ */
+%%extern int MAXV_send_mess(int cardNumber, char const *message, char *name);
+/* recv_mess:
+ * amount: -1 means flush and discard; other values specify number of messages to read
+ * 
+ */
+%%extern int MAXV_recv_mess(int cardNumber, char *message, int amount);
+#endif
+
+/* Maximum # of trajectory elements.  The MAXV allows something like 2550 for
+ * a trajectory preloaded into the controller (unlimited if you're willing to
+ * write elements while the trajectory is running).  For now, we limit the number
+ * of elements to 1000.  This uses a lot of memory, the variable motorTrajectory
+ * 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.)
+ */
+#define MAX_ELEMENTS 100
+
+/* Maximum # of output pulses.  For now, we emit a pulse at the beginning of
+ * every trajectory element.
+ */
+#define MAX_PULSES 1000
+
+/* Note that MAX_ELEMENTS, and MAX_PULSES must be defined before including
+ * trajectoryScan.h, which defines MAX_AXES. */
+#include "MAX_trajectoryScan.h"
+
+/* Maximum size of string messages we'll be sending to the MAX controller */
+#define MAX_MESSAGE_STRING 100
+
+/* Buffer sizes */
+#define NAME_LEN 100
+
+/* Maximum size of string in EPICS string PVs.  This is defined in 
+ * epicsTypes.h, but in order to include that file it must be escaped, and then
+ * SNL compiler gives a warning. */
+#define MAX_STRING_SIZE 40
+
+/* Polling interval for waiting for motors to reach their targets */
+#define POLL_INTERVAL 0.1
+
+char stringOut[MAX_MESSAGE_STRING];
+char sbuf[MAX_MESSAGE_STRING];
+char stringIn[MAX_MESSAGE_STRING];
+char *asynPort;
+char *pasynUser;   /* This is really asynUser* */
+int status;
+int i;
+int j;
+int k;
+int n;
+double delay;
+int anyMoving;
+int ncomplete;
+int nextra;
+int npoints;
+int dir;
+double dtime;
+double dpos;
+double posActual;
+double posTheory;
+double expectedTime;
+double initialPos[MAX_AXES];
+char macroBuf[NAME_LEN];
+char motorName[NAME_LEN];
+char *p;
+char *tok_save;
+int currPulse;
+double frac;
+
+/* All PVs which will be accessed in local C functions need to have their index
+ * extracted with pvIndex() */
+int motorCurrentIndex[MAX_AXES];
+int epicsMotorDoneIndex[MAX_AXES];
+
+/* Note, this should be time_t, but SNL doesn't understand that.  This is
+ * the defininition in vxWorks. */
+unsigned long startTime;  
+%%epicsTimeStamp eStartTime;
+
+/* Define escaped C functions at end of file */
+%% static int writeOnly(SS_ID ssId, struct UserVar *pVar, char *command);
+%% static int writeRead(SS_ID ssId, struct UserVar *pVar, char *command);
+%% static int getMotorPositions(SS_ID ssId, struct UserVar *pVar, double *pos, int *raw, double *dtime);
+%% static int getMotorMoving(SS_ID ssId, struct UserVar *pVar);
+%% 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, double epicsMotorDir, int moveMode, int npoints, int npulses, double motorResolution,
+%%	int *position, int *velocity, int *acceleration);
+
+/* Numerical Recipes spline routines */
+%% static int spline(double *x, double *y, int n);
+%% static int splint(double *xa, double *ya, int n, double x, double *y);
+
+int position[MAX_AXES][MAX_ELEMENTS];
+int velocity[MAX_AXES][MAX_ELEMENTS];
+int acceleration[MAX_AXES][MAX_ELEMENTS];
+
+
+/*** variables for digital I/O ***/
+/* detector trigger (e.g., MCS channel advance) */
+int outBitNum;
+int onMask;
+int offMask;
+int outMask;
+/* trajectory-start signal */
+int inBitNum;
+/* variables for constructing trajectory commands */
+int segment_accel;
+int segment_decel;
+int segment_v_start;
+int segment_v_end;
+char absRel;
+int taskNum;
+int movingMask;
+/* variables for splitting a segment */
+int p1;
+int v1;
+int do_split;
+double t1;
+
+ss maxTrajectoryScan {
+
+	/* Initialize things when first starting */
+	state init {
+		when() {
+			cardNumber = -2;
+			outBitNum = -1;
+			inBitNum = -1; /* no input bit to trigger the trajectory, just start when user says */
+			initStatus = STATUS_UNDEFINED;
+			absRel='A';
+			/* Force numAxes to be <= MAX_AXES */
+			if (numAxes > MAX_AXES) numAxes = MAX_AXES;
+			for (i=0; i<numAxes; i++) {
+				sprintf(macroBuf, "M%d", i+1);
+				sprintf(motorName, "%s%s.VAL", macValueGet("P"), macValueGet(macroBuf));
+				pvAssign(epicsMotorPos[i], motorName);
+				sprintf(motorName, "%s%s.DIR", macValueGet("P"), macValueGet(macroBuf));
+				pvAssign(epicsMotorDir[i], motorName);
+				sprintf(motorName, "%s%s.OFF", macValueGet("P"), macValueGet(macroBuf));
+				pvAssign(epicsMotorOff[i], motorName);
+				sprintf(motorName, "%s%s.DMOV", macValueGet("P"), macValueGet(macroBuf));
+				pvAssign(epicsMotorDone[i], motorName);
+				sprintf(motorName, "%s%s.MRES", macValueGet("P"), macValueGet(macroBuf));
+				pvAssign(epicsMotorMres[i], motorName);
+				sprintf(motorName, "%s%s.CARD", macValueGet("P"), macValueGet(macroBuf));
+				pvAssign(epicsMotorCard[i], motorName);
+				if (cardNumber == -2) {
+					cardNumber = epicsMotorCard[i];
+				} else {
+					if (cardNumber != epicsMotorCard[i]) {
+						printf("MAX_trajectoryScan: motors not on same card: %d %d\n", cardNumber, epicsMotorCard[i]);
+						initStatus = STATUS_FAILURE;
+					}
+				}
+			}
+#if USE_ASYN
+			asynPort = macValueGet("PORT");
+			%%pVar->status = pasynOctetSyncIO->connect(pVar->asynPort, 0, (asynUser **)&pVar->pasynUser, NULL);
+			if (status != 0) {
+			   printf("trajectoryScan error in pasynOctetSyncIO->connect\n");
+			   printf("   status=%d, port=%s\n", status, asynPort);
+			}
+#endif
+			for (j=0; j<numAxes; j++) {
+				motorCurrentIndex[j]   =  pvIndex(motorCurrent[j]);
+				epicsMotorDoneIndex[j] =  pvIndex(epicsMotorDone[j]);
+			}
+
+			/* Clear all event flags */
+			efClear(buildMon);
+			efClear(executeMon);
+			efClear(abortMon);
+			efClear(readbackMon);
+			efClear(nelementsMon);
+			efClear(motorMDVSMon); /* we don't use this */
+			if (initStatus == STATUS_UNDEFINED) initStatus = STATUS_SUCCESS;
+		} state monitor_inputs
+	}
+
+	/* Monitor inputs which control what to do (Build, Execute, Read) */
+	state monitor_inputs {
+		when(efTestAndClear(buildMon) && (build==1) && (initStatus == STATUS_SUCCESS)) {
+		} state build
+
+		when(efTestAndClear(executeMon) && (execute==1) && (buildStatus == STATUS_SUCCESS)) {
+		} state execute
+
+		when(efTestAndClear(readbackMon) && (readback==1) && (execStatus == STATUS_SUCCESS)) {
+		} state readback
+
+		when(efTestAndClear(nelementsMon) && (nelements>=1)) {
+			/* If nelements changes, then change endPulses to this value,
+			 * since this is what the user normally wants.  endPulses can be
+			 * changed again after changing nelements if this is desired. */
+			endPulses = nelements;
+			pvPut(endPulses);
+		} state monitor_inputs
+
+		when(efTestAndClear(motorMDVSMon)) {
+			/* We don't use this. */
+		} state monitor_inputs
+	}
+
+	/* Build trajectory */
+	state build {
+		when() {
+			/* Set busy flag while building */
+			buildState = BUILD_STATE_BUSY;
+			pvPut(buildState);
+			buildStatus=STATUS_UNDEFINED;
+			pvPut(buildStatus);
+			/* Initialize new trajectory */
+			/* If time mode is TIME_MODE_TOTAL then construct timeTrajectory and post it */
+			if (timeMode == TIME_MODE_TOTAL) {
+				dtime = time/nelements;
+				for (i=0; i<nelements; i++) timeTrajectory[i] = dtime;
+				pvPut(timeTrajectory);
+			}
+
+			if (moveMode == MOVE_MODE_RELATIVE) {
+				npoints = nelements;
+			} else {
+				npoints = nelements-1;
+			}
+
+			/* Calculate velocities and accelerations for trajectories. */
+			for (j=0; j<MAX_AXES; j++) {
+				if (moveAxis[j]) {
+					%%buildTrajectory(ssId, pVar, pVar->timeTrajectory, pVar->motorTrajectory[pVar->j],
+					%%	pVar->epicsMotorDir[pVar->j], pVar->moveMode, pVar->npoints, pVar->npulses,
+					%%	pVar->epicsMotorMres[pVar->j],
+					%%	pVar->position[pVar->j], pVar->velocity[pVar->j], pVar->acceleration[pVar->j]);
+				}
+			}
+
+			/* Compute expected time for trajectory */
+			expectedTime=0;
+			for (i=0; i<npoints; i++) expectedTime += timeTrajectory[i];
+
+			/*** load trajectory into controller. ***/
+			sprintf(stringOut, "AM;");	/* multitasking mode */
+			%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+
+			/* digital I/O commands */
+			if (outBitNum >= 0) {
+				onMask = 1<<outBitNum;
+				offMask = 0;
+				outMask = 1<<outBitNum;
+				sprintf(stringOut, "BD%04x;", outMask);	/* set bit as output */
+				%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+				sprintf(stringOut, "BL%d;", outBitNum);	/* set output bit low */
+				%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+			}
+
+			/* trajectory commands */
+			absRel = (moveMode == MOVE_MODE_ABSOLUTE) ? 'A' : 'R';
+
+			/* clear motor queue */
+			sprintf(stringOut, "AM; SI");
+			for (j=0; j<MAX_AXES; j++) {
+				if (moveAxis[j]) strcat(stringOut, "1");
+				if (j<(MAX_AXES-1)) strcat(stringOut, ",");
+			}
+			strcat(stringOut, ";");
+			%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+
+			for (j=0, taskNum=1; j<MAX_AXES; j++) {
+				if (moveAxis[j]) {
+					
+					/* we may need current raw positions to mock up relative mode */
+					%%epicsTimeGetCurrent(&eStartTime); /* not actually the start time, we just need a value */
+					%%getMotorPositions(ssId, pVar, pVar->motorCurrent, pVar->motorCurrentRaw, &(pVar->dtime));
+
+					/* output bit */
+					if ((taskNum == 1) && (outBitNum >= 0)) {
+						/* Tell controller to output a pulse at the beginning of every trajectory segment. */
+						sprintf(stringOut, "AM; VIO[%d]%04x,%04x,%04x;", taskNum, onMask, offMask, outMask);
+						%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+					} else {
+						/* Tell controller NOT to output a pulse at the beginning of every trajectory segment. */
+						sprintf(stringOut, "AM; VIO[%d];", taskNum);
+						%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+					}
+
+					/* done flag and interrupt */
+					sprintf(stringOut, "AM; VID[%d]1;", taskNum);
+					%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+
+					/* Don't start until I tell you to start */
+					sprintf(stringOut, "AM; VH[%d]0;", taskNum);
+					%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+
+					for (i=0; i<npoints; i++) {
+						if (acceleration[j][i] > 0) {
+							segment_accel = acceleration[j][i];
+							segment_decel = acceleration[j][i];
+						} else {
+							segment_accel = -acceleration[j][i];
+							segment_decel = -acceleration[j][i];
+						}
+						if (segment_accel < 1) segment_accel = 1;
+						if (segment_accel > 8000000) segment_accel = 8000000;
+						if (segment_decel < 1) segment_decel = 1;
+						if (segment_decel > 8000000) segment_decel = 8000000;
+
+						segment_v_start = (i==0)? velocity[j][0]:velocity[j][i-1];
+						segment_v_end = velocity[j][i];
+#if SPLIT_SEGMENT
+						/* If velocity goes through zero during this segment, we'll need to split the segment. */
+						do_split = (segment_v_start>0) != (segment_v_end>0);
+						do_split = do_split && (abs(segment_v_start)>2) && (abs(segment_v_end)>2);
+						if (do_split) {
+							/* time at which velocity reaches zero */
+							t1 = -segment_v_start;
+							t1 = t1/acceleration[j][i];
+							printf("t1=%f\n", t1);
+							v1 = 1;
+							p1 = position[j][i] + segment_v_start*t1 + 0.5 * acceleration[j][i]*t1*t1;
+						}
+#else
+						do_split = 0;
+#endif
+						segment_v_start = abs(segment_v_start); 
+						segment_v_end = abs(segment_v_end); 
+
+						if (segment_v_start < 1) segment_v_start = 1;
+						if (segment_v_start > 4194303) segment_v_start = 4194303;
+						if (segment_v_end < 0) segment_v_end = 0;
+						if (segment_v_end > 4194303) segment_v_end = 4194303;
+
+						/* BUG in MAXV: doesn't do relative trajectories right. Try to work around. */
+						if (moveMode != MOVE_MODE_ABSOLUTE) position[j][i] += motorCurrentRaw[j];
+
+						if (do_split) {
+							/* we have to split this segment into two where velocity goes through zero. */
+							n = sprintf(stringOut, "AM; VA[%d]%d,%d;", taskNum, segment_accel, segment_decel);
+							n += sprintf(&stringOut[n], "VV[%d]%d,%d;", taskNum, segment_v_start, v1);
+							n += sprintf(&stringOut[n], "VP[%d]", taskNum);
+							for (k=0; k<j; k++) {strcat(stringOut, ","); n++;}
+							n += sprintf(&(stringOut[n]), "%d", p1);
+							for (k=j+1; k<MAX_AXES; k++) {strcat(stringOut, ","); n++;}
+							strcat(stringOut, ";");
+							%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+
+							n = sprintf(stringOut, "AM; VA[%d]%d,%d;", taskNum, segment_accel, segment_decel);
+							n += sprintf(&stringOut[n], "VV[%d]%d,%d;", taskNum, v1, segment_v_end);
+							n += sprintf(&stringOut[n], "VP[%d]", taskNum);
+							for (k=0; k<j; k++) {strcat(stringOut, ","); n++;}
+							n += sprintf(&(stringOut[n]), "%d", position[j][i]);
+							for (k=j+1; k<MAX_AXES; k++) {strcat(stringOut, ","); n++;}
+							strcat(stringOut, ";");
+							%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+						} else {
+							n = sprintf(stringOut, "AM; VA[%d]%d,%d;", taskNum, segment_accel, segment_decel);
+							n += sprintf(&stringOut[n], "VV[%d]%d,%d;", taskNum, segment_v_start, segment_v_end);
+							n += sprintf(&stringOut[n], "VP[%d]", taskNum);
+							for (k=0; k<j; k++) {strcat(stringOut, ","); n++;}
+							n += sprintf(&(stringOut[n]), "%d", position[j][i]);
+							for (k=j+1; k<MAX_AXES; k++) {strcat(stringOut, ","); n++;}
+							strcat(stringOut, ";");
+							%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+						}
+					}
+					sprintf(stringOut, "AM; VE[%d];", taskNum);
+					%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+					taskNum++;
+				}
+			}
+
+
+			/* Set status and message string */
+			buildStatus = STATUS_SUCCESS;
+
+			/* Clear busy flag, post status */
+			buildState = BUILD_STATE_DONE;
+			pvPut(buildState);
+			pvPut(buildStatus);
+			pvPut(buildMessage);
+			/* Clear build command, post.  This is a "busy" record, don't want
+			 * to do this until build is complete. */
+			build=0;
+			pvPut(build);
+		} state monitor_inputs
+	}
+
+
+	state execute {
+		when () {
+			/* Set busy flag */
+			execState = EXECUTE_STATE_MOVE_START;
+			pvPut(execState);
+			/* Set status to INVALID */
+			execStatus = STATUS_UNDEFINED;
+			pvPut(execStatus);
+			/* Erase the readback and error arrays */
+			for (j=0; j<numAxes; j++) {
+				for (i=0; i<MAX_PULSES; i++) {
+					motorReadbacks[j][i] = 0.;
+					motorError[j][i] = 0.;
+				}
+			}
+			currPulse = 0;
+			/* Get the initial positions of the motors */
+			for (j=0; j<numAxes; j++) initialPos[j] = epicsMotorPos[j];
+			/* Move to start position if required */
+			if (moveMode == MOVE_MODE_ABSOLUTE) {
+				for (j=0; j<numAxes; j++) {
+					if (moveAxis[j]) {
+						epicsMotorPos[j] = motorTrajectory[j][0];
+						pvPut(epicsMotorPos[j]);
+					}
+				}
+				%%waitEpicsMotors(ssId, pVar);
+			}
+			/* Arm the trajectories to start on an input trigger bit.  If no input trigger bit, then start now. */
+			if (inBitNum >= 0) {
+				/* Wait for input bit to go high before processing any more commands. */
+				sprintf(stringOut, "SW%d;", inBitNum);
+				%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+			}
+
+			n = sprintf(stringOut, "AM;"); /* Axis multitasking mode */
+			for (j=0, taskNum=1; j<MAX_AXES; j++) {
+				if (moveAxis[j]) {
+					n += sprintf(&(stringOut[n]), "VG[%d];", taskNum++); /* GO! */
+				}
+			}
+			%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+
+			/* Get start time of execute */
+			startTime = time(0);
+			%%epicsTimeGetCurrent(&eStartTime);
+			execState = EXECUTE_STATE_EXECUTING;
+			pvPut(execState);
+			/* This was "an attempt to fix the problem of MM4000's 'TP' command sometimes not
+			 * responding".
+			 */
+			/*epicsThreadSleep(0.1);*/
+		} state wait_execute
+	}
+
+	/* Wait for trajectory to complete */
+	state wait_execute {
+		when (execStatus == STATUS_ABORT) {
+			/* The trajectory_abort state set has detected an abort. It has
+			 * already posted the status and message.  Don't execute flyback
+			 * return to top */
+			execState = EXECUTE_STATE_DONE;
+			pvPut(execState);
+			/* Clear execute command, post.  This is a "busy" record, don't
+			 * want to do this until execution is complete. */
+			execute=0;
+			pvPut(execute);
+		} state monitor_inputs
+
+		when(execState==EXECUTE_STATE_EXECUTING) {
+			/* Get the current motor positions, post them */
+			%%getMotorPositions(ssId, pVar, pVar->motorCurrent, pVar->motorCurrentRaw, &(pVar->dtime));
+			for (j=0, movingMask = 0; j<numAxes; j++) {
+				pvPut(motorCurrent[j]);
+				if (moveAxis[j]) movingMask |= (1<<j);
+				/* MAXV has no readback function, so we read while it's moving. */
+				if (currPulse < MAX_PULSES-1) {
+					motorReadbacks[j][currPulse] = motorCurrent[j];
+					motorError[j][currPulse] = dtime;
+				}
+			}
+			++currPulse;
+			%%pVar->anyMoving = getMotorMoving(ssId, pVar);
+			if (debugLevel >= 10) printf("movingMask=%x, anyMoving=%x\n", movingMask, anyMoving);
+			if (!(anyMoving&movingMask)) {
+				execState = EXECUTE_STATE_FLYBACK;
+				execStatus = STATUS_SUCCESS;
+				strcpy(execMessage, " ");
+			}
+			/* See if the elapsed time is more than twice expected, time out */
+			if (difftime(time(0), startTime) > expectedTime*timeScale*2.) {
+				execState = EXECUTE_STATE_FLYBACK;
+				execStatus = STATUS_TIMEOUT;
+				strcpy(execMessage, "Timeout");
+			}
+			/* Check for errors while trajectories are in progress */
+		} state wait_execute
+
+		when(execState==EXECUTE_STATE_FLYBACK) {
+			pvPut(execState);
+			pvPut(execStatus);
+			pvPut(execMessage);
+			/* Get the current motor positions, post them */
+			%%getMotorPositions(ssId, pVar, pVar->motorCurrent, pVar->motorCurrentRaw, &(pVar->dtime));
+			for (j=0; j<numAxes; j++) pvPut(motorCurrent[j]);
+			for (j=0; j<numAxes; j++) {
+				if (moveAxis[j]) {
+					epicsMotorPos[j] = motorCurrent[j];
+					pvPut(epicsMotorPos[j]);
+				}
+			}
+			%%waitEpicsMotors(ssId, pVar);
+			execState = EXECUTE_STATE_DONE;
+			pvPut(execState);
+			/* Clear execute command, post.  This is a "busy" record, don't
+			 * want to do this until execution is complete. */
+			execute=0;
+			pvPut(execute);
+		} state monitor_inputs
+	}
+
+	/* Read back actual positions */
+	state readback {
+		when() {
+			/* Set busy flag */
+			readState = READ_STATE_BUSY;
+			pvPut(readState);
+			readStatus=STATUS_UNDEFINED;
+			pvPut(readStatus);
+#if 1
+			/* During trajectory execution, time and motor position were accumulated into
+			 * motorError[j] and motorReadbacks[j], respectively.  Interpolate motorReadbacks[j]
+			 * to get readbacks at the times implied by timeTrajectory (but note that these are dwell
+			 * times, not real time), so they can be plotted on the same axis with motorTrajectory[j].
+			 */
+            for (j=0, i=0; j<numAxes; j++) {
+				dtime = 0.;
+				for (k=0; k<npoints; k++) {
+					while ((motorError[j][i] < dtime) && (i < MAX_PULSES-1)) i++;
+					if ((i>0) && (fabs(motorError[j][i] - motorError[j][i-1]) > 1e-6)) {
+						frac = (dtime - motorError[j][i-1])/(motorError[j][i] - motorError[j][i-1]);
+						motorReadbacks[j][k] = motorReadbacks[j][i-1] + frac * (motorReadbacks[j][i] - motorReadbacks[j][i-1]);
+					} else {
+						motorReadbacks[j][k] = motorReadbacks[j][i];
+					}
+					dtime += timeTrajectory[k];
+				}
+				for (; k<MAX_PULSES; k++) motorReadbacks[j][k] = 0.;
+			}
+#endif
+            /* Post the readback and error arrays */
+            for (j=0; j<numAxes; j++) {
+                pvPut(motorReadbacks[j]);
+                pvPut(motorError[j]);
+            }
+			/* Clear busy flag */
+			readState = READ_STATE_DONE;
+			pvPut(readState);
+			/* For now we are not handling read errors */
+			readStatus = STATUS_SUCCESS;  
+			pvPut(readStatus);
+			strcpy(readMessage, " ");
+			pvPut(readMessage);
+			/* Clear readback command, post.  This is a "busy" record, don't
+			 * want to do this until readback is complete. */
+			readback=0;
+			pvPut(readback);
+		} state monitor_inputs
+	}
+}
+
+/* This state set simply monitors the abort input.  It is a separate state set
+ * so that it is always active, no matter what the state of the trajectoryScan
+ * state set. If an abort is received it sends the "SA" command to the MAX controller, 
+ * sets the execStatus to STATUS_ABORT and writes a message to execMessage */
+ss trajectoryAbort {
+	state monitorAbort {
+		when (efTestAndClear(abortMon) && (abort==1)) {
+			sprintf(stringOut, "SA;"); /* Stop all motors, and flush all queues. */
+			%%if (pVar->simMode==0) writeOnly(ssId, pVar, pVar->stringOut);
+			execStatus = STATUS_ABORT;
+			pvPut(execStatus);
+			strcpy(execMessage, "Motion aborted");
+			pvPut(execMessage);
+			/* Clear abort command, post.  This is a "busy" record, don't
+			 * want to do this until abort command has been sent. */
+			abort=0;
+			pvPut(abort);
+		} state monitorAbort
+	}
+}
+
+/* C functions */
+%{
+
+/* writeOnly sends a command to the MAX controller */
+static int writeOnly(SS_ID ssId, struct UserVar *pVar, char *command)
+{
+	asynStatus status;
+#if USE_ASYN
+	size_t nwrite;
+	char buffer[MAX_MESSAGE_STRING];
+
+	/* Copy command so we can add terminator */
+	strncpy(buffer, command, MAX_MESSAGE_STRING-3);
+	strcat(buffer, "\r");
+	status = pasynOctetSyncIO->write((asynUser *)pVar->pasynUser, buffer,
+							   strlen(buffer), 1.0, &nwrite);
+#else
+	status = (asynStatus) MAXV_send_mess(pVar->cardNumber, command, (char *) NULL);
+#endif
+	if (pVar->debugLevel >= 5) printf("    writeOnly:command='%s'\n", command);
+	return(status);
+}
+
+
+/* writeRead sends a command to the MAX controller and reads the response into
+ * the global character buffer, stringIn.
+ */
+static int writeRead(SS_ID ssId, struct UserVar *pVar, char *command)
+{
+	asynStatus status;
+#if USE_ASYN
+	size_t nwrite, nread;
+	int eomReason;
+	char buffer[MAX_MESSAGE_STRING];
+
+	/* Copy command so we can add terminator */
+	strncpy(buffer, command, MAX_MESSAGE_STRING-3);
+	strcat(buffer, "\r");
+	/* Use 30 second timeout, some commands take a long time to reply */
+	status = pasynOctetSyncIO->writeRead((asynUser *)pVar->pasynUser, buffer,
+							   strlen(buffer), pVar->stringIn, MAX_MESSAGE_STRING, 
+							   30.0, &nwrite, &nread, &eomReason);
+#else
+	status = (asynStatus) MAXV_send_mess(pVar->cardNumber, command, (char *) NULL);
+	status |= (asynStatus) MAXV_recv_mess(pVar->cardNumber, pVar->stringIn, 1);
+#endif
+	if (pVar->debugLevel >= 10) {
+		printf("    writeRead:command='%s', reply='%s'\n", command, pVar->stringIn);
+	}
+	return(status);
+}
+
+/* getMotorPositions returns the positions of each motor */
+static int getMotorPositions(SS_ID ssId, struct UserVar *pVar, double *pos, int *raw, double *dtime)
+{
+    char *p, *tok_save;
+    int j;
+    int dir;
+	epicsTimeStamp currtime;
+
+	double dt, x=0, v, a;
+
+	epicsTimeGetCurrent(&currtime);
+	dt = epicsTimeDiffInSeconds(&currtime, &eStartTime);
+	*dtime = dt;
+
+    /* Read the current positions of all the axes */
+    writeRead(ssId, pVar, "PP");
+    /* Parse the return string which is of the form 
+     * 100,0,83 ... */
+    tok_save = 0;
+    p = epicsStrtok_r(pVar->stringIn, ",", &tok_save);
+    for (j=0; (j<pVar->numAxes && p!=0); j++) {
+		raw[j] = atof(p);
+        if (pVar->epicsMotorDir[j] == 0) dir=1; else dir=-1;
+		/* printf("getMotorPositions: motor %d; step='%s'\n", j, p); */
+        pos[j] = raw[j]*dir*pVar->epicsMotorMres[j] + pVar->epicsMotorOff[j];
+		if (j==0) x = atof(p);
+        p = epicsStrtok_r(0, ",", &tok_save);
+    }
+	if (pVar->debugLevel >= 2) {
+		writeRead(ssId, pVar, "VRV[1];"); v = atof(&(pVar->stringIn[1]));
+		writeRead(ssId, pVar, "VRC[1];"); a = atof(&(pVar->stringIn[1]));
+		printf("getMotorPositions: dt=%6.3f, p=%7.0f, v=%7.0f, a=%7.0f\n", dt, x, v, a);
+		if (pVar->debugLevel >= 10) printf("\n");
+	} else if (pVar->debugLevel >= 1) {
+		printf("getMotorPositions: dt=%6.3f, p=%7.1f\n", dt, x);
+	}
+	return(0);
+}
+
+
+/* getMotorMoving returns the moving status of each motor, packed into a single
+ * int.  Bit 0 = motor 1, bit 1 = motor 2, etc. 0=not moving, 1=moving.
+ * If the entire int is 0 then no motors are moving */
+static int getMotorMoving(SS_ID ssId, struct UserVar *pVar)
+{
+	int i, mask=1, result=0;
+	char s[MAX_MESSAGE_STRING];
+
+	for (i=0; i==0;) {
+		/* Read the current status of all the axes */
+		writeRead(ssId, pVar, "QI");
+		strcpy(s, pVar->stringIn);
+		writeRead(ssId, pVar, "QI");
+		if (strcmp(s, pVar->stringIn) != 0) {
+			if (pVar->debugLevel >= 2) {
+				printf("getMotorMoving: inconsistent replies:\n");
+				printf("r1:'%s', r2:'%s'\n", s, pVar->stringIn);
+			}
+		} else {
+			i = 1;
+		}
+	}
+	/* Parse the return string which is of the form 
+	 * MDNN,MDNN,PNLN,PNNN,PNLN,PNNN,PNNN,PNNN,<LF>
+	 * The second character of each status word is 'D' (done) or 'N' (not done)
+	 */
+
+	for (i=1; i<37; i+=5, mask<<=1) {
+		if (pVar->stringIn[i] == 'N') result |= mask;
+	}
+
+	pVar->stringIn[40] = '\0';
+	if (pVar->debugLevel >= 10) {
+		printf("getMotorMoving: reply = '%s', movingMask = %2x\n", pVar->stringIn, result);
+	}
+	return(result);
+}
+
+/* getEpicsMotorMoving returns the EPICS moving status of each motor, packed into 
+ * a single int.  Bit 0 = motor 1, bit 1 = motor 2, etc. 0=not moving, 1=moving.
+ * If the entire int is 0 then no motors are moving */
+static int getEpicsMotorMoving(SS_ID ssId, struct UserVar *pVar)
+{
+	int j;
+	int result=0, mask=0x01;
+
+	for (j=0; j<pVar->numAxes; j++) {
+		seq_pvGet(ssId, pVar->epicsMotorDoneIndex[j], 0);
+		if (pVar->epicsMotorDone[j] == 0) result |= mask;
+		mask = mask << 1;
+	}
+	return(result);
+}
+
+/* waitEpicsMotors waits for all motors to stop moving using the EPICS motor
+ * records..  It reads and posts the motor positions during each loop. */
+static int waitEpicsMotors(SS_ID ssId, struct UserVar *pVar)
+{
+	int j;
+
+	/* Logic is that we always want to post position motor positions 
+	 * after the end of move is detected. */
+	while(getEpicsMotorMoving(ssId, pVar)) {
+		/* Get the current motor positions, post them */
+		for (j=0; j<pVar->numAxes; j++) {
+			pVar->motorCurrent[j] = pVar->epicsMotorPos[j];
+			seq_pvPut(ssId, pVar->motorCurrentIndex[j], 0);
+		}
+		epicsThreadSleep(POLL_INTERVAL);
+	}
+	for (j=0; j<pVar->numAxes; j++) {
+		pVar->motorCurrent[j] = pVar->epicsMotorPos[j];
+		seq_pvPut(ssId, pVar->motorCurrentIndex[j], 0);
+	}
+	return(0);
+}
+
+#define MAX(a,b) ((a) > (b) ? (a) : (b))
+#define MIN(a,b) ((a) > (b) ? (b) : (a))
+#define NINT(f)  (int)((f)>0 ? (f)+0.5 : (f)-0.5)
+
+/* Calculate velocities and accelerations suitable for MAX variable velocity contouring commands.
+ * We're given x(t) in the form x[i], t[i].  We need to calculate v(x) and a(x) that will produce x(t).
+ */
+
+double	v_out[MAX_ELEMENTS], a_out[MAX_ELEMENTS], calcMotorTrajectory[MAX_ELEMENTS], realTime[MAX_ELEMENTS];
+static int buildTrajectory(SS_ID ssId, struct UserVar *pVar, double *timeTrajectory,
+	double *motorTrajectory, double epicsMotorDir, int moveMode, int npoints, int npulses, double motorResolution,
+	int *position, int *velocity, int *acceleration)
+{
+	double dp, dt, v_ideal, v_lin, v_quad, v_spline, accel_p, accel_v, time;
+	double x0, x1, x2, v0, dt2;
+	double delta, yy0, yy1;
+	int i;
+
+	for (i=0, time=0.; i<npoints; i++) {
+		realTime[i] = time;
+		time += timeTrajectory[i];
+	}
+	spline(realTime, motorTrajectory, npoints);
+
+	calcMotorTrajectory[0] = motorTrajectory[0];
+	v_out[0] = 0;
+	if (pVar->debugLevel >= 5) {
+		printf("###:%8s %8s %7s %4s %8s %8s %8s\n",
+			"pos", "calcPos", "dp", "dt", "v_ideal", "accel_p", "accel_v");
+	}
+	for (i=1; i<npoints; i++) {
+		/*dp = motorTrajectory[i]-motorTrajectory[i-1];*/
+		/* Don't assume we achieved exactly the desired [i-1] position. */
+		dp = motorTrajectory[i]-calcMotorTrajectory[i-1];
+		/* timeTrajectory[i] is the time to move from motorTrajectory[i] to motorTrajectory[i+1] */
+		dt = timeTrajectory[i-1];
+		dt2 = timeTrajectory[i];
+		/* the acceleration that will get us to the desired position */
+		accel_p = 2*(dp - v_out[i-1]*dt)/(dt*dt);
+		if (i < npoints-1) {
+			x0 = motorTrajectory[i-1];
+			x1 = motorTrajectory[i];
+			x2 = motorTrajectory[i+1];
+
+			/* the ideal velocity at motorTrajectory[i] */
+			/* linear interpolation */
+			v_lin = (motorTrajectory[i+1]-motorTrajectory[i-1])/(timeTrajectory[i]+timeTrajectory[i-1]);
+			/* Don't assume we achieved exactly the desired [i-1] position. */
+			/*v_lin = (motorTrajectory[i+1]-calcMotorTrajectory[i-1])/(timeTrajectory[i]+timeTrajectory[i-1]);*/
+			/* next guess:
+			 * x1 = x0 + v0*dt + a0*dt*dt/2
+			 * v1 = v0 + a0*dt
+			 * x2 = x1 + v1*dt2 + a1*dt2*dt2/2
+			 *    = x1 + [v0 + a0*dt]*dt2 + a1*dt2*dt2/2
+			 * assume a1==a0
+			 * a0 = (x2 - x1 - v0*dt) / (dt*dt2 + (dt2*dt2)/2)
+			 * v1 = v0 + a0*dt
+			 */
+			v0 = v_out[i-1];
+
+			/* quadratic calculation */
+			v_quad = v0 + dt*((x2 - x1 - v0*dt) / (dt*dt2 + (dt2*dt2)/2));
+
+			/* spline calculation */
+			delta = (realTime[i+1] - realTime[i-1])/10.;
+			splint(realTime, motorTrajectory, npoints, realTime[i]-delta, &yy0);
+			splint(realTime, motorTrajectory, npoints, realTime[i]+delta, &yy1);
+			v_spline = (yy1-yy0)/(2*delta);
+
+			if (pVar->debugLevel >= 10) {
+				printf("v_lin=%f, v_quad=%f, v_spline=%f\n", v_lin, v_quad, v_spline);
+			}
+
+			/* the acceleration that will get us to the ideal velocity */
+			if (pVar->debugLevel%2) {
+				v_ideal = v_lin;
+			} else {
+				/*v_ideal = v_quad;*/
+				v_ideal = v_spline;
+			}
+			accel_v = (v_ideal - v_out[i-1])/dt;
+			/* compromise between desired position and ideal velocity */
+			a_out[i-1] = (accel_p + accel_v)/2;
+		} else {
+			v_ideal = 0.;
+			accel_v = (v_ideal - v_out[i-1])/dt;
+			a_out[i-1] = accel_p;
+		}
+		if (pVar->debugLevel >= 5) {
+			printf("%3d:%8.2f %8.2f %7.2f %4.2f %8.3f %8.3f %8.3f\n",
+				i, motorTrajectory[i], calcMotorTrajectory[i-1], dp, dt, v_ideal, accel_p, accel_v);
+		}
+		v_out[i] = v_out[i-1] + a_out[i-1]*dt;
+		calcMotorTrajectory[i] = calcMotorTrajectory[i-1] + v_out[i-1]*dt + .5 * a_out[i-1]*dt*dt;
+	}
+	a_out[npoints-1] = a_out[npoints-2];
+	
+	if (pVar->debugLevel >= 2) {
+		printf("buildTrajectory:\n");
+		printf("%10s %10s %10s %10s %10s\n", "timeTraj", "motorTraj", "calcTraj", "v_out", "a_out");
+		for (i=0, time=0; i<npoints; i++) {
+			printf("%10.2f %10.5f %10.5f %10.5f %10.5f\n",
+				time, motorTrajectory[i], calcMotorTrajectory[i], v_out[i], a_out[i]);
+			time += timeTrajectory[i];
+		}
+	}
+
+	/* Translate into MAX commands */
+	v_out[0] = v_out[1];
+	v_out[0] = 0;
+	if (pVar->debugLevel >= 1) {
+		printf("motor resolution %f\n", motorResolution);
+		printf("%10s %10s %10s %10s %10s\n", "time", "position", "calcpos", "velocity", "acceleration");
+	}
+	for (i=0, time=0.,x0=0.; i<npoints; i++) {
+		time += timeTrajectory[i];
+		if (i < npoints-1) {
+			position[i] = NINT(calcMotorTrajectory[i+1]/motorResolution);
+			velocity[i] = NINT(v_out[i+1]/motorResolution);
+			acceleration[i] = NINT(a_out[i]/motorResolution);
+		} else {
+			position[i] = NINT(calcMotorTrajectory[i]/motorResolution);
+			velocity[i] = 0;
+			acceleration[i] = NINT(a_out[i]/motorResolution);
+		}
+		if (i>0) {
+			x0 = position[i-1] + velocity[i-1]*timeTrajectory[i] + .5 * acceleration[i]*timeTrajectory[i]*timeTrajectory[i];
+		} else {
+			x0 = .5 * acceleration[i]*timeTrajectory[i]*timeTrajectory[i];
+		}
+		if (pVar->debugLevel >= 1) printf("%10.2f %10d %10d %10d %10d\n", time, position[i], NINT(x0), velocity[i], acceleration[i]);
+	}
+
+	return(0);
+}
+
+/* Numerical recipes spline routines */
+double y2[MAX_ELEMENTS+1];
+double u[MAX_ELEMENTS+1];
+
+static int spline(double *x, double *y, int n)
+{
+	int i, k;
+	double p, qn, sig, un;
+
+	/* convert from c array to fortran array */
+	x--; y--;
+
+	y2[1] = u[1] = 0.0;
+	for (i=2; i<=n-1; i++) {
+		sig = (x[i]-x[i-1])/(x[i+1]-x[i-1]);
+		p = sig*y2[i-1]+2.0;
+		y2[i] = (sig-1.0)/p;
+		u[i] = (y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
+		u[i] = (6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
+	}
+	qn = un = 0.0;
+	y2[n] = (un-qn*u[n-1])/(qn*y2[n-1]+1.0);
+	for (k=n-1; k>=1; k--)
+		y2[k] = y2[k]*y2[k+1]+u[k];
+	return(0);
+}
+
+static int splint(double *xa, double *ya, int n, double x, double *y)
+{
+	int klo,khi,k;
+	double h,b,a;
+
+	/* convert from c array to fortran array */
+	xa--; ya--;
+
+	klo = 1;
+	khi = n;
+	while (khi-klo > 1) {
+		k = (khi+klo) >> 1;
+		if (xa[k] > x) khi = k;
+		else klo = k;
+	}
+	h = xa[khi]-xa[klo];
+	if (h == 0.0) {
+		printf("Bad XA input to routine SPLINT");
+		return(-1);
+	}
+	a = (xa[khi]-x)/h;
+	b = (x-xa[klo])/h;
+	*y = a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2[klo]+(b*b*b-b)*y2[khi])*(h*h)/6.0;
+	return(0);
+}
+
+}%