Servoregulator with direct positioning

The function works as a combined servo regulator, deadzone and positioning ramp.
The features may be used individually or all at once.
It was developed to ensure trajectory tracking and positioning in automated cranes with anti-sway control,
but can be deployed for any positioning problem. The output of the function is a velocity reference.

A brief description of the features:

1. The servo regulator is used to enhance the real-time reference tracking of a system.
   It is a simple three-term regulator, where the output is determined as

	 uReg = kP*(uReference - uMeasured) + kI*(xR - xM) + kD*(aR - aM)

   If a non-zero PID delay time is set (see DelayPID attribute), input reference
   values will be placed in a queue and extracted to the PID regulator with the given delay.
   This is useful if a delay is unavoidable, but acceptable if it is uniform throughout travel.
   The D-part should not be used if a low-noise measured signal aM is not available.

2. The positioning ramp limits the output according to the equation

	 uRamp = -DelayRamp*amaxS + sqrt(DelayRamp^2*amaxS^2 + 2.0*|Delta x|*amaxS)

   where Delta x is the remaining distance to the command position.
   In the case of zero delay, uRamp(x) is just a constant deceleration curve,
   where amaxS is the magnitude of deceleration.

3. The deadzone sets the output to zero, if within its limits. A second larger deadzone that is
   activated after a specified time within its limits may also be used.

• Ssab_AntiSway

Current reference acceleration of the desired trajectory. The input is used by the PID regulator of the object.

Current reference velocity of the desired trajectory. The input is used by the PID regulator of the object.

Current reference position of the desired trajectory. The input is used by the PID regulator of the object.

Current measured acceleration. The input is used by the PID regulator of the object.

Current measured velocity. The input is used by the PID regulator of the object.

Current measured position. The input is used by the PID regulator of the object.

Command position. Final travel destination.

Maximum positive output velocity. This value may depend on other system variables,
such as local low-speed restrictions etc, and is therefore supplied as an input.

Maximum negative output velocity. Same as above, but this value should be < 0,
and reflects a restriction on movement in the other direction.

If true, and if the ramp is enabled, the control output is initially set to umaxP or umaxN,
depending on the sign of Delta x, the distance to the command position.
The output is then limited by the ramp. The effect is a very accurate positioning, given that
the relevant parameters are set correctly.
Direct positioning disables the PID regulator, as the two cannot be used simultaneously.

On/Off switch. If false, uR is simply passed untouched through the object.

"Soft" maximum acceleration, i.e. the acceleration or deceleration that the system
can be guaranteed to uphold.

Enabling or disabling of PID regulator. The positioning input overrides this setting, however.

PID constants:
kPID[0] - kP
kPID[1] - kI
kPID[2] - kD

Reference delay in seconds.

Limit as to how many time steps will be stored in the queue.
If DelayPID / ScanTime > maxdelaysteps, the full delay will not be realized.

Pointer to the reference list / queue.

Pointer to the reference list / queue.

Delay parameter in the ramp model. A larger delay causes a less steep deceleration.

Enabling or disabling of deadzone.

Determines the extension of the deadzone in both directions about the command position.
Interval = [xCommand - DeadZone, xCommand + DeadZone]

Enabling or disabling of timer deadzone.

Determines the extension of the deadzone in both directions about the command position.
Interval = [xCommand - TimerDeadZone, xCommand + TimerDeadZone]

Waiting time for the timer deadzone. The deadzone is activated after
TDZTime seconds within the timer deadzone.

Time counter of the timer deadzone.

Control output velocity.

Indicates whether the ramp is active or not, that is if it currently
limits the magnitude of the control output.

Indicates whether the deadzone is active or not, that is if it currently
limits the magnitude of the control output.
The output may be used as a confirmation of completed positioning.

Indicates whether the timer deadzone is active or not, that is if it currently
limits the magnitude of the control output.
The output may be used as a confirmation of completed positioning.