|
| 1 | +# Control of a sliding block |
| 2 | +```julia |
| 3 | +using ComponentArrays |
| 4 | +using DifferentialEquations |
| 5 | +using Interact: @manipulate |
| 6 | +using Parameters: @unpack |
| 7 | +using Plots |
| 8 | +``` |
| 9 | +## Problem Setup |
| 10 | +```julia |
| 11 | +const g = 9.80665 |
| 12 | + |
| 13 | +maybe_apply(f::Function, x, p, t) = f(x, p, t) |
| 14 | +maybe_apply(f, x, p, t) = f |
| 15 | + |
| 16 | +# Applies functions of form f(x,p,t) to be applied and passed in as inputs |
| 17 | +function simulator(func; kwargs...) |
| 18 | + simfun(dx, x, p, t) = func(dx, x, p, t; map(f->maybe_apply(f, x, p, t), (;kwargs...))...) |
| 19 | + simfun(x, p, t) = func(x, p, t; map(f->maybe_apply(f, x, p, t), (;kwargs...))...) |
| 20 | + return simfun |
| 21 | +end |
| 22 | + |
| 23 | +softsign(x) = tanh(1e3x) |
| 24 | +``` |
| 25 | +## Component Functions |
| 26 | +### A sliding block with two different friction models |
| 27 | +```julia |
| 28 | +# Sliding block with viscous friction |
| 29 | +function viscous_block!(D, vars, p, t; u=0.0) |
| 30 | + @unpack m, c, k = p |
| 31 | + @unpack v, x = vars |
| 32 | + |
| 33 | + D.x = v |
| 34 | + D.v = (-c*v + k*(u-x))/m |
| 35 | + return x |
| 36 | +end |
| 37 | + |
| 38 | +# Sliding block with coulomb friction |
| 39 | +function coulomb_block!(D, vars, p, t; u=0.0) |
| 40 | + @unpack m, μ, k = p |
| 41 | + @unpack v, x = vars |
| 42 | + |
| 43 | + D.x = v |
| 44 | + a = -μ*g*softsign(v) + k*(u-x)/m |
| 45 | + D.v = abs(a)<1e-3 && abs(v)<1e-3 ? -10v : a #deadzone to help the simulation |
| 46 | + return x |
| 47 | +end |
| 48 | +``` |
| 49 | +### PID feedback control |
| 50 | +```julia |
| 51 | +function PID_controller!(D, vars, p, t; err=0.0, v=0.0) |
| 52 | + @unpack kp, ki, kd = p |
| 53 | + @unpack x = vars |
| 54 | + |
| 55 | + D.x = ki*err |
| 56 | + return x + kp*err + kd*v |
| 57 | +end |
| 58 | + |
| 59 | +function feedback_sys!(D, components, p, t; ref=0.0) |
| 60 | + @unpack ctrl, plant = components |
| 61 | + |
| 62 | + u = p.ctrl.fun(D.ctrl, ctrl, p.ctrl.params, t; err=ref-plant.x, v=-plant.v) |
| 63 | + return p.plant.fun(D.plant, plant, p.plant.params, t; u=u) |
| 64 | +end |
| 65 | + |
| 66 | +step_input(;time=1.0, mag=1.0) = (x,p,t) -> t>time ? mag : 0 |
| 67 | +sine_input(;mag=1.0, period=10.0) = (x,p,t) -> mag*sin(t*2π/period) |
| 68 | + |
| 69 | +# Equivalent viscous damping coefficient taken from: |
| 70 | +# https://engineering.purdue.edu/~deadams/ME563/lecture2010.pdf |
| 71 | +visc_equiv(μ, N, ω, mag) = 4*μ*N/(π*ω*mag) |
| 72 | +``` |
| 73 | +## Open-Loop Response |
| 74 | +To see the open-loop response of the coulomb system, let's set the input to ```5``` and plot |
| 75 | +the results. |
| 76 | +```julia |
| 77 | +const tspan = (0.0, 30.0) |
| 78 | +const m = 50.0 |
| 79 | +const μ = 0.1 |
| 80 | +const k = 50.0 |
| 81 | + |
| 82 | +p = (m=m, μ=μ, k=k) |
| 83 | +ic = ComponentArray(v=0, x=0) |
| 84 | + |
| 85 | +ODEProblem(simulator(coulomb_block!, u=5), ic, tspan, p) |> solve |> plot |
| 86 | +``` |
| 87 | + |
| 88 | + |
| 89 | +## Closed-Loop Response |
| 90 | +For the closed-loop response, let's make an interactive GUI. Since we are using |
| 91 | +```ComponentArray```s, we don't have to change anything about our plant model to incorporate |
| 92 | +it in the overall system simulation. |
| 93 | +```julia |
| 94 | +p = ( |
| 95 | + ctrl = ( |
| 96 | + params = (kp=13, ki=12, kd=5), |
| 97 | + fun = PID_controller!, |
| 98 | + ), |
| 99 | + plant = ( |
| 100 | + params = plant_p, |
| 101 | + fun = coulomb_block!, |
| 102 | + ), |
| 103 | +) |
| 104 | + |
| 105 | +ic = ComponentArray(ctrl=(;x=0), plant=plant_ic) |
| 106 | + |
| 107 | +sol = ODEProblem(simulator(feedback_sys!, ref=10), ic, tspan, p) |> solve |
| 108 | +plot(sol, vars=3) |
| 109 | +``` |
| 110 | + |
| 111 | +```julia |
| 112 | +## Interactive GUI for switching out plant models and varying PID gains |
| 113 | +@manipulate for kp in 0:0.01:15, |
| 114 | + ki in 0:0.01:15, |
| 115 | + kd in 0:0.01:15, |
| 116 | + damping in Dict( |
| 117 | + "Coulomb" => coulomb_block!, |
| 118 | + "Viscous" => viscous_block!, |
| 119 | + ), |
| 120 | + reference in Dict( |
| 121 | + "Sine" => sine_input, |
| 122 | + "Step" => step_input, |
| 123 | + ), |
| 124 | + magnitude in 0:0.01:10, # pop-pop! |
| 125 | + period in 1:0.01:30, |
| 126 | + plot_v in false |
| 127 | + |
| 128 | + # Inputs |
| 129 | + tspan = (0.0, 30.0) |
| 130 | + |
| 131 | + ctrl_fun = PID_controller! |
| 132 | + # plant_fun = coulomb_block! |
| 133 | + |
| 134 | + ref = if reference==sine_input |
| 135 | + reference(period=period, mag=magnitude) |
| 136 | + else |
| 137 | + reference(mag=magnitude) |
| 138 | + end |
| 139 | + |
| 140 | + m = 50.0 |
| 141 | + μ = 0.1 |
| 142 | + ω = 2π/period |
| 143 | + c = 4*μ*m*g/(π*ω*magnitude) # Viscous equivalent damping |
| 144 | + k = 50.0 |
| 145 | + |
| 146 | + plant_p = (m=m, μ=μ, c=c, k=k) # We'll just put everything for both models in here |
| 147 | + ctrl_p = (kp=kp, ki=ki, kd=kd) |
| 148 | + |
| 149 | + plant_ic = (v=0, x=0) |
| 150 | + ctrl_ic = (;x=0) |
| 151 | + |
| 152 | + |
| 153 | + |
| 154 | + # Set up and solve |
| 155 | + sys_p = ( |
| 156 | + ctrl = ( |
| 157 | + params = ctrl_p, |
| 158 | + fun = ctrl_fun, |
| 159 | + ), |
| 160 | + plant = ( |
| 161 | + params = plant_p, |
| 162 | + fun = damping, |
| 163 | + ), |
| 164 | + ) |
| 165 | + sys_ic = ComponentArray(ctrl=ctrl_ic, plant=plant_ic) |
| 166 | + sys_fun = ODEFunction(simulator(feedback_sys!, ref=ref), syms=[:u, :v, :x]) |
| 167 | + sys_prob = ODEProblem(sys_fun, sys_ic, tspan, sys_p) |
| 168 | + |
| 169 | + sol = solve(sys_prob, Tsit5()) |
| 170 | + |
| 171 | + |
| 172 | + # Plot |
| 173 | + t = tspan[1]:0.1:tspan[2] |
| 174 | + lims = magnitude*[-1, 1] |
| 175 | + plotvars = plot_v ? [3, 2] : [3] |
| 176 | + strip = plot(t, ref.(0, 0, t), ylim=1.2lims, label="r(t)") |
| 177 | + plot!(strip, sol, vars=plotvars) |
| 178 | + phase = plot(ref.(0, 0, t), map(x->x.plant.x, sol(t).u), |
| 179 | + xlim=lims, |
| 180 | + ylim=1.2lims, |
| 181 | + legend=false, |
| 182 | + xlabel="r(t)", |
| 183 | + ylabel="x(t)", |
| 184 | + ) |
| 185 | + plot(strip, phase, layout=(2, 1), size=(700, 800)) |
| 186 | + |
| 187 | +end |
| 188 | +``` |
| 189 | + |
0 commit comments