Fixed error in delay sample handling

src/interface_implementations/flexibleservo.jl

@@ -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