Commit 57b76009 authored by Nils Vreman's avatar Nils Vreman
Browse files

Added delay fucntionality to the simulator. Will continue with delay...

Added delay fucntionality to the simulator. Will continue with delay functionality for the general process
parent a4963135
Pipeline #728 canceled with stage
# Interface implementation Flexible Servo ===========================
using DataStructures: CircularBuffer
#TODO export get_states and define process
export FlexibleServo, FlexibleServoSimulator, AbstractFlexibleServo, define_flexible_servo
......@@ -45,24 +46,45 @@ function define_flexible_servo()
return ss(A,B,C,D)
end
# Delay function
function get_sample_delay(delay, delay_var, sample_time)
return ceil((delay + abs(delay_var*randn())) / sample_time)
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)
function FlexibleServoSimulator(
;
h::Float64 = 0.01,
delay::Float64 = 0.0,
delay_var::Float64 = 0.0)
Gp = define_flexible_servo()
Gp_disc = c2d(Gp, h)
return FlexibleServoSimulator(h,zeros(4,1), Gp_disc[1].A, Gp_disc[1].B, Gp_disc[1].C)
# 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)
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}
num_outputs(p::AbstractFlexibleServo) = 2
num_inputs(p::AbstractFlexibleServo) = 1
num_states(P::AbstractFlexibleServo) = 4 #For statefeedback simulation in ak lab3
......@@ -80,12 +102,34 @@ end
control(p::FlexibleServo, u) = send(p.control,u)
function control(p::FlexibleServoSimulator, u)
p.x .= p.Φ*p.x + p.Γ*u
# Used to simulate delay in the system
u1 = nothing
# If we have no delay, ignore this section
if p.delay != 0
# Get control signal from queue
u1 = popfirst!(p.cb)
# 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.
while delay < length(p.cb) + 1
pop!(p.cb)
end
last_ele = p.cb[end]
while delay > length(p.cb) + 1
push!(p.cb, last_ele)
end
# Add the new control signal to the back of the queue
push!(p.cb,u)
else
u1 = u
end
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
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))")
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment