Control of a sliding block

using ComponentArrays
using DifferentialEquations
using Interact: @manipulate
using Parameters: @unpack
using Plots

Problem Setup

const g = 9.80665

maybe_apply(f::Function, x, p, t) = f(x, p, t)
maybe_apply(f, x, p, t) = f

# Applies functions of form f(x,p,t) to be applied and passed in as inputs
function simulator(func; kwargs...)
    simfun(dx, x, p, t) = func(
        dx, x, p, t; map(f->maybe_apply(f, x, p, t), (; kwargs...))...)
    simfun(x, p, t) = func(x, p, t; map(f->maybe_apply(f, x, p, t), (; kwargs...))...)
    return simfun
end

softsign(x) = tanh(1e3x)

Component Functions

A sliding block with two different friction models

# Sliding block with viscous friction
function viscous_block!(D, vars, p, t; u = 0.0)
    @unpack m, c, k = p
    @unpack v, x = vars

    D.x = v
    D.v = (-c*v + k*(u-x))/m
    return x
end

# Sliding block with coulomb friction
function coulomb_block!(D, vars, p, t; u = 0.0)
    @unpack m, μ, k = p
    @unpack v, x = vars

    D.x = v
    a = -μ*g*softsign(v) + k*(u-x)/m
    D.v = abs(a)<1e-3 && abs(v)<1e-3 ? -10v : a #deadzone to help the simulation
    return x
end

PID feedback control

function PID_controller!(D, vars, p, t; err = 0.0, v = 0.0)
    @unpack kp, ki, kd = p
    @unpack x = vars

    D.x = ki*err
    return x + kp*err + kd*v
end

function feedback_sys!(D, components, p, t; ref = 0.0)
    @unpack ctrl, plant = components

    u = p.ctrl.fun(D.ctrl, ctrl, p.ctrl.params, t; err = ref-plant.x, v = -plant.v)
    return p.plant.fun(D.plant, plant, p.plant.params, t; u = u)
end

step_input(; time = 1.0, mag = 1.0) = (x, p, t) -> t>time ? mag : 0
sine_input(; mag = 1.0, period = 10.0) = (x, p, t) -> mag*sin(t*2π/period)

# Equivalent viscous damping coefficient taken from:
# https://engineering.purdue.edu/~deadams/ME563/lecture2010.pdf
visc_equiv(μ, N, ω, mag) = 4*μ*N/(π*ω*mag)

Open-Loop Response

To see the open-loop response of the coulomb system, let's set the input to 5 and plot the results.

const tspan = (0.0, 30.0)
const m = 50.0
const μ = 0.1
const k = 50.0

p = (m = m, μ = μ, k = k)
ic = ComponentArray(v = 0, x = 0)

ODEProblem(simulator(coulomb_block!, u = 5), ic, tspan, p) |> solve |> plot

Closed-Loop Response

For the closed-loop response, let's make an interactive GUI. Since we are using ComponentArrays, we don't have to change anything about our plant model to incorporate it in the overall system simulation.

p = (
    ctrl = (
        params = (kp = 13, ki = 12, kd = 5),
        fun = PID_controller!
    ),
    plant = (
        params = plant_p,
        fun = coulomb_block!
    )
)

ic = ComponentArray(ctrl = (; x = 0), plant = plant_ic)

sol = ODEProblem(simulator(feedback_sys!, ref = 10), ic, tspan, p) |> solve
plot(sol, vars = 3)

## Interactive GUI for switching out plant models and varying PID gains
@manipulate for kp in 0:0.01:15,
    ki in 0:0.01:15,
    kd in 0:0.01:15,
    damping in Dict(
        "Coulomb" => coulomb_block!,
        "Viscous" => viscous_block!
    ),
    reference in Dict(
        "Sine" => sine_input,
        "Step" => step_input
    ),
    magnitude in 0:0.01:10, # pop-pop!
    period in 1:0.01:30,
    plot_v in false

    # Inputs
    tspan = (0.0, 30.0)

    ctrl_fun = PID_controller!
    # plant_fun = coulomb_block!

    ref = if reference==sine_input
        reference(period = period, mag = magnitude)
    else
        reference(mag = magnitude)
    end

    m = 50.0
    μ = 0.1
    ω = 2π/period
    c = 4*μ*m*g/(π*ω*magnitude) # Viscous equivalent damping
    k = 50.0

    plant_p = (m = m, μ = μ, c = c, k = k) # We'll just put everything for both models in here
    ctrl_p = (kp = kp, ki = ki, kd = kd)

    plant_ic = (v = 0, x = 0)
    ctrl_ic = (; x = 0)

    # Set up and solve
    sys_p = (
        ctrl = (
            params = ctrl_p,
            fun = ctrl_fun
        ),
        plant = (
            params = plant_p,
            fun = damping
        )
    )
    sys_ic = ComponentArray(ctrl = ctrl_ic, plant = plant_ic)
    sys_fun = ODEFunction(simulator(feedback_sys!, ref = ref), syms = [:u, :v, :x])
    sys_prob = ODEProblem(sys_fun, sys_ic, tspan, sys_p)

    sol = solve(sys_prob, Tsit5())

    # Plot
    t = tspan[1]:0.1:tspan[2]
    lims = magnitude*[-1, 1]
    plotvars = plot_v ? [3, 2] : [3]
    strip = plot(t, ref.(0, 0, t), ylim = 1.2lims, label = "r(t)")
    plot!(strip, sol, vars = plotvars)
    phase = plot(ref.(0, 0, t), map(x->x.plant.x, sol(t).u),
        xlim = lims,
        ylim = 1.2lims,
        legend = false,
        xlabel = "r(t)",
        ylabel = "x(t)"
    )
    plot(strip, phase, layout = (2, 1), size = (700, 800))
end