Add mwe_03.jl

Author: ufechner7

Project.toml

@@ -6,6 +6,7 @@ version = "0.5.3"
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
+FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"
 KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56"
@@ -24,6 +25,7 @@ ControlPlots = "0.2.7"
 ControlSystemsBase = "1.15.1"
 DocStringExtensions = "0.9.4"
 Documenter = "1.11.4"
+FiniteDiff = "2.27.0"
 KiteUtils = "0.10.7"
 LinearAlgebra = "1.10, 1.11"
 NLsolve = "4.5.1"

mwes/mwe_02.jl

@@ -66,7 +66,7 @@ function finite_jacobian(f, x; ϵ=sqrt(eps(eltype(x))))
     J
 end
 
-A = finite_jacobian(x -> motor_dynamics(x, u0), x0)
+@time A = finite_jacobian(x -> motor_dynamics(x, u0), x0)
 B = finite_jacobian(u -> motor_dynamics(x0, u), u0)
 C = [1.0]
 D = [0.0 0.0]

mwes/mwe_03.jl

@@ -0,0 +1,70 @@
+# Linearize the winch
+# input: set_speed
+# output: speed
+
+using Pkg
+if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+    using Test
+end
+using WinchControllers, WinchModels, KiteUtils, ControlPlots, ControlSystemsBase, FiniteDiff
+import FiniteDiff: finite_difference_jacobian
+
+if isfile("data/system_tuned.yaml")
+    set = load_settings("system_tuned.yaml")
+else
+    set = load_settings("system.yaml")
+end
+wcs = WCSettings(dt=0.02)
+update(wcs)
+wcs.test = true
+
+winch = Winch(wcs, set)
+
+# find equilibrium speed
+function find_equilibrium_speed(winch, set_speed, force, n=2000)
+    last_v_act = 0.0
+    for v_set in range(0.0, 2*set_speed, n)
+        lim_speed = minimum([v_set, set_speed])
+        set_force(winch, force)
+        set_v_set(winch, lim_speed)
+        v_act = get_speed(winch)
+        on_timer(winch)
+        if v_set > 0 && abs(v_act - last_v_act) < 1e-6
+            return v_act
+        end
+        last_v_act = v_act
+    end
+    set_v_set(winch, set_speed)
+    on_timer(winch)
+end
+
+function motor_dynamics(x, u)
+    # x: state vector, e.g., [v_act]
+    # u: input vector, e.g., [v_set, force]
+    v_act = x[1]
+    v_set, force = u[1], u[2]
+    acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set)
+    return [acc]
+end
+
+v_set = 8.0
+force = 4000.0
+v_act = find_equilibrium_speed(winch, v_set, force)
+
+x0 = [v_act]         # State at operating point
+u0 = [v_set, force]  # Input at operating point
+
+# System matrix A = ∂f/∂x at operating point
+@time A = finite_difference_jacobian(x -> motor_dynamics(x, u0), x0)
+
+# Input matrix B = ∂f/∂u at operating point
+B = finite_difference_jacobian(u -> motor_dynamics(x0, u), u0)
+
+C = [1.0]
+D = [0.0 0.0]
+
+sys = ss(A, B, C, D)
+sys_new = ss(A, B[:, 1], C, D[:, 1])
+
+bode_plot(sys_new; to=2, title="Linearized Winch, F=$force N")