Here is PROG 1 (called from PROG 31 above):
; Variables to pass arguments to subroutine
; Note that the variable numbers here are fixed
#define AArg Q101 ; Variable to pass A argument
#define DArg Q104 ; Variable to pass D argument
#define PArg Q116 ; Variable to pass P argument
#define SArg Q119 ; Variable to pass S argument
#define VArg Q122 ; Variable to pass V argument
#define XArg Q124 ; Variable to pass X argument
; Variables for general-purpose use
#define ProgDist Q150 ; Signed distance for move
#define Dir Q151 ; Direction of move
#define AccDist Q152 ; Mag of distance for accel
#define DecDist Q153 ; Mag of distance for decel
#define AccTime Q154 ; Total time for accel
#define DecTime Q155 ; Total time for S decel
#define AccSTime Q156 ; Time for half of S accel
#define DecSTime Q157 ; Time for half of S decel
#define Apeak Q158 ; Mag of top accel reached
#define Dpeak Q159 ; Mag of top decel reached
#define ApeakTime Q160 ; Time at top accel reached
#define DpeakTime Q161 ; Time at top decel reached
#define ReachSpeed Q162 ; True (1) if move hits prog speed
#define Seg1Dist Q163 ; Signed distance of 1st seg
#define Seg1EndVel Q164 ; Signed end vel of 1st seg
#define Seg2Dist Q165 ; Signed distance of 2nd seg
#define Seg2EndVel Q166 ; Signed end vel of 2nd seg
#define Seg3Dist Q167 ; Signed distance of 3rd seg
#define Seg3EndVel Q168 ; Signed end vel of 3rd seg
#define Seg4Dist Q169 ; Signed distance of 4th seg
#define Seg4EndVel Q170 ; Signed end vel of 4th seg
#define Seg5Dist Q171 ; Signed distance of 5th seg
#define Seg5EndVel Q172 ; Signed end vel of 5th seg
#define Seg6Dist Q173 ; Signed distance of 6th seg
#define Seg6EndVel Q174 ; Signed end vel of 6th seg
#define Seg7Dist Q175 ; Signed distance of 7th seg
#define TopVTime Q176 ; Time at top speed
#define Vpeak Q177 ; Peak speed if less than prog
#define OldAbsIncMode Q178 ; Saved to restore at end
#define FourSecDist Q179 ; Distance in 4 sec at top speed
; Subroutine used by multiple other programs to execute move using PVT segments
OPEN PROG 1 CLEAR
READ(X,S,V,A,D,P)
ProgDist=XArg-SArg ; End minus Start, signed qty
Dir=ABS(ProgDist)/ProgDist ; +1 or -1 (0 if no distance)
; Make sure S-curve percentage is in proper range
IF(PArg<0.04) PArg=0.04 ; Minimum 4% S-curve
IF(PArg>0.96) PArg=0.96 ; Maximum 96% S-curve
; LIMITS ACCEL RATE SO THAT EACH ACCEL SLICE DOES NOT EXCEED 4000 ms
IF (((VArg/AArg)*1000*PArg/2)>4000) ; Calculates S-Curve AccTime
AArg=VArg*1000*PArg/(4000*2)
ENDIF
IF (((VArg/AArg)*1000*(1-PArg))>4000) ; Calculates Linear AccTime
AArg=VArg*1000*(1-PArg)/4000
ENDIF
; LIMITS DECEL RATE SO THAT EACH DECEL SLICE DOES NOT EXCEED 4000 ms
IF (((VArg/DArg)*1000*PArg/2)>4000) ; Calculates S-Curve DecTime
DArg=VArg*1000*PArg/(4000*2)
ENDIF
IF (((VArg/DArg)*1000*(1-PArg))>4000) ; Calculates Linear DecTime
DArg=VArg*1000*(1-PArg)/4000
ENDIF
; Compute distances needed to get to and from full speed
AccDist=VArg*VArg/AArg/2 ; Dist mag to reach full speed
DecDist=VArg*VArg/DArg/2 ; Dist mag to decel to zero
IF(AccDist+DecDist Vpeak=VArg ; Top speed is programmed
ReachSpeed=1 ; Flag for future logic
ELSE ; No full-speed segment
Vpeak=SQRT(ABS(ProgDist)/((1/AArg+1/DArg)/2)) ; Lower peak speed mag
AccDist=Vpeak*Vpeak/AArg/2 ; Dist mag to reach peak speed
DecDist=Vpeak*Vpeak/DArg/2 ; Dist mag to decel to zero
ReachSpeed=0 ; Flag for future logic
ENDIF
AccTime=Vpeak/AArg ; Total accel time in sec
AccSTime=AccTime*PArg/2 ; In each half of S, in sec
ApeakTime=AccTime*(1-PArg) ; Time at peak accel, in sec
Apeak=Vpeak/(AccTime-AccSTime) ; Magnitude of peak accel
Seg1EndVel=Dir*Apeak*AccSTime/2 ; Segment 1 signed end velocity
Seg1Dist=Dir*Apeak*AccSTime*AccSTime/6 ; Segment 1 signed distance
Seg2EndVel=Seg1EndVel+Dir*Apeak*ApeakTime ; Seg 2 signed end velocity
Seg2Dist=(Seg1EndVel+Seg2EndVel)*ApeakTime/2 ; Seg 2 signed distance
Seg3EndVel=Dir*Vpeak ; Segment 3 signed end velocity
Seg3Dist=Dir*AccDist-Seg1Dist-Seg2Dist ; Segment 3 signed distance
IF(ReachSpeed=1) ; Segment at programmed speed?
Seg4Dist=ProgDist-Dir*(AccDist+DecDist) ; Signed distance at speed
Seg4EndVel=Dir*Vpeak ; Signed end velocity
TopVTime=Seg4Dist/Seg4EndVel ; Total time at top speed, in sec
ENDIF
DecTime=Vpeak/DArg ; Total decel time in sec
DecSTime=DecTime*PArg/2 ; In each half of S, in sec
DpeakTime=DecTime*(1-PArg) ; Time at peak decel, in sec
Dpeak=Vpeak/(DecTime-DecSTime) ; Magnitude of peak decel
Seg7Dist=Dir*Dpeak*DecSTime*DecSTime/6 ; Segment 7 signed distance
Seg6EndVel=Dir*Dpeak*DecSTime/2 ; Segment 6 signed end vel
Seg5EndVel=Seg6EndVel+Dir*Dpeak*DpeakTime ; Segment 5 signed end vel
Seg6Dist=(Seg5EndVel+Seg6EndVel)*DpeakTime/2 ; Seg 6 signed distance
Seg5Dist=Dir*DecDist-Seg6Dist-Seg7Dist ; Segment 5 signed distance
BLOCKSTART ; Beginning of step sequence
INC(X) ; Specify X moves by distance
PVT(AccSTime*1000) ; Set move mode and time in msec
X(Seg1Dist):(Seg1EndVel) ; 1st half of S-curve accel
PVT(ApeakTime*1000) ; Time at peak accel in msec
X(Seg2Dist):(Seg2EndVel) ; Peak accel segment
PVT(AccSTime*1000) ; Time in 2nd half of S-curve
X(Seg3Dist):(Seg3EndVel) ; Segment to reach top speed
IF(ReachSpeed=1) ; Segment at programmed speed?
WHILE(TopVTime>4) ; Too long for 1 PVT move
FourSecDist=Seg4EndVel*4 ; Distance in 4 sec at top speed
PVT4000 ; Set 4-sec move
X(FourSecDist):(Seg4EndVel) ; Constant speed move for 4 sec
TopVTime=TopVTime-4 ; Time remaining at top speed
Seg4Dist=Seg4Dist-FourSecDist ; Distance remaining at top speed
ENDWHILE
IF(TopVTime>.0011) ; If Seg4 Move time is > 1.1 msec
PVT(TopVTime*1000) ; (Remaining) time at top speed
X(Seg4Dist):(Seg4EndVel) ; (Remaining) top-speed segment
ELSE
Seg5Dist=Seg5Dist+Seg4Dist ; Add skipped Seg4Dist to Seg5Dist
ENDIF
ENDIF
PVT(DecSTime*1000) ; Time in 1st half of decel S-curve
X(Seg5Dist):(Seg5EndVel) ; 1st half of S-curve decel
PVT(DpeakTime*1000) ; Time at peak decel
X(Seg6Dist):(Seg6EndVel) ; Peak decel segment
PVT(DecSTime*1000) ; Time in 2nd half of decel S-curve
X(Seg7Dist):0
BLOCKSTOP
RETURN
CLOSE