|
| 1 | +# Winch winch controller component, implemented as described in the PhD thesis of Uwe Fechner. |
| 2 | +@enum WinchControllerState wcsLowerForceLimit wcsSpeedControl wcsUpperForceLimit |
| 3 | + |
| 4 | +# Basic winch controller. Works in one of the three modes wcsLowerForceLimit, wcsSpeedControl and |
| 5 | +# wcsUpperForceLimit. |
| 6 | +@with_kw mutable struct WinchController @deftype Float64 |
| 7 | + wcs::WCSettings |
| 8 | + time = 0 |
| 9 | + v_set_pc::Union{Float64, Nothing} = nothing # last set value from the position controller (can be nothing) |
| 10 | + v_set_in = 0.0 # input of the speed controller |
| 11 | + v_set_out = 0.0 # output of the speed controller |
| 12 | + v_set_ufc = 0.0 # output of the upper force controller |
| 13 | + v_set_lfc = 0.0 # output of the lower force controller |
| 14 | + v_set = 0.0 # output of the winchcontroller, going to the motor-controller/ model |
| 15 | + v_act = 0.0 # actual, measured speed |
| 16 | + force = 0.0 # actual, measured force |
| 17 | + calc::CalcVSetIn = CalcVSetIn(wcs) |
| 18 | + mix3::Mixer_3CH = Mixer_3CH(wcs.dt, wcs.t_blend) |
| 19 | + pid1::SpeedController = SpeedController(wcs) |
| 20 | + pid2::LowerForceController = LowerForceController(wcs) |
| 21 | + pid3::UpperForceController = UpperForceController(wcs) |
| 22 | +end |
| 23 | + |
| 24 | +function WinchController(wcs::WCSettings) |
| 25 | + wc = WinchController(wcs=wcs) |
| 26 | + set_f_set(wc.pid2, wcs.f_low) |
| 27 | + set_reset(wc.pid2, true) |
| 28 | + set_v_sw(wc.pid2, -1.0) |
| 29 | + set_f_set(wc.pid3, wcs.f_high) |
| 30 | + set_v_sw(wc.pid3, calc_vro(wcs, wc.pid3.f_set)) |
| 31 | + set_reset(wc.pid3, true) |
| 32 | + set_reset(wc.pid3, false) |
| 33 | + wc |
| 34 | +end |
| 35 | + |
| 36 | +function calc_v_set(wc::WinchController, v_act, force, f_low, v_set_pc=nothing) |
| 37 | + set_f_set(wc.pid2, f_low) |
| 38 | + wc.v_act = v_act |
| 39 | + wc.force = force |
| 40 | + set_vset_pc(wc.calc, v_set_pc, wc.force) |
| 41 | + v_set_in = calc_output(wc.calc) |
| 42 | + # set the inputs of pid1 |
| 43 | + set_v_set_in(wc.pid1, v_set_in) |
| 44 | + set_v_act(wc.pid1, v_act) |
| 45 | + if wc.time <= wc.wcs.t_startup |
| 46 | + reset = true |
| 47 | + else |
| 48 | + reset = false |
| 49 | + end |
| 50 | + # set the inputs of pid2 and pid3 |
| 51 | + set_reset(wc.pid2, reset) |
| 52 | + set_v_sw(wc.pid2, calc_vro(wc.wcs, wc.pid2.f_set) * 1.05) |
| 53 | + set_v_sw(wc.pid3, calc_vro(wc.wcs, wc.pid3.f_set) * 0.95) |
| 54 | + set_v_act(wc.pid2, v_act) |
| 55 | + set_v_act(wc.pid3, v_act) |
| 56 | + # set tracking and force for all controllers |
| 57 | + set_tracking(wc.pid1, wc.v_set) |
| 58 | + set_tracking(wc.pid2, wc.v_set) |
| 59 | + set_tracking(wc.pid3, wc.v_set) |
| 60 | + set_force(wc.pid2, force) |
| 61 | + set_force(wc.pid3, force) |
| 62 | + # activate or deactivate the speed controller |
| 63 | + set_inactive(wc.pid1, (wc.pid2.active || wc.pid3.active) && isnothing(v_set_pc)) |
| 64 | + if ! isnothing(v_set_pc) |
| 65 | + wc.pid2.active=false |
| 66 | + wc.pid3.active=false |
| 67 | + end |
| 68 | + # calculate the output, using the mixer |
| 69 | + select_b(wc.mix3, wc.pid2.active) |
| 70 | + select_c(wc.mix3, wc.pid3.active) |
| 71 | + v_set_out_A = get_v_set_out(wc.pid1) |
| 72 | + v_set_out_B = get_v_set_out(wc.pid2) |
| 73 | + v_set_out_C = get_v_set_out(wc.pid3) |
| 74 | + wc.v_set = calc_output(wc.mix3, v_set_out_A, v_set_out_B, v_set_out_C) |
| 75 | + wc.v_set_out = v_set_out_A # for logging, store the output of the speed controller |
| 76 | + wc.v_set |
| 77 | +end |
| 78 | + |
| 79 | +function on_timer(wc::WinchController) |
| 80 | + wc.time += wc.wcs.dt |
| 81 | + on_timer(wc.calc) |
| 82 | + on_timer(wc.pid1) |
| 83 | + on_timer(wc.pid2) |
| 84 | + on_timer(wc.pid3) |
| 85 | + on_timer(wc.mix3) |
| 86 | +end |
| 87 | + |
| 88 | +function get_state(wc::WinchController) |
| 89 | + get_state(wc.mix3) |
| 90 | +end |
| 91 | + |
| 92 | +function get_set_force(wc::WinchController) |
| 93 | + state = get_state(wc) |
| 94 | + if state == 0 |
| 95 | + return wc.pid2.f_set |
| 96 | + elseif state == 2 |
| 97 | + return wc.pid3.f_set |
| 98 | + else |
| 99 | + return nothing |
| 100 | + end |
| 101 | +end |
| 102 | + |
| 103 | +# for logging and debugging |
| 104 | +function get_status(wc::WinchController) |
| 105 | + f_set = get_set_force(wc) |
| 106 | + if isnothing(f_set) |
| 107 | + f_set = 0.0 |
| 108 | + end |
| 109 | + result = [false, false, 0.0, 0.0, 0.0, 0.0, 0.0] |
| 110 | + result[1] = wc.pid3.reset |
| 111 | + result[2] = wc.pid3.active |
| 112 | + result[3] = wc.pid3.force |
| 113 | + result[4] = f_set |
| 114 | + result[5] = wc.pid1.v_set_out |
| 115 | + result[6] = wc.pid2.v_set_out |
| 116 | + result[7] = wc.pid3.v_set_out |
| 117 | + result |
| 118 | +end |
0 commit comments