Skip to content

Commit

Permalink
ported(d4128fc): phase sync for second-order
Browse files Browse the repository at this point in the history
  • Loading branch information
@stefanbesler
stefanbesler committed May 12, 2024
1 parent 8963f36 commit 54237a8
Show file tree
Hide file tree
Showing 15 changed files with 179 additions and 133 deletions.
7 changes: 6 additions & 1 deletion Struckig/Struckig/Struckig/Struckig.plcproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@
<Compile Include="Struckig\Profile\_DataTypes\ProfileDirection.TcDUT">
<SubType>Code</SubType>
</Compile>
<Compile Include="Struckig\Profile\_DataTypes\ProfileJerkSigns.TcDUT">
<Compile Include="Struckig\Profile\_DataTypes\ProfileControlSigns.TcDUT">
<SubType>Code</SubType>
</Compile>
<Compile Include="Struckig\Profile\_DataTypes\ReachedLimits.TcDUT">
Expand Down Expand Up @@ -156,6 +156,11 @@
<SubType>Code</SubType>
</Compile>
</ItemGroup>
<ItemGroup>
<None Include="Struckig.tmc">
<SubType>Content</SubType>
</None>
</ItemGroup>
<ProjectExtensions>
<PlcProjectOptions>
<XmlArchive>
Expand Down
93 changes: 67 additions & 26 deletions Struckig/Struckig/Struckig/Struckig/Otg.TcPOU
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ VAR
_currentPosition, _currentVelocity, _currentAcceleration : ARRAY[0..ParameterList.MaxDoFs] OF LREAL; //< Current state
_targetPosition, _targetVelocity, _targetAcceleration : ARRAY[0..ParameterList.MaxDoFs] OF LREAL; //< Target state
_maxVelocity, _maxAcceleration, _maxJerk : ARRAY[0..ParameterList.MaxDoFs] OF LREAL; //< Kinematic constraints
_new_max_jerk : ARRAY[0..ParameterList.MaxDoFs] OF LREAL;
_new_phase_control : ARRAY[0..ParameterList.MaxDoFs] OF LREAL;
_minVelocity, _minAcceleration : ARRAY[0..ParameterList.MaxDoFs] OF LREAL; //< Optional kindematic constraints
_minDuration : LREAL := 0; //< Optional minimum duration of a trajectory. This is only considered if the Synchronization type is not none
_dt : LREAL; //< Time step between updates (cycle time) in [s]
Expand Down Expand Up @@ -431,7 +431,7 @@ END_IF
IF limiting_dof >= 0 AND_THEN phaseSychronizations >= 0 THEN
p_limiting REF= _profiles[limiting_dof];
IF IsInputCollinear(MaxJerk, p_limiting.Direction, limiting_dof, _new_max_jerk) THEN
IF IsInputCollinear(p_limiting.Direction, limiting_dof, _new_phase_control) THEN
found_time_synchronization := TRUE;
FOR dof:=0 TO _dofsEnd DO
IF NOT Enabled[dof] OR_ELSE dof = limiting_dof OR_ELSE _inp_per_dof_synchronization[dof] <> SynchronizationType.Phase THEN
Expand All @@ -442,40 +442,76 @@ IF limiting_dof >= 0 AND_THEN phaseSychronizations >= 0 THEN
t_profile := Duration - p.Brake.Duration - p.Accel.Duration;
p.t := p_limiting.t; // Copy timing information from limiting DoF
p.JerkSigns := p_limiting.JerkSigns;
p.ControlSigns := p_limiting.ControlSigns;
CASE _inp_per_dof_control_interface[dof]
OF
ControlInterfaceType.Position:
CASE p.JerkSigns
CASE p.ControlSigns
OF
ProfileJerkSigns.Uddu:
IF NOT p.CheckWithTiming2(ProfileJerkSigns.Uddu, ReachedLimits.None, t_profile, _new_max_jerk[dof], MaxVelocity[dof], _inp_min_velocity[dof], MaxAcceleration[dof], _inp_min_acceleration[dof], MaxJerk[dof])
ProfileControlSigns.Uddu:
IF NOT IsInfinity(MaxJerk[dof])
THEN
found_time_synchronization := FALSE;
IF NOT p.CheckWithTiming2(ProfileControlSigns.Uddu, ReachedLimits.None, t_profile, _new_phase_control[dof], MaxVelocity[dof], _inp_min_velocity[dof], MaxAcceleration[dof], _inp_min_acceleration[dof], MaxJerk[dof])
THEN
found_time_synchronization := FALSE;
END_IF
ELSE
IF NOT p.CheckForSecondOrderWithTiming2(ProfileControlSigns.Uddu, ReachedLimits.None, t_profile, _new_phase_control[dof], -_new_phase_control[dof], MaxVelocity[dof], _inp_min_velocity[dof], MaxAcceleration[dof], _inp_min_acceleration[dof])
THEN
found_time_synchronization := FALSE;
END_IF
END_IF
ProfileJerkSigns.Udud:
IF NOT p.CheckWithTiming2(ProfileJerkSigns.Udud, ReachedLimits.None, t_profile, _new_max_jerk[dof], MaxVelocity[dof], _inp_min_velocity[dof], MaxAcceleration[dof], _inp_min_acceleration[dof], MaxJerk[dof])
ProfileControlSigns.Udud:
IF NOT IsInfinity(MaxJerk[dof])
THEN
found_time_synchronization := FALSE;
END_IF
IF NOT p.CheckWithTiming2(ProfileControlSigns.Udud, ReachedLimits.None, t_profile, _new_phase_control[dof], MaxVelocity[dof], _inp_min_velocity[dof], MaxAcceleration[dof], _inp_min_acceleration[dof], MaxJerk[dof])
THEN
found_time_synchronization := FALSE;
END_IF
ELSE
IF NOT p.CheckForSecondOrderWithTiming2(ProfileControlSigns.Udud, ReachedLimits.None, t_profile, _new_phase_control[dof], -_new_phase_control[dof], MaxVelocity[dof], _inp_min_velocity[dof], MaxAcceleration[dof], _inp_min_acceleration[dof])
THEN
found_time_synchronization := FALSE;
END_IF
END_IF
END_CASE
ControlInterfaceType.Velocity:
CASE p.JerkSigns
CASE p.ControlSigns
OF
ProfileJerkSigns.Uddu:
IF NOT p.CheckForVelocityWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.None, _new_max_jerk[dof], MaxAcceleration[dof], _inp_min_acceleration[dof], MaxJerk[dof])
THEN
found_time_synchronization := FALSE;
ProfileControlSigns.Uddu:
IF NOT IsInfinity(MaxJerk[dof])
THEN
IF NOT p.CheckForVelocityWithTiming(ProfileControlSigns.Uddu, ReachedLimits.None, _new_phase_control[dof], MaxAcceleration[dof], _inp_min_acceleration[dof], MaxJerk[dof])
THEN
found_time_synchronization := FALSE;
END_IF
ELSE
IF NOT p.CheckForSecondOrderVelocityWithTiming2(ProfileControlSigns.Uddu, ReachedLimits.None, t_profile, _new_phase_control[dof], MaxAcceleration[dof], _inp_min_acceleration[dof])
THEN
found_time_synchronization := FALSE;
END_IF
END_IF
ProfileJerkSigns.Udud:
IF NOT p.CheckForVelocityWithTiming(ProfileJerkSigns.Udud, ReachedLimits.None, _new_max_jerk[dof], MaxAcceleration[dof], _inp_min_acceleration[dof], MaxJerk[dof])
THEN
found_time_synchronization := FALSE;
END_IF
ProfileControlSigns.Udud:
IF NOT IsInfinity(MaxJerk[dof])
THEN
IF NOT p.CheckForVelocityWithTiming(ProfileControlSigns.Udud, ReachedLimits.None, _new_phase_control[dof], MaxAcceleration[dof], _inp_min_acceleration[dof], MaxJerk[dof])
THEN
found_time_synchronization := FALSE;
END_IF
ELSE
IF NOT p.CheckForSecondOrderVelocityWithTiming2(ProfileControlSigns.Udud, ReachedLimits.None, t_profile, _new_phase_control[dof], MaxAcceleration[dof], _inp_min_acceleration[dof])
THEN
found_time_synchronization := FALSE;
END_IF
END_IF
END_CASE
END_CASE
Expand Down Expand Up @@ -692,18 +728,17 @@ END_VAR
<Declaration><![CDATA[/// Is the trajectory (in principle) phase synchronizable?
METHOD INTERNAL IsInputCollinear : BOOL
VAR_INPUT
jMax : REFERENCE TO ARRAY[0..ParameterList.MaxDoFs] OF LREAL;
limitingDirection : ProfileDirection;
limitingDof : INT;
new_max_jerk : REFERENCE TO ARRAY[0..ParameterList.MaxDoFs] OF LREAL;
new_phase_control : REFERENCE TO ARRAY[0..ParameterList.MaxDoFs] OF LREAL;
END_VAR
VAR_INST
scale_dof : INT := -1;
dof : INT;
pd : ARRAY[0..ParameterList.MaxDoFs] OF LREAL;
scale_vector : POINTER TO LREAL;
pd_scale, v0_scale, a0_scale, vf_scale, af_scale, scale, scale_limiting, current_scale : LREAL;
max_jerk_limiting : LREAL;
control_limiting : LREAL;
END_VAR
]]></Declaration>
<Implementation>
Expand Down Expand Up @@ -765,7 +800,13 @@ a0_scale := CurrentAcceleration[scale_dof] / scale;
af_scale := TargetAcceleration[scale_dof] / scale;
scale_limiting := scale_vector[limitingDof];
max_jerk_limiting := SEL(limitingDirection = ProfileDirection.Up, -jMax[limitingDof], jMax[limitingDof]);
control_limiting := SEL(limitingDirection = ProfileDirection.Up, -MaxJerk[limitingDof], MaxJerk[limitingDof]);
IF IsInfinity(MaxJerk[limitingDof])
THEN
control_limiting := SEL(limitingDirection = ProfileDirection.Up, _inp_min_acceleration[limitingDof], MaxAcceleration[limitingDof]);
END_IF
FOR dof:=0 TO _dofsEnd
DO
IF _inp_per_dof_synchronization[dof] <> SynchronizationType.Phase
Expand All @@ -783,7 +824,7 @@ DO
RETURN;
END_IF
new_max_jerk[dof] := max_jerk_limiting * current_scale / scale_limiting;
new_phase_control[dof] := control_limiting * current_scale / scale_limiting;
END_FOR
IsInputCollinear := TRUE;]]></ST>
Expand Down
10 changes: 5 additions & 5 deletions Struckig/Struckig/Struckig/Struckig/Position/PositionSecondOrderStep1.TcPOU
View file
Original file line number Diff line number Diff line change
Expand Up @@ -304,7 +304,7 @@ profile.t[4] := 0;
profile.t[5] := 0;
profile.t[6] := 0;
IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, aMax, aMin, vMax, vMin)
IF profile.CheckForSecondOrder(ProfileControlSigns.Uddu, ReachedLimits.Acc0, aMax, aMin, vMax, vMin)
THEN
AddProfile(profile);
END_IF]]></ST>
Expand Down Expand Up @@ -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, 0.0, vMax, vMin))
IF (profile.CheckForSecondOrder(ProfileControlSigns.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, 0.0, vMax, vMin))
IF (profile.CheckForSecondOrder(ProfileControlSigns.Uddu, ReachedLimits.None, 0.0, 0.0, vMax, vMin))
THEN
time_all_single_step := TRUE;
END_IF
Expand Down Expand Up @@ -370,7 +370,7 @@ THEN
profile.t[0] := -(v0 + h1)/aMax;
profile.t[1] := 0;
profile.t[2] := (vf + h1)/aMin;
IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, aMax, aMin, vMax, vMin)
IF profile.CheckForSecondOrder(ProfileControlSigns.Uddu, ReachedLimits.None, aMax, aMin, vMax, vMin)
THEN
AddProfile(profile);
IF return_after_found
Expand All @@ -384,7 +384,7 @@ THEN
profile.t[0] := (-v0 + h1)/aMax;
profile.t[1] := 0;
profile.t[2] := (vf - h1)/aMin;
IF profile.CheckForSecondOrder(ProfileJerkSigns.Uddu, ReachedLimits.None, aMax, aMin, vMax, vMin)
IF profile.CheckForSecondOrder(ProfileControlSigns.Uddu, ReachedLimits.None, aMax, aMin, vMax, vMin)
THEN
AddProfile(profile);
IF return_after_found
Expand Down
10 changes: 5 additions & 5 deletions Struckig/Struckig/Struckig/Struckig/Position/PositionSecondOrderStep2.TcPOU
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ THEN
profile.t[5] := 0;
profile.t[6] := 0;
IF profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
IF profile.CheckForSecondOrderWithTiming(ProfileControlSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
THEN
profile.pf := profile.p[7];
time_acc0 := TRUE;
Expand All @@ -142,7 +142,7 @@ THEN
profile.t[5] := 0;
profile.t[6] := tf - (profile.t[0] + profile.t[1]);
IF profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
IF profile.CheckForSecondOrderWithTiming(ProfileControlSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
THEN
profile.pf := profile.p[7];
time_acc0 := TRUE;
Expand All @@ -160,7 +160,7 @@ profile.t[4] := 0;
profile.t[5] := 0;
profile.t[6] := vd/aMax;
IF profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
IF profile.CheckForSecondOrderWithTiming(ProfileControlSigns.Uddu, ReachedLimits.Acc0, tf, aMax, aMin, vMax, vMin)
THEN
profile.pf := profile.p[7];
time_acc0 := TRUE;
Expand All @@ -187,7 +187,7 @@ THEN
profile.t[5] := 0;
profile.t[6] := 0;
IF profile.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.None, tf, aMax, aMin, vMax, vMin)
IF profile.CheckForSecondOrderWithTiming(ProfileControlSigns.Uddu, ReachedLimits.None, tf, aMax, aMin, vMax, vMin)
THEN
profile.pf := profile.p[7];
time_none := TRUE;
Expand All @@ -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.CheckForSecondOrderWithTiming(ProfileJerkSigns.Uddu, ReachedLimits.None, tf, af, -af, vMax, vMin))
IF ((aMin - 1E-12 < af) AND_THEN (af < aMax + 1E-12) AND_THEN profile.CheckForSecondOrderWithTiming(ProfileControlSigns.Uddu, ReachedLimits.None, tf, af, -af, vMax, vMin))
THEN
profile.pf := profile.p[7];
time_none := TRUE;
Expand Down
Loading

0 comments on commit 54237a8

Please sign in to comment.