Rewrote some code to improve code-reusage. Also added delay functionality to...

Rewrote some code to improve code-reusage. Also added delay functionality to real process (not only simulator)

src/interface_implementations/flexibleservo.jl

@@ -5,24 +5,6 @@ using DataStructures: CircularBuffer
 #TODO export get_states and define process
 export FlexibleServo, FlexibleServoSimulator, AbstractFlexibleServo, define_flexible_servo
 export num_states, get_state
-struct FlexibleServo <: PhysicalProcess
-    h::Float64
-    stream::LabStream
-    measure1::AnalogInput10V
-    measure2::AnalogInput10V
-    control::AnalogOutput10V
-end
-
-function FlexibleServo(;
-    h::Float64                  = 0.01,
-    stream::LabStream            = ComediStream(),
-    measure1::AnalogInput10V    = AnalogInput10V(0),
-    measure2::AnalogInput10V    = AnalogInput10V(1),
-    control::AnalogOutput10V    = AnalogOutput10V(1))
-    p = FlexibleServo(Float64(h), stream, measure1, measure2, control)
-    init_devices!(p.stream, p.measure1, p.measure2, p.control) #TODO no idea what this does
-    return p
-end
 
 function define_flexible_servo()
     # Creates linear process model
@@ -46,9 +28,35 @@ 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)
+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
@@ -67,12 +75,14 @@ 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(delay/h) + 100) # TODO: Max capacity
     push!(cb, 0)
     push!(cb, 0)
+
     return FlexibleServoSimulator(h,
                                   zeros(4,1), 
                                   Gp_disc[1].A, 
@@ -85,23 +95,12 @@ 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
-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])
+# Delay function
+function get_sample_delay(delay, delay_var, sample_time)
+    return ceil((delay + abs(delay_var*randn())) / sample_time)
 end
 
-control(p::FlexibleServo, u)  = send(p.control,u)
-function control(p::FlexibleServoSimulator, u)
+function delay_control_signal(p::AbstractFlexibleServo, u)
     # Used to simulate delay in the system
     u1 = nothing
     # If we have no delay, ignore this section
@@ -124,6 +123,33 @@ function control(p::FlexibleServoSimulator, u)
     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