ported(fc925fe0): different min/max acceleration in second order

stefanbesler · May 12, 2024 · 8963f36 · 8963f36
1 parent 85cfaf2
commit 8963f36
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 46 deletions.
diff --git a/Struckig/Struckig/Struckig/Struckig/Position/PositionSecondOrderStep1.TcPOU b/Struckig/Struckig/Struckig/Struckig/Position/PositionSecondOrderStep1.TcPOU
@@ -297,14 +297,14 @@ VAR_INPUT
 END_VAR]]></Declaration>
       <Implementation>
         <ST><![CDATA[profile.t[0] := (-v0 + vMax)/aMax;
-profile.t[1] := (v0*v0 + vf*vf)/(2*vMax*aMax) - vMax/aMax + pd/vMax;
-profile.t[2] := (-vf + vMax)/aMax;
+profile.t[1] :=(aMin*v0*v0 - aMax*vf*vf)/(2*aMax*aMin*vMax) + vMax*(aMax - aMin)/(2*aMax*aMin) + pd/vMax;
+profile.t[2] := (vf - vMax)/aMin;
 profile.t[3] := 0;
 profile.t[4] := 0;
 profile.t[5] := 0;
 profile.t[6] := 0;
 
-IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, aMax, vMax, vMin)
+IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, aMax, aMin, vMax, vMin)
 THEN
   AddProfile(profile);
 END_IF]]></ST>
@@ -333,13 +333,13 @@ profile.t[6] := 0;
 IF ABS(v0) > Constants.DoubleEpsilon
 THEN
   profile.t[3] := pd / v0;
-  IF (profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, 0.0, vMax, vMin))
+  IF (profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, 0.0, 0.0, vMax, vMin))
   THEN
     time_all_single_step := TRUE;
   END_IF
 ELSIF ABS(pd) < Constants.DoubleEpsilon
 THEN
-  IF (profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, 0.0, vMax, vMin))
+  IF (profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, 0.0, 0.0, vMax, vMin))
   THEN
     time_all_single_step := TRUE;
   END_IF
@@ -369,8 +369,8 @@ THEN
   // Solution 1
   profile.t[0] := -(v0 + h1)/aMax;
   profile.t[1] := 0;
-  profile.t[2] := -(vf + h1)/aMax;
-  IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, aMax, vMax, vMin)
+  profile.t[2] := (vf + h1)/aMin;
+  IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, aMax, aMin, vMax, vMin)
   THEN
     AddProfile(profile);
     IF return_after_found
@@ -383,8 +383,8 @@ THEN
   // Solution 2
   profile.t[0] := (-v0 + h1)/aMax;
   profile.t[1] := 0;
-  profile.t[2] := (-vf + h1)/aMax;
-  IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, aMax, vMax, vMin)
+  profile.t[2] := (vf - h1)/aMin;
+  IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, aMax, aMin, vMax, vMin)
   THEN
     AddProfile(profile);
     IF return_after_found

diff --git a/Struckig/Struckig/Struckig/Struckig/Position/PositionSecondOrderStep2.TcPOU b/Struckig/Struckig/Struckig/Struckig/Position/PositionSecondOrderStep2.TcPOU
@@ -107,20 +107,20 @@ VAR_INPUT
   vMax, vMin, aMax, aMin : LREAL;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[h1 := (-vd*vd + 2*aMax*((v0 + vf)*tf - 2*pd))/(aMax*aMax) + tf*tf;
+        <ST><![CDATA[h1 := (2*aMax*(pd - tf*vf) - 2*aMin*(pd - tf*v0) + vd*vd)/(aMax*aMin) + tf*tf;
 IF h1 >= 0
 THEN
   h1 := SQRT(h1);
 
-  profile.t[0] := vd/(2*aMax) + (tf - h1)/2;
+  profile.t[0] := (aMax*vd - aMax*aMin*(tf - h1))/(aMax*(aMax - aMin));
   profile.t[1] := h1;
   profile.t[2] := tf - (profile.t[0] + h1);
   profile.t[3] := 0;
   profile.t[4] := 0;
   profile.t[5] := 0;
   profile.t[6] := 0;
 
-  IF profile.CheckForSecondOrderVelocityWithTiming2(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, vMax, vMin)
+  IF profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
   THEN
     profile.pf := profile.p[7];
     time_acc0 := TRUE;
@@ -142,7 +142,7 @@ THEN
   profile.t[5] := 0;
   profile.t[6] := tf - (profile.t[0] + profile.t[1]);
   
-  IF profile.CheckForSecondOrderVelocityWithTiming2(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, vMax, vMin)
+  IF profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
   THEN
     profile.pf := profile.p[7];
     time_acc0 := TRUE;
@@ -160,7 +160,7 @@ profile.t[4] := 0;
 profile.t[5] := 0;
 profile.t[6] := vd/aMax;
 
-IF profile.CheckForSecondOrderVelocityWithTiming2(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, vMax, vMin)
+IF profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
 THEN
   profile.pf := profile.p[7];
   time_acc0 := TRUE;
@@ -187,7 +187,7 @@ THEN
   profile.t[5] := 0;
   profile.t[6] := 0;
 
-  IF profile.CheckForSecondOrderVelocityWithTiming2(ProfileJerkSigns.Uddu, ReachedLimits.None, tf, aMax, vMax, vMin)
+  IF profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.None, tf, aMax, aMin, vMax, vMin)
   THEN
     profile.pf := profile.p[7];
     time_none := TRUE;
@@ -208,7 +208,7 @@ profile.t[6] := 0;
 
 af := vd*vd/h1;
 
-IF ((aMin - 1E-12 < af) AND_THEN (af < aMax + 1E-12) AND_THEN profile.CheckForSecondOrderVelocityWithTiming2(ProfileJerkSigns.Uddu, ReachedLimits.None, tf, af, vMax, vMin))
+IF ((aMin - 1E-12 < af) AND_THEN (af < aMax + 1E-12) AND_THEN profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.None, tf, af, -af, vMax, vMin))
 THEN
   profile.pf := profile.p[7];
   time_none := TRUE;

diff --git a/Struckig/Struckig/Struckig/Struckig/Profile/ProfileDesc.TcPOU b/Struckig/Struckig/Struckig/Struckig/Profile/ProfileDesc.TcPOU
@@ -24,7 +24,7 @@ METHOD CheckForSecondOrder : BOOL
 VAR_INPUT
   jerkSigns : ProfileJerkSigns;
   limits : ReachedLimits;  
-  af : LREAL;
+  aUp, aDown : LREAL;
   vMax, vMin : LREAL;
 END_VAR
 VAR
@@ -83,21 +83,21 @@ END_IF
 
 IF jerkSigns = ProfileJerkSigns.Uddu
 THEN
-  j[0] := SEL(t[0] > 0, 0, af);
+  j[0] := SEL(t[0] > 0, 0, aUp);
   j[1] := 0;
-  j[2] := SEL(t[2] > 0, 0, -af);
+  j[2] := SEL(t[2] > 0, 0, aDown);
   j[3] := 0;
-  j[4] := SEL(t[4] > 0, 0, -af);
+  j[4] := SEL(t[4] > 0, 0, aDown);
   j[5] := 0;
-  j[6] := SEL(t[6] > 0, 0, af);          
+  j[6] := SEL(t[6] > 0, 0, aUp);          
 ELSE
-  j[0] := SEL(t[0] > 0, 0, af);
+  j[0] := SEL(t[0] > 0, 0, aUp);
   j[1] := 0;
-  j[2] := SEL(t[2] > 0, 0, -af);
+  j[2] := SEL(t[2] > 0, 0, aDown);
   j[3] := 0;
-  j[4] := SEL(t[4] > 0, 0, af);
+  j[4] := SEL(t[4] > 0, 0, aUp);
   j[5] := 0;
-  j[6] := SEL(t[6] > 0, 0, -af); 
+  j[6] := SEL(t[6] > 0, 0, aDown); 
 END_IF
 
 direction := SEL(vMax > 0, ProfileDirection.Down, ProfileDirection.Up);
@@ -145,7 +145,7 @@ METHOD CheckForSecondOrderVelocity : BOOL
 VAR_INPUT
   jerkSigns : ProfileJerkSigns;
   limits : ReachedLimits;  
-  af : LREAL;
+  aUp : LREAL;
 END_VAR
 VAR
   i : INT;
@@ -183,7 +183,7 @@ j[5] := 0;
 j[6] := 0;
 
 a[0] := 0;
-a[1] := af;
+a[1] := aUp;
 a[2] := 0;
 a[3] := 0;
 a[4] := 0;
@@ -198,7 +198,7 @@ END_FOR
 
 THIS^.JerkSigns := jerkSigns;
 THIS^.Limits := limits;
-THIS^.Direction := SEL(af > 0, ProfileDirection.Down, ProfileDirection.Up);
+THIS^.Direction := SEL(aUp > 0, ProfileDirection.Down, ProfileDirection.Up);
 
 // Velocity limit can be broken in the beginning if both initial velocity and acceleration are too high
 CheckForSecondOrderVelocity := ABS(v[6] - vf) < Constants.VelocityPrecision;]]></ST>
@@ -210,15 +210,15 @@ METHOD CheckForSecondOrderVelocityWithTiming : BOOL
 VAR_INPUT
   jerkSigns : ProfileJerkSigns;
   limits : ReachedLimits;  
-  af : LREAL;
+  aUp : LREAL;
 END_VAR
 VAR
   i : INT;
   aUppLim : LREAL;
   aLowLim : LREAL;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[CheckForSecondOrderVelocityWithTiming := CheckForSecondOrderVelocity(jerkSigns, limits, af); // AND_THEN (ABS(t[6] - tf) < t_precision);]]></ST>
+        <ST><![CDATA[CheckForSecondOrderVelocityWithTiming := CheckForSecondOrderVelocity(jerkSigns, limits, aUp); // AND_THEN (ABS(t[6] - tf) < t_precision);]]></ST>
       </Implementation>
     </Method>
     <Method Name="CheckForSecondOrderVelocityWithTiming2" Id="{915c6be4-07ad-4bf6-8b4d-9b5951f90edc}">
@@ -228,7 +228,7 @@ VAR_INPUT
   jerkSigns : ProfileJerkSigns;
   limits : ReachedLimits;  
   tf : LREAL;
-  af : LREAL;
+  aUp : LREAL;
   aMax : LREAL;
   aMin : LREAL;
 END_VAR
@@ -239,18 +239,19 @@ VAR
 END_VAR]]></Declaration>
       <Implementation>
         <ST><![CDATA[CheckForSecondOrderVelocityWithTiming2 :=
- (aMin - Constants.Epsilon < af)
- AND_THEN (af < aMax + Constants.Epsilon)
- AND_THEN CheckForSecondOrderVelocityWithTiming(jerkSigns, limits, af); // AND_THEN (ABS(t[6] - tf) < t_precision);]]></ST>
+ (aMin - Constants.Epsilon < aUp)
+ AND_THEN (aUp < aMax + Constants.Epsilon)
+ AND_THEN CheckForSecondOrderVelocityWithTiming(jerkSigns, limits, aUp); // AND_THEN (ABS(t[6] - tf) < t_precision);]]></ST>
       </Implementation>
     </Method>
-    <Method Name="CheckForSecondOrderVelocityWithTiming3" Id="{add5d654-e57e-4ae8-b083-d5a062804107}">
+    <Method Name="CheckForSecondOrderWithTiming" Id="{add5d654-e57e-4ae8-b083-d5a062804107}">
       <Declaration><![CDATA[/// For velocity interface
-METHOD CheckForSecondOrderVelocityWithTiming3 : BOOL
+METHOD CheckForSecondOrderWithTiming : BOOL
 VAR_INPUT
   jerkSigns : ProfileJerkSigns;
   limits : ReachedLimits;  
-  af : LREAL;
+  tf : LREAL;
+  aUp, aDown : LREAL;
   vMax, vMin : LREAL;
 END_VAR
 VAR
@@ -259,16 +260,17 @@ VAR
   aLowLim : LREAL;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[CheckForSecondOrderVelocityWithTiming3 := CheckForSecondOrder(jerkSigns, limits, af, vMax, vMin);]]></ST>
+        <ST><![CDATA[CheckForSecondOrderWithTiming := CheckForSecondOrder(jerkSigns, limits, aUp, aDown, vMax, vMin);]]></ST>
       </Implementation>
     </Method>
-    <Method Name="CheckForSecondOrderVelocityWithTiming4" Id="{2570a4cc-90d1-47de-bf99-989113385597}">
+    <Method Name="CheckForSecondOrderWithTiming2" Id="{2570a4cc-90d1-47de-bf99-989113385597}">
       <Declaration><![CDATA[/// For velocity interface
-METHOD CheckForSecondOrderVelocityWithTiming4 : BOOL
+METHOD CheckForSecondOrderWithTiming2 : BOOL
 VAR_INPUT
   jerkSigns : ProfileJerkSigns;
   limits : ReachedLimits;  
-  af : LREAL;
+  tf : LREAL;
+  aUp, aDown : LREAL;
   vMax, vMin : LREAL;
   aMax, aMin : LREAL;  
 END_VAR
@@ -278,10 +280,10 @@ VAR
   aLowLim : LREAL;
 END_VAR]]></Declaration>
       <Implementation>
-        <ST><![CDATA[CheckForSecondOrderVelocityWithTiming4 := 
-  (aMin - Constants.AccelerationEpsilon < af) 
-  AND_THEN (af < aMax + Constants.JerkEpsilon)
-  AND_THEN CheckForSecondOrder(jerkSigns, limits, af, vMax, vMin);]]></ST>
+        <ST><![CDATA[CheckForSecondOrderWithTiming2 := 
+  (aMin - Constants.AccelerationEpsilon < aDown) 
+  AND_THEN (aUp < aMax + Constants.JerkEpsilon)
+  AND_THEN CheckForSecondOrderWithTiming(jerkSigns, limits, tf, aUp, aDown, vMax, vMin);]]></ST>
       </Implementation>
     </Method>
     <Method Name="CheckForVelocity" Id="{dec39d38-9a64-41bc-99b6-6a7b67c016da}">

diff --git a/ruckig b/ruckig
+2 −0		CMakeLists.txt
+2 −2		include/ruckig/calculator_target.hpp
+61 −2		include/ruckig/position.hpp
+96 −7		include/ruckig/profile.hpp
+4 −4		include/ruckig/velocity.hpp
+138 −0		src/ruckig/position-second-step1.cpp
+119 −0		src/ruckig/position-second-step2.cpp
+1 −1		src/ruckig/position-third-step2.cpp
+1 −1		src/ruckig/velocity-second-step2.cpp
+1 −1		src/ruckig/velocity-third-step2.cpp