From 54f5cf41f79f5f0857c0afeb860951d2274f11fc Mon Sep 17 00:00:00 2001
From: Uwe Fechner <u.fechner-1@tudelft.nl>
Date: Thu, 19 Dec 2024 14:30:28 +0100
Subject: [PATCH] add winch_controller

---
 Project.toml            |  4 +++-
 src/WinchModels.jl      |  1 +
 src/winch_controller.jl | 33 +++++++++++++++++++++++++++++++++
 3 files changed, 37 insertions(+), 1 deletion(-)
 create mode 100644 src/winch_controller.jl

diff --git a/Project.toml b/Project.toml
index 28d4442..971a441 100644
--- a/Project.toml
+++ b/Project.toml
@@ -4,12 +4,14 @@ authors = ["Uwe Fechner <fechner@aenarete.eu> and contributors"]
 version = "0.3.5"
 
 [deps]
+DiscretePIDs = "c1363496-6848-4723-8758-079b737f6baf"
 KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533"
 Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a"
 
 [compat]
+KiteUtils = "0.9.6"
+DiscretePIDs = "0.1.5"
 Parameters = "0.12"
-KiteUtils = "0.7.8, 0.8, 0.9"
 julia = "1.10, 1.11"
 
 [extras]
diff --git a/src/WinchModels.jl b/src/WinchModels.jl
index 0320c7e..9000f3b 100644
--- a/src/WinchModels.jl
+++ b/src/WinchModels.jl
@@ -40,5 +40,6 @@ const AWM = AbstractWinchModel
 
 include("async_generator.jl")
 include("torque_controlled_generator.jl")
+include("winch_controller.jl")
 
 end
\ No newline at end of file
diff --git a/src/winch_controller.jl b/src/winch_controller.jl
new file mode 100644
index 0000000..888581b
--- /dev/null
+++ b/src/winch_controller.jl
@@ -0,0 +1,33 @@
+using DiscretePIDs
+# low level winch controller; this code will be moved to WinchControllers.jl in the future
+
+mutable struct WinchSpeedController
+    kp::Float64
+    ki::Float64
+    pid::DiscretePID
+end
+function WinchSpeedController(;kp=20.0, ki=5.0, dt)
+    pid = DiscretePID(;K=kp, Ti=kp/ki, Ts=dt)
+    WinchSpeedController(kp, ki, pid)
+end
+"""
+    calc_set_torque(set::Settings, wcs::WinchSpeedController, v_set, v_reelout, force)
+
+Calculate the set torque for the winch as defined in settings.yaml file.
+
+# Arguments
+- `set::Settings`: the settings struct
+- `wcs::WinchSpeedController`: the winch speed controller
+- `v_set`: the setpoint of the reelout speed
+- `v_reelout`: the current reelout speed
+- `force`: the current tether force
+"""
+function calc_set_torque(set::Settings, wcs::WinchSpeedController, v_set, v_reelout, force)
+    # calculate the set force
+    set_force = DiscretePIDs.calculate_control!(wcs.pid, v_set, v_reelout)
+    err = set_force - force
+    # calculate the set torque
+    r = set.drum_radius
+    n = set.gear_ratio
+    set_torque = -r/n * (0.0*set_force-err)
+end
\ No newline at end of file