Commit 8b989722 authored by Nils Vreman's avatar Nils Vreman

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)
parent 57b76009
Pipeline #729 failed with stage
......@@ -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
......
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