add winch_controller

Project.toml

@@ -4,12 +4,14 @@ authors = ["Uwe Fechner <[email protected]> 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]

src/WinchModels.jl

@@ -40,5 +40,6 @@ const AWM = AbstractWinchModel
 
 include("async_generator.jl")
 include("torque_controlled_generator.jl")
+include("winch_controller.jl")
 
 end

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