Compare revisions

include("interface_documentation.jl")

include("interface_implementations/ballandbeam.jl")

include("interface_implementations/eth_helicopter.jl")

include("interface_implementations/flexibleservo.jl")

include("reference_generators.jl")

include("controllers.jl")

# Interface implementation Flexible Servo ===========================

using DataStructures: CircularBuffer

#TODO export get_states and define process

export FlexibleServo, FlexibleServoSimulator, AbstractFlexibleServo, define_flexible_servo

export num_states, get_state

function define_flexible_servo()

    # Creates linear process model

    m1 = 2.29; m2 = 2.044        # Masses

    d1 = 3.12; d2 = 3.73         # Damping coefficients

    k = 400                      # Spring constant

    km = 2.96                   # Motor constant

    ky = 280                     # Measurement constant

    # Process matrices

    A = [0 1 0 0;

        -k/m1 -d1/m1 k/m1 0

        0 0 0 1

        ; k/m2 0 -k/m2 -d2/m2]

    B = [0; km/m1; 0; 0]

    C = [ky 0 0 0 ;

        0 0 ky 0]

    D = 0

    return ss(A,B,C,D)

end

struct FlexibleServo <: PhysicalProcess

    h::Float64

    stream::LabStream

    measure1::AnalogInput10V

    measure2::AnalogInput10V

    control::AnalogOutput10V

    delay::Float64

    delay_var::Float64

    cb::CircularBuffer{Float64}

end

function FlexibleServo(

                       ;

                       h::Float64               = 0.01,

                       stream::LabStream        = ComediStream(),

                       measure1::AnalogInput10V = AnalogInput10V(0),

                       measure2::AnalogInput10V = AnalogInput10V(1),

                       control::AnalogOutput10V = AnalogOutput10V(1),

                       delay::Float64           = 0.0, 

                       delay_var::Float64       = 0.0)

    # Circular buffer two keep track of control signals with delay

    cb = CircularBuffer{Float64}(Int(delay/h) + 100) # TODO: Max capacity

    push!(cb, 0)

    push!(cb, 0)

    p = FlexibleServo(Float64(h), stream, measure1, measure2, control, delay, delay_var, cb)

    init_devices!(p.stream, p.measure1, p.measure2, p.control) #TODO no idea what this does

    return p

end

struct FlexibleServoSimulator <: SimulatedProcess

    h::Float64

    x::Array{Float64,2} #TODO Dim 1 or 2? Result of matrix mult is dim 2?

    Φ::Array{Float64,2}

    Γ::Array{Float64,2}

    C::Array{Float64,2}

    delay::Float64

    delay_var::Float64

    cb::CircularBuffer{Float64}

end

function FlexibleServoSimulator(

                                ;

                                h::Float64         = 0.01, 

                                delay::Float64     = 0.0, 

                                delay_var::Float64 = 0.0)

    Gp = define_flexible_servo()

    Gp_disc = c2d(Gp, h)

    # Circular buffer two keep track of control signals with delay

    cb = CircularBuffer{Float64}(Int(round(delay / h)) + 100) # TODO: Max capacity

    push!(cb, 0)

    push!(cb, 0)

    return FlexibleServoSimulator(h,

                                  zeros(4,1), 

                                  Gp_disc[1].A, 

                                  Gp_disc[1].B, 

                                  Gp_disc[1].C, 

                                  delay, 

                                  delay_var,

                                  cb)

end

const AbstractFlexibleServo = Union{FlexibleServo, FlexibleServoSimulator}

# Delay function

function get_sample_delay(delay, delay_var, sample_time)

    return ceil((delay + abs(delay_var*randn())) / sample_time)

end

function delay_control_signal(p::AbstractFlexibleServo, u)

    # Used to simulate delay in the system

    u1 = nothing

    # If we have no delay, ignore this section

    if p.delay != 0

        # Pull new delay based on simulators distribution

        delay = get_sample_delay(p.delay, p.delay_var, p.h)

        # add new control signal to its corresponding position in the queue by

        # removing all older control signals behind it in the queue.

        last_ele = p.cb[end]

        while delay < length(p.cb)

            last_ele = pop!(p.cb)

        end

        while delay > length(p.cb) 

            push!(p.cb, last_ele)

        end

        # Add the new control signal to the back of the queue

        push!(p.cb,u)

        # Get control signal from queue

        u1 = popfirst!(p.cb)

    else

        u1 = u

    end

    # Return the control signal that can be seen at this time instance

    return u1

end

num_outputs(p::AbstractFlexibleServo) = 2

num_inputs(p::AbstractFlexibleServo)  = 1

num_states(P::AbstractFlexibleServo)  = 4  #For statefeedback simulation in ak lab3

outputrange(p::AbstractFlexibleServo) = [(-10,10),(-10,10)] #Process has two outputs, only one uses in AK lab3

inputrange(p::AbstractFlexibleServo)  = [(-10,10)]

isstable(p::AbstractFlexibleServo)    = true

isasstable(p::AbstractFlexibleServo)  = false

sampletime(p::AbstractFlexibleServo)  = p.h

bias(p::AbstractFlexibleServo)        = 0 #TODO: Add bias to structs?

function control(p::AbstractFlexibleServo, u::AbstractArray)

    length(u) == 1 || error("Process $(typeof(p)) only accepts one control signal, tried to send u=$u.")

    control(p,u[1])

end

function control(p::FlexibleServo, u)

    u1 = delay_control_signal(p, u)

    send(p.control,u)

end

function control(p::FlexibleServoSimulator, u)

    u1   = delay_control_signal(p, u)

    p.x .= p.Φ*p.x + p.Γ*u1

    nothing

end

measure(p::FlexibleServo)            = [read(p.measure1) read(p.measure2)]

measure(p::FlexibleServoSimulator)   = p.C*p.x

get_state(p::FlexibleServoSimulator) = p.x

initialize(p::AbstractProcess)  = error("Function not implemented for $(typeof(p))")

finalize(p::AbstractProcess)    = error("Function not implemented for $(typeof(p))")