test_velocity_limit.jl

using DiffEqBase
using OrdinaryDiffEq

###############################################################################
#                                  Variables
###############################################################################

# Global variable to store the acceleration.
# This solution was selected just for the sake of simplification. Don't use
# global variables!
a = 0.0

# Parameters.
r  = 10.0   # Reference position.
k  = 0.3    # Proportional gain.
d  = 0.8    # Derivative gain.
vl = 2.0    # Velocity limit.

# Configuration of the simulation.
tf     = 30.0             # Final simulation time.
tstops = collect(0:1:tf)  # Instants that the control loop will be computed.
u0     = [0.0; 0.0]       # Initial state.

###############################################################################
#                                  Functions
###############################################################################

# Dynamic equation.
function dyn_eq(t,u,du)
    du .= [0 1; 0 0]*u + [0;1]*a
end

# CALLBACK: Control loop.
# =======================

# Condition function.
function condition_control_loop(t,u,integrator)
    (t in tstops)
end

# Affect function.
function control_loop!(integrator)
    global a

    p = integrator.u[1]
    v = integrator.u[2]

    a = k*(r-p) + d*(0.0-v)
end

# CALBACK: Velocity limit.
# ========================

# Condition function.
function condition_vel_limit(t,u,integrator)
    vl - abs(u[2])
end

function vel_limit!(integrator)
    integrator.u[2] = sign(integrator.u[2])*vl-0.001
end

cb_cl = DiscreteCallback(condition_control_loop, control_loop!)
cb_vl = ContinuousCallback(condition_vel_limit, vel_limit!, interp_points=100000)

cb = CallbackSet(cb_cl, cb_vl)

###############################################################################
#                                    Solver
###############################################################################

prob = ODEProblem(dyn_eq, u0, (0.0, tf))
sol  = solve(prob, Tsit5(), callback = cb, tstops=tstops, force_dtmin=true)
nothing