|
| 1 | +# Linearize the closed loop system consisting of the winch, kite and upper force controller. |
| 2 | + |
| 3 | +using Pkg |
| 4 | +if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) |
| 5 | + using TestEnv; TestEnv.activate() |
| 6 | + using Test |
| 7 | +end |
| 8 | +using WinchControllers, WinchModels, KiteUtils, ControlPlots, ControlSystemsBase, FiniteDiff, RobustAndOptimalControl |
| 9 | +import FiniteDiff: finite_difference_jacobian |
| 10 | + |
| 11 | +if isfile("data/system_tuned.yaml") |
| 12 | + set = load_settings("system_tuned.yaml") |
| 13 | +else |
| 14 | + set = load_settings("system.yaml") |
| 15 | +end |
| 16 | +wcs = WCSettings(dt=0.02) |
| 17 | +update(wcs) |
| 18 | +wcs.test = true |
| 19 | + |
| 20 | +winch = Winch(wcs, set) |
| 21 | + |
| 22 | +@info "Testing stability of the upper force controller (UFC) with the winch and kite system." |
| 23 | + |
| 24 | +# find equilibrium speed |
| 25 | +function find_equilibrium_speed(winch, set_speed, force, n=10000) |
| 26 | + last_v_act = 0.0 |
| 27 | + for v_set in range(0.0, 2*set_speed, n) |
| 28 | + lim_speed = minimum([v_set, set_speed]) |
| 29 | + set_force(winch, force) |
| 30 | + set_v_set(winch, lim_speed) |
| 31 | + v_act = get_speed(winch) |
| 32 | + on_timer(winch) |
| 33 | + if v_set > 0 && abs(v_act - last_v_act) < 1e-6 |
| 34 | + return v_act |
| 35 | + end |
| 36 | + last_v_act = v_act |
| 37 | + end |
| 38 | + set_v_set(winch, set_speed) |
| 39 | + on_timer(winch) |
| 40 | + @error "Failed to find equilibrium speed" |
| 41 | +end |
| 42 | + |
| 43 | +function motor_dynamics(x, u) |
| 44 | + # x: state vector, e.g., [v_act] |
| 45 | + # u: input vector, e.g., [v_set, force] |
| 46 | + v_act = x[1] |
| 47 | + v_set, force = u[1], u[2] |
| 48 | + acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set) |
| 49 | + return [acc] |
| 50 | +end |
| 51 | + |
| 52 | +function calc_force(v_wind, v_ro) |
| 53 | + (v_wind - v_ro)^2 * 4000.0 / 16.0 |
| 54 | +end |
| 55 | + |
| 56 | +# create the upper force controller |
| 57 | +# Tf can typically be chosen as Ti/NT for a PI controller and Td/N for a PID controller, and N is commonly in the range 2 to 20. |
| 58 | +# Td = wcs.df_high / wcs.pf_high Kd/Kp |
| 59 | +function upper_force_controller(wcs) |
| 60 | + # wcs: WCSettings object |
| 61 | + Td = wcs.df_high / wcs.pf_high |
| 62 | + N = wcs.nf_high |
| 63 | + Tf = Td / N |
| 64 | + C = pid(wcs.pf_high, wcs.if_high, wcs.df_high; form=:parallel, filter_order=1, state_space=true, Tf) |
| 65 | + return C |
| 66 | +end |
| 67 | + |
| 68 | +function system_dynamics(x, u) |
| 69 | + # x: state vector, e.g., [v_act] |
| 70 | + # u: input vector, e.g., [v_set, v_wind] |
| 71 | + v_act = x[1] |
| 72 | + v_set, v_wind = u[1], u[2] |
| 73 | + force = calc_force(v_wind, v_act) |
| 74 | + acc = calc_acceleration(winch.wm, v_act, force; set_speed = v_set) |
| 75 | + return [acc] |
| 76 | +end |
| 77 | + |
| 78 | +function linearize(winch, v_set, v_wind) |
| 79 | + force = calc_force(v_wind, v_set) |
| 80 | + v_act = find_equilibrium_speed(winch, v_set, force) |
| 81 | + x0 = [v_act] # State at operating point |
| 82 | + u0 = [v_set, v_wind] # Input at operating point |
| 83 | + A = finite_difference_jacobian(x -> system_dynamics(x, u0), x0) |
| 84 | + B = finite_difference_jacobian(u -> system_dynamics(x0, u), u0) |
| 85 | + C = [1.0] |
| 86 | + D = [0.0 0.0] |
| 87 | + force = calc_force(v_wind, v_act) |
| 88 | + siso_sys = ss(A, B[:, 1], C, D[:, 1]) * force/v_act |
| 89 | +end |
| 90 | + |
| 91 | +function open_loop_system(winch, v_set, v_wind) |
| 92 | + # Create the open loop system with the upper force controller |
| 93 | + C = upper_force_controller(winch.wcs) |
| 94 | + sys = linearize(winch, v_set, v_wind) |
| 95 | + # sys_open = feedback(C * sys, 1.0; sign=-1) |
| 96 | + return C * sys |
| 97 | +end |
| 98 | + |
| 99 | +function margins() |
| 100 | + margins = [] |
| 101 | + for v_wind in range(1, 9, length=9) |
| 102 | + local v_set, dm |
| 103 | + v_set = 0.57*v_wind |
| 104 | + sys = open_loop_system(winch, v_set, v_wind) |
| 105 | + dm = diskmargin(sys) |
| 106 | + push!(margins, dm.margin) |
| 107 | + end |
| 108 | + min_margin = minimum(margins) |
| 109 | + if min_margin < 0.3 |
| 110 | + @error "System is unstable with a minimum margin of: $min_margin" |
| 111 | + elseif min_margin < 0.5 |
| 112 | + @warn "System is marginally stable with a minimum margin of: $min_margin" |
| 113 | + else |
| 114 | + @info "System is stable with a minimum margin of: $(round(min_margin, digits=2))" |
| 115 | + end |
| 116 | + return margins |
| 117 | +end |
| 118 | +margins() |
| 119 | + |
| 120 | +for v_wind in range(7.5, 9, length=3) |
| 121 | + global sys |
| 122 | + local v_set |
| 123 | + v_set = 0.57*v_wind |
| 124 | + sys = open_loop_system(winch, v_set, v_wind) |
| 125 | + bode_plot(sys; from=0.76, to=2.85, title="System with UFC, v_wind=7.5..9 m/s") |
| 126 | +end |
| 127 | +nothing |
0 commit comments