From ec8e940a4ee840169fbb91a76da178421150f8b6 Mon Sep 17 00:00:00 2001 From: Baptiste Labat Date: Sun, 6 Jul 2025 17:30:25 +0200 Subject: [PATCH 1/7] First commit (save pseudocode) --- src/multiwinchcontroller.jl | 83 +++++++++++++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 src/multiwinchcontroller.jl diff --git a/src/multiwinchcontroller.jl b/src/multiwinchcontroller.jl new file mode 100644 index 0000000..c0fec84 --- /dev/null +++ b/src/multiwinchcontroller.jl @@ -0,0 +1,83 @@ +using DiscretePIDs + + + +using TrajectoryLimiters + +ẍM = 50 # Maximum acceleration +ẋM = 10 # Maximum velocity +Ts = 0.005 # Sample time +r(t) = 2.5 + 3 * (t - floor(t)) # Reference to be smoothed +t = 0:Ts:3 # Time vector +R = r.(t) # An array of sampled position references + +limiter = TrajectoryLimiter(Ts, ẋM, ẍM) +state, ẍ = limiter(state, r(t)) + +set_torque_P = set_torque + + +# compute_differential_from_reference_linelength +length_left_order = reference_linelength + steering_order * max_differential_steering_length / 2 + depower_order * max_depower_length +length_right_order = reference_linelength + steering_order * max_differential_steering_length / 2 + depower_order * max_depower_length + +# compute feedback from measurements +steering_fdbk = (length_left_fdbk - length_right_fdbk) / max_differential_steering_length +depower_fdbk = ((length_left + length_right) / 2 - length_power) / max_depower_length + + +set_torque_left = pid(steering_fdbk - steering_order) + pid(depower_fdbk - depower_order) +set_torque_right = pid(steering_fdbk - steering_order) + pid(depower_fdbk - depower_order) + + +## Parameters of PID controller +sampling_time = 0.02 + +# Proportional gain from standard form (multiplying both proportional, integral and derivative terms) +proportional_gain = 1 +integral_time = 10 +derivative_time = 1 + +antiwindup_reset_time = 1 + +#parameter that limits the gain of the derivative term at high frequencies, typically ranges from 2 to 20, +maximum_derivative_gain = 5 + +# parameter that gives the proportion of the reference signal that appears in the proportional term. Default to 1 +# Might be reduced depending on feedforward or direct feedthrough used +fraction_of_set_point = 1 + +# Normalized output +umin = -1 +umax = 1 +initial_integral_state = 0 + +# Parameters to initialize derivative filter +initial_derivative_state = 0 +previous_feedback = feedback + + +parallel2standard(Kp, Ki, Kd) + +pid = DiscretePID(; K=proportional_gain, Ti=integral_time, + Td=derivative_time, Tt=antiwindup_reset_time, + N=maximum_derivative_gain, b=1fraction_of_set_point, + umin=-min_output, umax=max_output, Ts=sampling_time, I=initial_integral_state, D=initial_derivative_stat, yold=previous_feedback) + +u = pid(reference, feedback, uff) + +reset_state!(pid) + +ctrl = function (x, t) + y = (P.C*x)[] + r = 1 + u = pid(r, y) +end + +P = c2d(ss(tf(1, [1, 1])), sampling_time) + +## PI control +Ti = 1 +C = c2d(ControlSystemsBase.pid(K, Ti), Ts) +res = step(feedback(P * C), 3) +res2 = lsim(P, ctrl, 3) \ No newline at end of file From 2006fb43f2bf47b392efa21adfc87721e9c8c21c Mon Sep 17 00:00:00 2001 From: Baptiste Labat Date: Sun, 6 Jul 2025 22:07:50 +0200 Subject: [PATCH 2/7] Save work in progress --- src/multiwinchcontroller.jl | 185 +++++++++++++++++++++++++----------- 1 file changed, 129 insertions(+), 56 deletions(-) diff --git a/src/multiwinchcontroller.jl b/src/multiwinchcontroller.jl index c0fec84..a3658f1 100644 --- a/src/multiwinchcontroller.jl +++ b/src/multiwinchcontroller.jl @@ -1,83 +1,156 @@ using DiscretePIDs +struct Positive{T<:Real} + value::T + function Positive(x::T) where T<:Real + if x < 0 + throw(ArgumentError("Value must be positive")) + end + new{T}(x) + end +end +# Allow getting the wrapped value easily +Base.getindex(p::Positive) = p.value +Base.show(io::IO, p::Positive) = print(io, p.value) using TrajectoryLimiters +using Parameters -ẍM = 50 # Maximum acceleration -ẋM = 10 # Maximum velocity -Ts = 0.005 # Sample time -r(t) = 2.5 + 3 * (t - floor(t)) # Reference to be smoothed -t = 0:Ts:3 # Time vector -R = r.(t) # An array of sampled position references - -limiter = TrajectoryLimiter(Ts, ẋM, ẍM) -state, ẍ = limiter(state, r(t)) - -set_torque_P = set_torque +@with_kw struct LineLengthControllerSettings + max_setpoint_firstderivative::Real + max_setpoint_secondderivative::Real + sampling_time::Real +end -# compute_differential_from_reference_linelength -length_left_order = reference_linelength + steering_order * max_differential_steering_length / 2 + depower_order * max_depower_length -length_right_order = reference_linelength + steering_order * max_differential_steering_length / 2 + depower_order * max_depower_length +# Define settings of controller +settings = LineLengthControllerSettings( + max_setpoint_firstderivative=10.0, + max_setpoint_secondderivative=50.0, + sampling_time=0.02 +) +limiter = TrajectoryLimiter(settings.sampling_time, settings.max_setpoint_firstderivative, settings.max_setpoint_secondderivative) -# compute feedback from measurements -steering_fdbk = (length_left_fdbk - length_right_fdbk) / max_differential_steering_length -depower_fdbk = ((length_left + length_right) / 2 - length_power) / max_depower_length +# Ensure continuity +linelength_feedback = 0. +linelength_feedback_firstderivative = 0. +linelength_order = 1. +state = TrajectoryLimiters.State(linelength_feedback, linelength_feedback_firstderivative, linelength_order, 0.) -set_torque_left = pid(steering_fdbk - steering_order) + pid(depower_fdbk - depower_order) -set_torque_right = pid(steering_fdbk - steering_order) + pid(depower_fdbk - depower_order) +state, x = limiter(state, linelength_order) +linelength_setpoint = state.x -## Parameters of PID controller -sampling_time = 0.02 -# Proportional gain from standard form (multiplying both proportional, integral and derivative terms) -proportional_gain = 1 -integral_time = 10 -derivative_time = 1 +# TODO plug the real feedback (for now assume winch controller allows to track setpoint perfectly) +linelength_feedback = linelength_setpoint -antiwindup_reset_time = 1 -#parameter that limits the gain of the derivative term at high frequencies, typically ranges from 2 to 20, -maximum_derivative_gain = 5 +# Taking directly the setpoint might help to be more reactive +# However due to the delay from the feedback from control lines, +# the differential depower control might be late when reeling or unreeling +# Using directly the feedback from all three lines is a more robust solution which would work +# whatever is the type of controller used to contol the power line (force, speed, length) +# However, again due to the dealy from feedbacks and in the actuation chain it might be late +# in the phase where the reeling speed is changing. +# By chance this should not impact the differential steering control +use_linelength_direct_feedthrough = false +if use_linelength_direct_feedthrough + reference_linelength = linelength_setpoint +else + reference_linelength = linelength_feedback -# parameter that gives the proportion of the reference signal that appears in the proportional term. Default to 1 -# Might be reduced depending on feedforward or direct feedthrough used -fraction_of_set_point = 1 -# Normalized output -umin = -1 -umax = 1 -initial_integral_state = 0 -# Parameters to initialize derivative filter -initial_derivative_state = 0 -previous_feedback = feedback +@with_kw struct DifferentialLineLengthControllerSettings + max_differential_steering_length::Real + max_depower_length::Real + sampling_time::Real +end +# Define settings of controller +settings = DifferentialLineLengthControllerSettings( + max_differential_steering_length=1.0, + max_depower_length=1.0, + sampling_time=0.02 +) -parallel2standard(Kp, Ki, Kd) -pid = DiscretePID(; K=proportional_gain, Ti=integral_time, - Td=derivative_time, Tt=antiwindup_reset_time, - N=maximum_derivative_gain, b=1fraction_of_set_point, - umin=-min_output, umax=max_output, Ts=sampling_time, I=initial_integral_state, D=initial_derivative_stat, yold=previous_feedback) +# compute_differential_from_reference_linelength +# We assume left/right to be back lines (attached on the trailing edge, at the back compared to power line with is in front of center of effort) +length_left_setpoint = reference_linelength + steering_order * settings.max_differential_steering_length / 2 + depower_order * settings.max_depower_length +length_right_setpoint = reference_linelength + steering_order * settings.max_differential_steering_length / 2 + depower_order * settings.max_depower_length + +# Compute feedback from measurements +steering_fdbk = (length_left_feedback - length_right_feedback) / settings.max_differential_steering_length +depower_fdbk = ((length_left_feedback + length_right_feedback) / 2 - length_power_feedback) / settings.max_depower_length + +# Solution 1: use decoupled pid for steering and depower +steering_pid = my_pid() +depower_pid = my_pid() +set_torque_left = steering_pid(steering_fdbk - steering_order) + depower_pid(depower_fdbk - depower_order) +set_torque_right = steering_pid(steering_fdbk - steering_order) + depower_pid(depower_fdbk - depower_order) + +# Solution 2: use two identifcal line length pid controller +controllerlinelength_pid = my_pid() +set_torque_left = controllinelength_pid(length_left_setpoint, length_left_feedback) +set_torque_right = controllinelength_pid(length_right_setpoint, length_right_feedback) + +function my_pid() + ## Parameters of PID controller + # Proportional gain from standard form (multiplying both proportional, integral and derivative terms) + proportional_gain = 1 + integral_time = 10 + derivative_time = 1 + + antiwindup_reset_time = 1 + + #parameter that limits the gain of the derivative term at high frequencies, typically ranges from 2 to 20, + maximum_derivative_gain = 5 + + # parameter that gives the proportion of the reference signal that appears in the proportional term. Default to 1 + # Might be reduced depending on feedforward or direct feedthrough used + fraction_of_set_point = 1 + + # Normalized output + umin = -1 + umax = 1 + initial_integral_state = 0 + + # Parameters to initialize derivative filter + initial_derivative_state = 0 + previous_feedback = feedback + + + parallel2standard(Kp, Ki, Kd) + + pid = DiscretePID(; K=proportional_gain, Ti=integral_time, + Td=derivative_time, Tt=antiwindup_reset_time, + N=maximum_derivative_gain, b=1fraction_of_set_point, + umin=-min_output, umax=max_output, Ts=sampling_time, I=initial_integral_state, D=initial_derivative_stat, yold=previous_feedback) + return pid +end +# u = pid(reference, feedback, uff) -u = pid(reference, feedback, uff) +# reset_state!(pid) -reset_state!(pid) +# ctrl = function (x, t) +# y = (P.C*x)[] +# r = 1 +# u = pid(r, y) +# end -ctrl = function (x, t) - y = (P.C*x)[] - r = 1 - u = pid(r, y) -end +# P = c2d(ss(tf(1, [1, 1])), sampling_time) -P = c2d(ss(tf(1, [1, 1])), sampling_time) +# ## PI control +# Ti = 1 +# C = c2d(ControlSystemsBase.pid(K, Ti), Ts) +# res = step(feedback(P * C), 3) +# res2 = lsim(P, ctrl, 3) -## PI control -Ti = 1 -C = c2d(ControlSystemsBase.pid(K, Ti), Ts) -res = step(feedback(P * C), 3) -res2 = lsim(P, ctrl, 3) \ No newline at end of file +@testitem "First tests" begin + x = 1 + @test x =1 +end \ No newline at end of file From fb9074c08f76f8c8fc3e229a68b3cfcd759dfb8a Mon Sep 17 00:00:00 2001 From: Baptiste Labat Date: Sun, 6 Jul 2025 22:24:45 +0200 Subject: [PATCH 3/7] My first Julia running code ! --- src/multiwinchcontroller.jl | 104 +++++++++++++----------------------- 1 file changed, 36 insertions(+), 68 deletions(-) diff --git a/src/multiwinchcontroller.jl b/src/multiwinchcontroller.jl index a3658f1..64c601f 100644 --- a/src/multiwinchcontroller.jl +++ b/src/multiwinchcontroller.jl @@ -1,21 +1,7 @@ using DiscretePIDs - -struct Positive{T<:Real} - value::T - function Positive(x::T) where T<:Real - if x < 0 - throw(ArgumentError("Value must be positive")) - end - new{T}(x) - end -end - -# Allow getting the wrapped value easily -Base.getindex(p::Positive) = p.value -Base.show(io::IO, p::Positive) = print(io, p.value) - using TrajectoryLimiters using Parameters +using Test @with_kw struct LineLengthControllerSettings @@ -24,27 +10,22 @@ using Parameters sampling_time::Real end -# Define settings of controller settings = LineLengthControllerSettings( max_setpoint_firstderivative=10.0, max_setpoint_secondderivative=50.0, sampling_time=0.02 ) -limiter = TrajectoryLimiter(settings.sampling_time, settings.max_setpoint_firstderivative, settings.max_setpoint_secondderivative) -# Ensure continuity -linelength_feedback = 0. -linelength_feedback_firstderivative = 0. +limiter = TrajectoryLimiter(settings.sampling_time, settings.max_setpoint_firstderivative, settings.max_setpoint_secondderivative) -linelength_order = 1. -state = TrajectoryLimiters.State(linelength_feedback, linelength_feedback_firstderivative, linelength_order, 0.) +linelength_feedback = 0.0 +linelength_feedback_firstderivative = 0.0 +linelength_order = 1.0 -state, x = limiter(state, linelength_order) +state = TrajectoryLimiters.State(linelength_feedback, linelength_feedback_firstderivative, linelength_order, 0.0) +state, _ = limiter(state, linelength_order) linelength_setpoint = state.x - - -# TODO plug the real feedback (for now assume winch controller allows to track setpoint perfectly) linelength_feedback = linelength_setpoint @@ -70,33 +51,28 @@ else sampling_time::Real end -# Define settings of controller -settings = DifferentialLineLengthControllerSettings( +differential_settings = DifferentialLineLengthControllerSettings( max_differential_steering_length=1.0, max_depower_length=1.0, sampling_time=0.02 ) +# Dummy orders (replace with actual signals) +steering_order = 0.2 +depower_order = 0.1 -# compute_differential_from_reference_linelength -# We assume left/right to be back lines (attached on the trailing edge, at the back compared to power line with is in front of center of effort) -length_left_setpoint = reference_linelength + steering_order * settings.max_differential_steering_length / 2 + depower_order * settings.max_depower_length -length_right_setpoint = reference_linelength + steering_order * settings.max_differential_steering_length / 2 + depower_order * settings.max_depower_length +# Compute setpoints +length_left_setpoint = reference_linelength + steering_order * differential_settings.max_differential_steering_length / 2 + depower_order * differential_settings.max_depower_length +length_right_setpoint = reference_linelength - steering_order * differential_settings.max_differential_steering_length / 2 + depower_order * differential_settings.max_depower_length -# Compute feedback from measurements -steering_fdbk = (length_left_feedback - length_right_feedback) / settings.max_differential_steering_length -depower_fdbk = ((length_left_feedback + length_right_feedback) / 2 - length_power_feedback) / settings.max_depower_length +# Dummy feedbacks (replace with real sensor inputs) +length_left_feedback = 0 +length_right_feedback = 0 +length_power_feedback = 0 -# Solution 1: use decoupled pid for steering and depower -steering_pid = my_pid() -depower_pid = my_pid() -set_torque_left = steering_pid(steering_fdbk - steering_order) + depower_pid(depower_fdbk - depower_order) -set_torque_right = steering_pid(steering_fdbk - steering_order) + depower_pid(depower_fdbk - depower_order) - -# Solution 2: use two identifcal line length pid controller -controllerlinelength_pid = my_pid() -set_torque_left = controllinelength_pid(length_left_setpoint, length_left_feedback) -set_torque_right = controllinelength_pid(length_right_setpoint, length_right_feedback) +# Feedback errors +steering_fdbk = (length_left_feedback - length_right_feedback) / differential_settings.max_differential_steering_length +depower_fdbk = ((length_left_feedback + length_right_feedback) / 2 - length_power_feedback) / differential_settings.max_depower_length function my_pid() ## Parameters of PID controller @@ -104,6 +80,7 @@ function my_pid() proportional_gain = 1 integral_time = 10 derivative_time = 1 + sampling_time = 0.02 antiwindup_reset_time = 1 @@ -115,42 +92,33 @@ function my_pid() fraction_of_set_point = 1 # Normalized output - umin = -1 - umax = 1 + min_output = -1 + max_output = 1 initial_integral_state = 0 # Parameters to initialize derivative filter initial_derivative_state = 0 - previous_feedback = feedback + previous_feedback = 0 - parallel2standard(Kp, Ki, Kd) + # parallel2standard(Kp, Ki, Kd) pid = DiscretePID(; K=proportional_gain, Ti=integral_time, Td=derivative_time, Tt=antiwindup_reset_time, N=maximum_derivative_gain, b=1fraction_of_set_point, - umin=-min_output, umax=max_output, Ts=sampling_time, I=initial_integral_state, D=initial_derivative_stat, yold=previous_feedback) + umin=min_output, umax=max_output, Ts=sampling_time, I=initial_integral_state, D=initial_derivative_state, yold=previous_feedback) return pid end -# u = pid(reference, feedback, uff) - -# reset_state!(pid) -# ctrl = function (x, t) -# y = (P.C*x)[] -# r = 1 -# u = pid(r, y) -# end - -# P = c2d(ss(tf(1, [1, 1])), sampling_time) +# Solution 1: decoupled PIDs +steering_pid = my_pid() +depower_pid = my_pid() -# ## PI control -# Ti = 1 -# C = c2d(ControlSystemsBase.pid(K, Ti), Ts) -# res = step(feedback(P * C), 3) -# res2 = lsim(P, ctrl, 3) +set_torque_left = steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) +set_torque_right = -steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) -@testitem "First tests" begin - x = 1 - @test x =1 +# Solution 2: individual line controllers (optional alternative) +controller_linelength_pid = my_pid() +set_torque_left_2 = controller_linelength_pid(length_left_setpoint, length_left_feedback) +set_torque_right_2 = controller_linelength_pid(length_right_setpoint, length_right_feedback) end \ No newline at end of file From 0674c4e0af370f30b3aee9efb622bb8b63c22c01 Mon Sep 17 00:00:00 2001 From: Baptiste Labat Date: Sun, 6 Jul 2025 22:29:11 +0200 Subject: [PATCH 4/7] Update missing dependencies --- Project.toml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 120cf40..372f045 100644 --- a/Project.toml +++ b/Project.toml @@ -4,6 +4,7 @@ authors = ["Uwe Fechner and contributors"] version = "0.5.3" [deps] +DiscretePIDs = "c1363496-6848-4723-8758-079b737f6baf" DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533" @@ -15,6 +16,7 @@ StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" StructTypes = "856f2bd8-1eba-4b0a-8007-ebc267875bd4" Timers = "21f18d07-b854-4dab-86f0-c15a3821819a" +TrajectoryLimiters = "9792e600-fe43-4e4e-833b-462f466b8006" WinchModels = "7dcfa46b-7979-4771-bbf4-0aee0da42e1f" YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6" @@ -45,9 +47,9 @@ julia = "1.10, 1.11" Aqua = "4c88cf16-eb10-579e-8560-4a9242c79595" ControlPlots = "23c2ee80-7a9e-4350-b264-8e670f12517c" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" -RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" NOMAD = "02130f1c-4665-5b79-af82-ff1385104aa0" +RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" [targets] From 6e7725398bf5b53c0cb31630565e7ce8850b1971 Mon Sep 17 00:00:00 2001 From: Baptiste Labat Date: Wed, 9 Jul 2025 00:15:22 +0200 Subject: [PATCH 5/7] Renaming and docstring --- src/multiwinchcontroller.jl | 199 +++++++++++++++++++++--------------- 1 file changed, 117 insertions(+), 82 deletions(-) diff --git a/src/multiwinchcontroller.jl b/src/multiwinchcontroller.jl index 64c601f..9394d6f 100644 --- a/src/multiwinchcontroller.jl +++ b/src/multiwinchcontroller.jl @@ -4,25 +4,25 @@ using Parameters using Test -@with_kw struct LineLengthControllerSettings - max_setpoint_firstderivative::Real - max_setpoint_secondderivative::Real - sampling_time::Real +@with_kw struct ReferenceLineLengthControllerSettings + max_setpoint_speed::Real # Maximum reel in or reel out speed allowed. Must be lower than winch limits [m/s] + max_setpoint_acceleration::Real # Maximum variation of reel in/reel out speed allowed. Must be lower than winch limits [m/s²] + sampling_time::Real # Sampling time used by the discrete controller. Must be small compared to the ratio speed/acceleration [s] end -settings = LineLengthControllerSettings( - max_setpoint_firstderivative=10.0, - max_setpoint_secondderivative=50.0, - sampling_time=0.02 -) +settings = ReferenceLineLengthControllerSettings( + max_setpoint_speed=10.0, + max_setpoint_acceleration=50.0, + sampling_time=0.02) + -limiter = TrajectoryLimiter(settings.sampling_time, settings.max_setpoint_firstderivative, settings.max_setpoint_secondderivative) +limiter = TrajectoryLimiter(settings.sampling_time, settings.max_setpoint_speed, settings.max_setpoint_acceleration) linelength_feedback = 0.0 -linelength_feedback_firstderivative = 0.0 +linelength_feedback_speed = 0.0 linelength_order = 1.0 -state = TrajectoryLimiters.State(linelength_feedback, linelength_feedback_firstderivative, linelength_order, 0.0) +state = TrajectoryLimiters.State(linelength_feedback, linelength_feedback_speed, linelength_order, 0.0) state, _ = limiter(state, linelength_order) linelength_setpoint = state.x @@ -44,81 +44,116 @@ else reference_linelength = linelength_feedback + # ud_prime: depower setting in the range of 0 to 1, 0 means fully powered, 1 means fully depowered -@with_kw struct DifferentialLineLengthControllerSettings - max_differential_steering_length::Real - max_depower_length::Real + # TODO: define if left/right is in kitesurfer (upwind in the back) or paraglider convention (upwind in the front) + + # Left/right symmetry is assumed + # Lines are assumed of the same length when steering and depower are zero. + # This corresponds to the convention already used @TU Delft + # Note this is not the usual convention for kitesurfers which defines that lines are equal when kitebar is in full power position + # No trim is considered for now + + @with_kw struct DifferentialLineLengthControllerSettings + max_differential_steering_length::Real # Absolute value of expected length differential between left and right line when steering is 1 or -1 [m] + max_depower_length::Real # When steering is zero, absolute value of expected length differential between front and back line [m] sampling_time::Real -end + end -differential_settings = DifferentialLineLengthControllerSettings( + differential_settings = DifferentialLineLengthControllerSettings( max_differential_steering_length=1.0, max_depower_length=1.0, sampling_time=0.02 -) - -# Dummy orders (replace with actual signals) -steering_order = 0.2 -depower_order = 0.1 - -# Compute setpoints -length_left_setpoint = reference_linelength + steering_order * differential_settings.max_differential_steering_length / 2 + depower_order * differential_settings.max_depower_length -length_right_setpoint = reference_linelength - steering_order * differential_settings.max_differential_steering_length / 2 + depower_order * differential_settings.max_depower_length - -# Dummy feedbacks (replace with real sensor inputs) -length_left_feedback = 0 -length_right_feedback = 0 -length_power_feedback = 0 - -# Feedback errors -steering_fdbk = (length_left_feedback - length_right_feedback) / differential_settings.max_differential_steering_length -depower_fdbk = ((length_left_feedback + length_right_feedback) / 2 - length_power_feedback) / differential_settings.max_depower_length - -function my_pid() - ## Parameters of PID controller - # Proportional gain from standard form (multiplying both proportional, integral and derivative terms) - proportional_gain = 1 - integral_time = 10 - derivative_time = 1 - sampling_time = 0.02 - - antiwindup_reset_time = 1 - - #parameter that limits the gain of the derivative term at high frequencies, typically ranges from 2 to 20, - maximum_derivative_gain = 5 - - # parameter that gives the proportion of the reference signal that appears in the proportional term. Default to 1 - # Might be reduced depending on feedforward or direct feedthrough used - fraction_of_set_point = 1 - - # Normalized output - min_output = -1 - max_output = 1 - initial_integral_state = 0 - - # Parameters to initialize derivative filter - initial_derivative_state = 0 - previous_feedback = 0 - - - # parallel2standard(Kp, Ki, Kd) - - pid = DiscretePID(; K=proportional_gain, Ti=integral_time, - Td=derivative_time, Tt=antiwindup_reset_time, - N=maximum_derivative_gain, b=1fraction_of_set_point, - umin=min_output, umax=max_output, Ts=sampling_time, I=initial_integral_state, D=initial_derivative_state, yold=previous_feedback) - return pid -end - -# Solution 1: decoupled PIDs -steering_pid = my_pid() -depower_pid = my_pid() - -set_torque_left = steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) -set_torque_right = -steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) + ) + + # Dummy orders (replace with actual signals) + steering_order = 0.2 + depower_order = 0.1 + + # Compute setpoints + length_left_setpoint = reference_linelength + steering_order * differential_settings.max_differential_steering_length / 2 + depower_order * differential_settings.max_depower_length + length_right_setpoint = reference_linelength - steering_order * differential_settings.max_differential_steering_length / 2 + depower_order * differential_settings.max_depower_length + + # Dummy feedbacks (replace with real sensor inputs) + length_left_feedback = 0 + length_right_feedback = 0 + length_power_feedback = 0 + + # Feedback errors + steering_fdbk = (length_left_feedback - length_right_feedback) / differential_settings.max_differential_steering_length + depower_fdbk = ((length_left_feedback + length_right_feedback) / 2 - length_power_feedback) / differential_settings.max_depower_length + + function my_pid() + ## Parameters of PID controller + # Proportional gain from standard form (multiplying both proportional, integral and derivative terms) + proportional_gain = 1 + integral_time = 10 + derivative_time = 1 + sampling_time = 0.02 + + antiwindup_reset_time = 1 + + #parameter that limits the gain of the derivative term at high frequencies, typically ranges from 2 to 20, + maximum_derivative_gain = 5 + + # parameter that gives the proportion of the reference signal that appears in the proportional term. Default to 1 + # Might be reduced depending on feedforward or direct feedthrough used + fraction_of_set_point = 1 + + # Normalized output + min_output = -1 + max_output = 1 + initial_integral_state = 0 + + # Parameters to initialize derivative filter + initial_derivative_state = 0 + previous_feedback = 0 + + + # parallel2standard(Kp, Ki, Kd) + + pid = DiscretePID(; K=proportional_gain, Ti=integral_time, + Td=derivative_time, Tt=antiwindup_reset_time, + N=maximum_derivative_gain, b=1fraction_of_set_point, + umin=min_output, umax=max_output, Ts=sampling_time, I=initial_integral_state, D=initial_derivative_state, yold=previous_feedback) + return pid + end + + # Solution 1: decoupled PIDs + steering_pid = my_pid() + depower_pid = my_pid() + + set_torque_left = steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) + set_torque_right = -steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) + + # Solution 2: individual line controllers (optional alternative) + controller_linelength_pid = my_pid() + set_torque_left_2 = controller_linelength_pid(length_left_setpoint, length_left_feedback) + set_torque_right_2 = controller_linelength_pid(length_right_setpoint, length_right_feedback) + + + """ + winch: + winch_model: "AsyncMachine" # or TorqueControlledMachine + max_force: 4000 # maximal (nominal) tether force; short overload allowed [N] + v_ro_max: 8.0 # maximal reel-out speed [m/s] + v_ro_min: -8.0 # minimal reel-out speed (=max reel-in speed) [m/s] + drum_radius: 0.1615 # radius of the drum [m] + max_acc: 4.0 # maximal acceleration of the winch [m/s²] + gear_ratio: 6.2 # gear ratio of the winch [-] + inertia_total: 0.204 # total inertia, as seen from the motor/generator [kgm²] + f_coulomb: 122.0 # coulomb friction [N] + c_vf: 30.6 # coefficient for the viscous friction [Ns/m] + p_speed: 1.0 # proportional gain of the winch speed controller [-] + i_speed: 0.1 # integral gain of the winch speed controller [-] + """ + + function main() + settings = LineLengthControllerSettings( + max_setpoint_speed=10.0, + max_setpoint_acceleration=50.0, + sampling_time=0.02) + return settings + end -# Solution 2: individual line controllers (optional alternative) -controller_linelength_pid = my_pid() -set_torque_left_2 = controller_linelength_pid(length_left_setpoint, length_left_feedback) -set_torque_right_2 = controller_linelength_pid(length_right_setpoint, length_right_feedback) end \ No newline at end of file From bd963b80e19406ef00cbb072ca37f7b5c12ab58e Mon Sep 17 00:00:00 2001 From: Baptiste Labat Date: Wed, 9 Jul 2025 00:29:12 +0200 Subject: [PATCH 6/7] GPT clean --- src/multiwinchcontroller.jl | 271 +++++++++++++++++++----------------- 1 file changed, 147 insertions(+), 124 deletions(-) diff --git a/src/multiwinchcontroller.jl b/src/multiwinchcontroller.jl index 9394d6f..a47cd5d 100644 --- a/src/multiwinchcontroller.jl +++ b/src/multiwinchcontroller.jl @@ -1,33 +1,143 @@ -using DiscretePIDs using TrajectoryLimiters using Parameters +using DiscretePIDs using Test - +# ---------------------------------------- +# Reference Line Length Control Settings +# ---------------------------------------- @with_kw struct ReferenceLineLengthControllerSettings - max_setpoint_speed::Real # Maximum reel in or reel out speed allowed. Must be lower than winch limits [m/s] - max_setpoint_acceleration::Real # Maximum variation of reel in/reel out speed allowed. Must be lower than winch limits [m/s²] - sampling_time::Real # Sampling time used by the discrete controller. Must be small compared to the ratio speed/acceleration [s] + max_setpoint_speed::Real # Maximum reel in/out speed allowed [m/s] + max_setpoint_acceleration::Real # Max variation of reel speed [m/s²] + sampling_time::Real # Sampling time for discrete controller [s] +end + +function create_trajectory_limiter(settings::ReferenceLineLengthControllerSettings) + return TrajectoryLimiter(settings.sampling_time, settings.max_setpoint_speed, settings.max_setpoint_acceleration) +end + +function update_line_length_feedback( + limiter::TrajectoryLimiter, + feedback::Real, feedback_speed::Real, + order::Real +) + state = TrajectoryLimiters.State(feedback, feedback_speed, order, 0.0) + state, _ = limiter(state, order) + return state.x # New line length setpoint +end + +# ---------------------------------------- +# Differential Line Length Control Settings +# ---------------------------------------- +@with_kw struct DifferentialLineLengthControllerSettings + max_differential_steering_length::Real # [m] + max_depower_length::Real # [m] + sampling_time::Real +end + +# ---------------------------------------- +# PID Factory Function +# ---------------------------------------- +function make_pid(; + # Proportional gain from standard form (multiplying both P, I and D terms) + Kp=1.0, + + # Integral time constant: how fast the integral action builds up + Ti=10.0, + + # Derivative time constant: how strongly it reacts to rate of change + Td=1.0, + + # Sampling time of the discrete controller [s] + Ts=0.02, + + # Antiwindup reset time (Tt): determines how fast integral antiwindup reacts + Tt=1.0, + + # Parameter that limits the gain of the derivative term at high frequencies (2–20 typical) + N=5.0, + + # Proportion of the reference signal in the proportional term (usually 1.0; <1 if feedforward used) + b=1.0, + + # Output limits (normalized) + umin=-1.0, + umax=1.0, + + # Initial state of integral term + I=0.0, + + # Initial state of derivative filter + D=0.0, + + # Previous feedback value, needed for derivative filter init + yold=0.0 +) + return DiscretePID( + K=Kp, Ti=Ti, Td=Td, Ts=Ts, Tt=Tt, + N=N, b=b, umin=umin, umax=umax, + I=I, D=D, yold=yold + ) +end + +# ---------------------------------------- +# Control Logic +# ---------------------------------------- +function compute_control( + ref_linelength_setpoint::Real, + steering_order::Real, depower_order::Real, + length_left_feedback::Real, length_right_feedback::Real, length_power_feedback::Real, + settings::DifferentialLineLengthControllerSettings +) + # Compute setpoints for left/right line lengths + left_sp = ref_linelength_setpoint + steering_order * settings.max_differential_steering_length / 2 + depower_order * settings.max_depower_length + right_sp = ref_linelength_setpoint - steering_order * settings.max_differential_steering_length / 2 + depower_order * settings.max_depower_length + + # Compute normalized feedback errors + steering_fdbk = (length_left_feedback - length_right_feedback) / settings.max_differential_steering_length + depower_fdbk = ((length_left_feedback + length_right_feedback)/2 - length_power_feedback) / settings.max_depower_length + + # Create PIDs + steering_pid = make_pid(Ts=settings.sampling_time) + depower_pid = make_pid(Ts=settings.sampling_time) + + # Control torques (Solution 1: decoupled) + torque_left = steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) + torque_right = -steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) + + # Control torques (Solution 2: individual line PIDs) + controller_pid = make_pid(Ts=settings.sampling_time) + torque_left_2 = controller_pid(left_sp, length_left_feedback) + torque_right_2 = controller_pid(right_sp, length_right_feedback) + + return torque_left, torque_right, torque_left_2, torque_right_2 end -settings = ReferenceLineLengthControllerSettings( - max_setpoint_speed=10.0, - max_setpoint_acceleration=50.0, - sampling_time=0.02) +# ---------------------------------------- +# Example Usage +# ---------------------------------------- +# Settings +ref_settings = ReferenceLineLengthControllerSettings( + max_setpoint_speed = 10.0, + max_setpoint_acceleration = 50.0, + sampling_time = 0.02 +) -limiter = TrajectoryLimiter(settings.sampling_time, settings.max_setpoint_speed, settings.max_setpoint_acceleration) +differential_settings = DifferentialLineLengthControllerSettings( + max_differential_steering_length = 1.0, + max_depower_length = 1.0, + sampling_time = 0.02 +) +# Initial feedback and order values linelength_feedback = 0.0 linelength_feedback_speed = 0.0 linelength_order = 1.0 -state = TrajectoryLimiters.State(linelength_feedback, linelength_feedback_speed, linelength_order, 0.0) -state, _ = limiter(state, linelength_order) - -linelength_setpoint = state.x -linelength_feedback = linelength_setpoint - +# Trajectory limiting +limiter = create_trajectory_limiter(ref_settings) +linelength_setpoint = update_line_length_feedback(limiter, linelength_feedback, linelength_feedback_speed, linelength_order) # Taking directly the setpoint might help to be more reactive # However due to the delay from the feedback from control lines, @@ -38,10 +148,7 @@ linelength_feedback = linelength_setpoint # in the phase where the reeling speed is changing. # By chance this should not impact the differential steering control use_linelength_direct_feedthrough = false -if use_linelength_direct_feedthrough - reference_linelength = linelength_setpoint -else - reference_linelength = linelength_feedback +reference_linelength = use_linelength_direct_feedthrough ? linelength_setpoint : linelength_feedback # ud_prime: depower setting in the range of 0 to 1, 0 means fully powered, 1 means fully depowered @@ -53,107 +160,23 @@ else # This corresponds to the convention already used @TU Delft # Note this is not the usual convention for kitesurfers which defines that lines are equal when kitebar is in full power position # No trim is considered for now - - @with_kw struct DifferentialLineLengthControllerSettings - max_differential_steering_length::Real # Absolute value of expected length differential between left and right line when steering is 1 or -1 [m] - max_depower_length::Real # When steering is zero, absolute value of expected length differential between front and back line [m] - sampling_time::Real - end - - differential_settings = DifferentialLineLengthControllerSettings( - max_differential_steering_length=1.0, - max_depower_length=1.0, - sampling_time=0.02 - ) - - # Dummy orders (replace with actual signals) - steering_order = 0.2 - depower_order = 0.1 - - # Compute setpoints - length_left_setpoint = reference_linelength + steering_order * differential_settings.max_differential_steering_length / 2 + depower_order * differential_settings.max_depower_length - length_right_setpoint = reference_linelength - steering_order * differential_settings.max_differential_steering_length / 2 + depower_order * differential_settings.max_depower_length - - # Dummy feedbacks (replace with real sensor inputs) - length_left_feedback = 0 - length_right_feedback = 0 - length_power_feedback = 0 - - # Feedback errors - steering_fdbk = (length_left_feedback - length_right_feedback) / differential_settings.max_differential_steering_length - depower_fdbk = ((length_left_feedback + length_right_feedback) / 2 - length_power_feedback) / differential_settings.max_depower_length - - function my_pid() - ## Parameters of PID controller - # Proportional gain from standard form (multiplying both proportional, integral and derivative terms) - proportional_gain = 1 - integral_time = 10 - derivative_time = 1 - sampling_time = 0.02 - - antiwindup_reset_time = 1 - - #parameter that limits the gain of the derivative term at high frequencies, typically ranges from 2 to 20, - maximum_derivative_gain = 5 - - # parameter that gives the proportion of the reference signal that appears in the proportional term. Default to 1 - # Might be reduced depending on feedforward or direct feedthrough used - fraction_of_set_point = 1 - - # Normalized output - min_output = -1 - max_output = 1 - initial_integral_state = 0 - - # Parameters to initialize derivative filter - initial_derivative_state = 0 - previous_feedback = 0 - - - # parallel2standard(Kp, Ki, Kd) - - pid = DiscretePID(; K=proportional_gain, Ti=integral_time, - Td=derivative_time, Tt=antiwindup_reset_time, - N=maximum_derivative_gain, b=1fraction_of_set_point, - umin=min_output, umax=max_output, Ts=sampling_time, I=initial_integral_state, D=initial_derivative_state, yold=previous_feedback) - return pid - end - - # Solution 1: decoupled PIDs - steering_pid = my_pid() - depower_pid = my_pid() - - set_torque_left = steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) - set_torque_right = -steering_pid(steering_order, steering_fdbk) + depower_pid(depower_order, depower_fdbk) - - # Solution 2: individual line controllers (optional alternative) - controller_linelength_pid = my_pid() - set_torque_left_2 = controller_linelength_pid(length_left_setpoint, length_left_feedback) - set_torque_right_2 = controller_linelength_pid(length_right_setpoint, length_right_feedback) - - - """ - winch: - winch_model: "AsyncMachine" # or TorqueControlledMachine - max_force: 4000 # maximal (nominal) tether force; short overload allowed [N] - v_ro_max: 8.0 # maximal reel-out speed [m/s] - v_ro_min: -8.0 # minimal reel-out speed (=max reel-in speed) [m/s] - drum_radius: 0.1615 # radius of the drum [m] - max_acc: 4.0 # maximal acceleration of the winch [m/s²] - gear_ratio: 6.2 # gear ratio of the winch [-] - inertia_total: 0.204 # total inertia, as seen from the motor/generator [kgm²] - f_coulomb: 122.0 # coulomb friction [N] - c_vf: 30.6 # coefficient for the viscous friction [Ns/m] - p_speed: 1.0 # proportional gain of the winch speed controller [-] - i_speed: 0.1 # integral gain of the winch speed controller [-] - """ - - function main() - settings = LineLengthControllerSettings( - max_setpoint_speed=10.0, - max_setpoint_acceleration=50.0, - sampling_time=0.02) - return settings - end - -end \ No newline at end of file + +# Dummy control orders and feedbacks +steering_order = 0.2 +depower_order = 0.1 +length_left_feedback = 0.0 +length_right_feedback = 0.0 +length_power_feedback = 0.0 + +# Compute control torques +torque_L1, torque_R1, torque_L2, torque_R2 = compute_control( + reference_linelength, + steering_order, depower_order, + length_left_feedback, length_right_feedback, length_power_feedback, + differential_settings +) + +println("Torque Left (decoupled): ", torque_L1) +println("Torque Right (decoupled): ", torque_R1) +println("Torque Left (individual): ", torque_L2) +println("Torque Right (individual): ", torque_R2) From dc43d1789ca370527d92306a691d8940e5df8f65 Mon Sep 17 00:00:00 2001 From: Baptiste Labat Date: Fri, 26 Sep 2025 11:26:05 +0200 Subject: [PATCH 7/7] Solve issue "WinchControllers does not declare a compat entry for the following deps" --- Project.toml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Project.toml b/Project.toml index 372f045..d6a9b0b 100644 --- a/Project.toml +++ b/Project.toml @@ -42,6 +42,8 @@ Timers = "0.1.5" WinchModels = "0.3.7" YAML = "0.4.13" julia = "1.10, 1.11" +DiscretePIDs = "0.1.6" +TrajectoryLimiters = "0.1.0" [extras] Aqua = "4c88cf16-eb10-579e-8560-4a9242c79595"