Flutter rig code

cbc.jl

function predict(prevtarget::Vector{T}, s = 1.0) where T <: AbstractArray
    secant = prevtarget[end] - prevtarget[end-1]
    (prevtarget[end] + s*secant, secant)
end

function correct!(expt, target::AbstractVector)
    correct!(expt, target, zeros(target))
end

function correct!(expt, target::T, tangent::T; print=false) where T <: AbstractVector
    h = 1e-5
    itr = 1
    Δ = zeros(target)
    target0 = copy(target)
    target2 = copy(target)
    while true
        settarget!(expt, target)
        achieved = getachieved!(expt) # runs the experiment and returns the result
        pseudoarclength = dot(target0 - target, tangent)
        res = [target[1:end-1] - achieved[1:end-1]; pseudoarclength]
        res[1] *= 50
        if (norm(res) < 1e-7) && (norm(Δ) < 1e-7)
            return true
        elseif itr >= 20
            return false
        end
        if print
            println("[correct!] Itr $itr: f = $(norm(res)), Δf = $(norm(Δ))")
        end
        # Get derivative wrt target[1] using a finite difference
        target2 .= target
        target2[1] += h
        settarget!(expt, target2)
        achieved2 = getachieved!(expt)
        pseudoarclength = dot(target0 - target2, tangent)
        res2 = [target2[1:end-1] - achieved2[1:end-1]; pseudoarclength]
        res2[1] *= 50
        # Construct the Jacobian matrix
        J = eye(length(res))
        J[:, 1] .= (res2 - res)/h
        # Newton iteration (with implicit Picard iteration since it's not a full Jacobian)
        Δ = J \ res
        target .= target .- Δ
        itr += 1
    end
end

function continuation!(expt::T, step=-0.1) where T
    prevexpt = Vector{T}()
    prevtarget = Vector{Vector{Float64}}()
    target = Vector(gettarget(expt))
    if ! correct!(expt, target)
        println("Correction failed")
        return (prevexpt, prevtarget)
    end
    push!(prevexpt, deepcopy(expt))  # save the corrected point
    push!(prevtarget, copy(target))
    println("It -1: $target")
    target[end] += step
    correct!(expt, target)
    if ! correct!(expt, target)
        println("Correction failed")
        return (prevexpt, prevtarget)
    end
    push!(prevexpt, deepcopy(expt))  # save the corrected point
    push!(prevtarget, copy(target))
    println("It 0: $target")
    for i = 1:100
        (target, secant) = predict(prevtarget)
        if ! correct!(expt, target, secant, print=true)
            println("Correction failed")
            return (prevexpt, prevtarget)
        end
        push!(prevexpt, deepcopy(expt))  # save the corrected point
        push!(prevtarget, copy(target))
        println("It $i: $target")
    end
    (prevexpt, prevtarget)
end

qs2dof.jl

using StaticArrays
using OrdinaryDiffEq
using QuadGK

#--- Parameters

mutable struct qs2dofParameters{T}
    m_t::Float64
    m_w::Float64
    s::Float64
    b::Float64
    a::Float64
    x_a::Float64
    r_θ::Float64
    I_θ::Float64
    ξ_h::Float64
    ξ_θ::Float64
    ω_h::Float64
    ω_θ::Float64
    d_h::Float64
    d_θ::Float64
    k_h::Float64
    k_θ::Float64
    ρ::Float64
    k_θ₁::Float64
    k_θ₂::Float64
    k_h₁::Float64
    k_h₂::Float64
    U::Float64
    control::T
end

function qs2dofParameters(control)
    # Default parameters
    m_t = 90.0 # 3.836 # total mass of the system. increasing the frequency is less
    m_w = 25.0 #1.0662 # mass of the wing
    s = 0.62 #0.6 # wing span
    b = 0.15 #0.0325 # half the length of the wing/semichord - decrease h is lco as well
    a = -0.5 # position of elastic axis relative to the semichord
    x_a = 0.02/b #0.5 # nondimensional distance between centre of gravity and elastic axis - a/b
    r_θ = 0.3 # ?
    I_θ = m_t*(b^2)*(r_θ^2) #0.0004438 # moment of inertia m_t*(b^2)*(r_θ^2)
    ξ_h = 0.04 #0.0735 # damping factor for heave
    ξ_θ = 0.15 #0.3#0.3 # damping factor for pitch

    ω_h = 14.0 #20 # natural freq for heave
    ω_θ = 10.0 # natural freq for pitch
    d_h = ξ_h*2*ω_h*m_w # 0.011 # damping coefficient for heave? I use the damping factor
    d_θ = ξ_θ*2*ω_θ*I_θ #0.0115 # damping coefficient for pitch? I use the damping factor
    k_h = ω_h^2*m_t #895.1 # heave stiffness - change freq
    k_θ = ω_θ^2*I_θ #0.942 # pitch stiffness
    ρ = 1.204 # air density

    k_θ₁ = 1.0 #3*3.95 # nonlinear pitch stiffness (quadratic)
    k_θ₂ = 1000.0 #307 # nonlinear pitch stiffness (cubic)
    k_h₁ = 1.0 # nonlinear heave stiffness (quadratic)
    k_h₂ = 1000.0 # nonlinear heave stiffness (cubic)

    U = 15.0

    return qs2dofParameters(m_t, m_w, s, b, a, x_a, r_θ, I_θ, ξ_h, ξ_θ, ω_h, ω_θ,
        d_h, d_θ, k_h, k_θ, ρ, k_θ₁, k_θ₂, k_h₁, k_h₂, U, control)
end

Base.getindex(p::qs2dofParameters, i) = p.U
Base.setindex!(p::qs2dofParameters, val, i) = (p.U = val)

#--- Equations of motion

function qs2dofRHS(x, p::qs2dofParameters{T}, t) where T
    # State variables
    h = x[1]
    dh = x[2]
    θ = x[3]
    dθ = x[4]

    # Structural mass matrix
    Ms₁₁ = p.m_t
    Ms₁₂ = p.m_w*p.x_a*p.b
    Ms₂₁ = p.m_w*p.x_a*p.b
    Ms₂₂ = p.I_θ

    # Aerodynamic mass matrix
    Ma₁₁ = -π*p.ρ*(p.b^2)
    Ma₁₂ = π*p.ρ*(p.b^3)*p.a
    Ma₂₁ = π*p.ρ*(p.b^3)*p.a
    Ma₂₂ = -π*p.ρ*(p.b^4)*(0.125+p.a^2)

    # Resulting mass matrix
    m₁ = Ms₁₁ - Ma₁₁
    m₂ = Ms₁₂ - Ma₁₂
    m₃ = Ms₂₁ - Ma₂₁
    m₄ = Ms₂₂ - Ma₂₂

    Q = dh + p.b*(0.5-p.a)*dθ + p.U*θ

    # Structural Stiffness
    Fs_θ = p.k_θ*θ + p.k_θ₁*(θ^2) + p.k_θ₂*(θ^3)
    Fs_h = p.k_h*h + p.k_h₁*(h^2) + p.k_h₂*(h^3)

    # Control input
    if p.control != nothing
        u = p.control(x)
    else
        u = 0.0
    end

    # RHS
    RHS1 = -2*π*p.ρ*p.b*p.U*Q - π*p.ρ*(p.b^2)*p.U*dθ - p.d_h*dh - Fs_h + u
    RHS2 = 2*π*p.ρ*(p.b^2)*p.U*(p.a+0.5)*Q - p.d_θ*dθ - Fs_θ - π*p.ρ*(p.b^3)*(0.5-p.a)*p.U*dθ

    # Accelerations
    ddh = (m₄*RHS1 - m₂*RHS2)/(m₄*m₁ - m₂*m₃)
    ddθ = (m₃*RHS1 - m₁*RHS2)/(m₂*m₃ - m₄*m₁)

    # State Derivatives
    SVector(dh, ddh, dθ, ddθ)
end

#--- Controllers

# Proportional control: use a structure to store the necessary info
mutable struct ProportionalControl
    Kp::Float64
    T::Float64
    A₀::Float64
    A₁::Float64
end

function (control::ProportionalControl)(x)
    h = x[1]
    dh = x[2]
    B₁ = -2π/control.T*control.A₁
    ϕ = atan2(dh/B₁, (h - control.A₀)/control.A₁)
    control.Kp*(control.A₀ + control.A₁*cos(ϕ) - h)
end

proportionalcontrol() = ProportionalControl(0.0, 1.0, 0.0, 1.0)

gettarget(control::ProportionalControl) = SVector(control.A₁, control.A₀, control.T)
settarget!(control::ProportionalControl, val) = (control.A₁ = val[1]; control.A₀ = val[2]; control.T = val[3])

function getachieved(control::ProportionalControl, sol, te)
    t₀ = te[end-1]
    t₁ = te[end]
    T = t₁ - t₀
    c = quadgk(t -> SVector(1.0, 2*cos(2π*t), 2*sin(2π*t))*sol(t₀ + t*T)[1], 0.0, 1.0)[1]
    SVector{3, Float64}(sqrt(c[2]^2 + c[3]^2), c[1], T)
end

#--- Experiment

struct qs2dofExperiment{T}
    p::qs2dofParameters{T}
    x₀::MVector{4, Float64}
end

function qs2dofExperiment(; x₀=MVector(0.1, 0.0, 0.0, 0.0), p=nothing, control=proportionalcontrol())
    if p == nothing
        p = qs2dofParameters(control)
    end
    qs2dofExperiment(p, x₀)
end

function simulate!(expt::qs2dofExperiment, t::Float64, x₀::AbstractVector)
    prob = ODEProblem(qs2dofRHS, SVector{4, Float64}(x₀), (0.0, t), expt.p)
    solve(prob, Vern6(), dtmax=0.02) # Vern6 for reasonably high accuracy, max stepsize 0.02
end

function simulate!(expt::qs2dofExperiment, t::Float64)
    sol = simulate!(expt, t, expt.x₀)
    expt.x₀ .= sol[:, end]
    sol
end

function simulate!(expt::qs2dofExperiment, te::Vector{Float64}, t::Float64, x₀::AbstractVector)
    prob = ODEProblem(qs2dofRHS, SVector{4, Float64}(x₀), (0.0, t), expt.p)
    solve(prob, Vern6(), dtmax=0.02) # Vern6 for reasonably high accuracy, max stepsize 0.02
    cb = ContinuousCallback((u, t, integrator) -> u[2],  # event function; fires when u[2] is zero (Poincare map)
        integrator -> push!(te, integrator.t),  # callback when event function goes positive; store the event time
        nothing)  # callback when event function goes negative
    sol = solve(prob, Vern6(), callback=cb, dtmax=0.02) # Vern6 for reasonably high accuracy, max stepsize 0.02
end

function simulate!(expt::qs2dofExperiment, te::Vector{Float64}, t::Float64)
    sol = simulate!(expt, te, t, expt.x₀)
    expt.x₀ .= sol[:, end]
    sol
end

gettarget(expt::qs2dofExperiment) = [gettarget(expt.p.control); SVector(expt.p.U)]
settarget!(expt::qs2dofExperiment, val) = (settarget!(expt.p.control, val); expt.p.U = val[end]; nothing)

function getachieved!(expt::qs2dofExperiment)
    te = Vector{Float64}()
    sol = simulate!(expt, te, 20.0)
    [getachieved(expt.p.control, sol, te); SVector(expt.p.U)]
end

scratch.jl

include("MATLABPlots.jl")
include("qs2dof.jl")
include("cbc.jl")

expt = qs2dofExperiment()
target = Vector(getachieved!(expt))
settarget!(expt, target)
expt.p.control.Kp = 10000

expt2 = qs2dofExperiment()
settarget!(expt2, target)

# TODO: Code up a shooting method to find the ground truth for the bifurcation
# diagram. Is the control failing at the bifurcation point because it is rubbish
# or because we're miles away from the solution?


U = 15:-0.1:1
A₁ = Float64[]
for u in U
    target[4] = u
    if ! correct!(expt, target, zeros(target), print=true)
        break
    end
    push!(A₁, gettarget(expt)[1])
end
plot(U[1:length(A₁)], A₁, ".")

A₁2 = Float64[]
for u in U
    target[4] = u
    settarget!(expt2, target)
    push!(A₁2, getachieved!(expt2)[1])
end
plot!(U[1:length(A₁2)], A₁2, "r.")




(expts, targets) = continuation!(expt)
U = [target[4] for target in targets]
A₁ = [target[1] for target in targets]
plot(U, A₁, ".")

(sol, te) = expt()
plot(sol.t, sol[1,:])