rbsim68new.jl

using OrdinaryDiffEq
using DiffEqCallbacks

lc1 = -0.5
l1 = -1.
m1 = 1.
I1 = 0.333
lc2 = -1.
l2 = -2.
m2 = 1.
I2 = 1.33
g = -9.81

tau = zeros(2)
last_control_time = Ref(NaN)
next_control_time = Ref(NaN)
controltimes = Float64[]

function dynamics(u, p, t)
    @show t
    
    if (t == next_control_time[] && t != last_control_time[]) || isnan(last_control_time[])
        println("control")
        tau[1] = sin(t)
        tau[2] = cos(t)
        push!(controltimes, t)
        last_control_time[] = t
    end
    
    # double pendulum    
    q = u[1 : 2]
    v = u[3 : 4]
    
    c1 = cos(q[1])
    c2 = cos(q[2])
    s1 = sin(q[1])
    s2 = sin(q[2])
    s12 = sin(q[1] + q[2])
    
    M11 = I1 + I2 + m2 * l1^2 + 2 * m2 * l1 * lc2 * c2
    M12 = I2 + m2 * l1 * lc2 * c2
    M22 = I2
    M = [M11 M12; M12 M22]

    C11 = -2 * m2 * l1 * lc2 * s2 * v[2]
    C12 = -m2 * l1 * lc2 * s2 * v[2]
    C21 = m2 * l1 * lc2 * s2 * v[1]
    C22 = 0
    C = [C11 C12; C21 C22]
    
    G = [m1 * g * lc1 * s1 + m2 * g * (l1 * s1 + lc2 * s12); m2 * g * lc2 * s12]
    
    vd = M \ (tau - C * v - G)
    [v; vd]
end

Δt = 0.25
f = function (integrator)
    next_control_time[] = integrator.t + Δt
    u_modified!(integrator, false)
end
initialize = (c, u, t, integrator) -> (empty!(controltimes); last_control_time[] = NaN)
periodic = PeriodicCallback(f, Δt; initialize = initialize, save_positions = (false, false))
u0 = zeros(4)
final_time = 25.3
problem = ODEProblem(dynamics, u0, (0., final_time), callback = periodic)
solve(problem, Tsit5(), abs_tol = 1e-10, dt = 0.05);
@assert controltimes == collect(0. : Δt : final_time - rem(final_time, Δt))