Commit f08c5b09 authored by Nils Vreman's avatar Nils Vreman
Browse files

Fixed error in delay sample handling

parent 8b989722
Pipeline #730 failed with stage
......@@ -79,7 +79,7 @@ function FlexibleServoSimulator(
Gp = define_flexible_servo()
Gp_disc = c2d(Gp, h)
# Circular buffer two keep track of control signals with delay
cb = CircularBuffer{Float64}(Int(delay/h) + 100) # TODO: Max capacity
cb = CircularBuffer{Float64}(Int(round(delay / h)) + 100) # TODO: Max capacity
push!(cb, 0)
push!(cb, 0)
......@@ -105,21 +105,22 @@ function delay_control_signal(p::AbstractFlexibleServo, u)
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
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
......
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