Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
Loading items

Target

Select target project
  • processes/LabProcesses.jl
  • martinheyden/LabProcesses.jl
2 results
Select Git revision
Loading items
Show changes
# Interface implementation Ball And Beam ====================================================
# The ball and beam can be used in two modes, either just the Beam, in which case there is a
# single output (measurement signal) or the BallAndBeam, in which case there are two.
# There are a few union types defined for convenience, these are
# AbstractBeam = Union{Beam, BeamSimulator}
# AbstractBallAndBeam = Union{BallAndBeam, BallAndBeamSimulator}
# AbstractBeamOrBallAndBeam = All types
# Although not Abstract per se, the names AbstractBeam etc. were chosen since this reflects
# their usage in dispatch.
export Beam, BeamSimulator, AbstractBeam, BallAndBeam, BallAndBeamSimulator, AbstractBeamOrBallAndBeam
# @with_kw allows specification of default values for fields. If none is given, this value must be supplied by the user. replaces many constructors that would otherwise only supply default values.
# Call constructor like Beam(bias=1.0) if you want a non-default value for bias
"""
Beam(;kwargs...)
Physical beam process
#Arguments (fields)
- `h::Float64 = 0.01`
- `bias::Float64 = 0.0`
- `stream::LabStream = ComediStream()`
- `measure::AnalogInput10V = AnalogInput10V(0)`
- `control::AnalogOutput10V = AnalogOutput10V(1)`
"""
struct Beam <: PhysicalProcess
h::Float64
bias::Float64
stream::LabStream
measure::AnalogInput10V
control::AnalogOutput10V
end
function Beam(;
h::Float64 = 0.01,
bias::Float64 = 0.,
stream::LabStream = ComediStream(),
measure::AnalogInput10V = AnalogInput10V(0),
control::AnalogOutput10V = AnalogOutput10V(1))
p = Beam(Float64(h),Float64(bias),stream,measure,control)
init_devices!(p.stream, p.measure, p.control)
p
end
include("define_beam_system.jl")
const beam_system, nice_beam_controller = define_beam_system()
# nice_beam_controller gives ϕₘ=56°, Aₘ=4, Mₛ = 1.6. Don't forget to discretize it before use
struct BeamSimulator <: SimulatedProcess
h::Float64
s::SysFilter
BeamSimulator(;h::Real = 0.01, bias=0) = new(Float64(h), SysFilter(beam_system, h))
end
struct BallAndBeam <: PhysicalProcess
h::Float64
bias::Float64
stream::LabStream
measure1::AnalogInput10V
measure2::AnalogInput10V
control::AnalogOutput10V
end
function BallAndBeam(;
h = 0.01,
bias = 0.,
stream = ComediStream(),
measure1::AnalogInput10V = AnalogInput10V(0),
measure2::AnalogInput10V = AnalogInput10V(1),
control::AnalogOutput10V = AnalogOutput10V(1))
p = BallAndBeam(h,bias,stream,measure1,measure2,control)
init_devices!(p.stream, p.measure1, p.measure2, p.control)
p
end
struct BallAndBeamSimulator <: SimulatedProcess
h::Float64
s::SysFilter
end
const AbstractBeam = Union{Beam, BeamSimulator}
const AbstractBallAndBeam = Union{BallAndBeam, BallAndBeamSimulator}
const AbstractBeamOrBallAndBeam = Union{AbstractBeam, AbstractBallAndBeam}
num_outputs(p::AbstractBeam) = 1
num_outputs(p::AbstractBallAndBeam) = 2
num_inputs(p::AbstractBeamOrBallAndBeam) = 1
outputrange(p::AbstractBeam) = [(-10,10)]
outputrange(p::AbstractBallAndBeam) = [(-10,10),(-1,1)] # Beam angle, Ball position
inputrange(p::AbstractBeamOrBallAndBeam) = [(-10,10)]
isstable(p::AbstractBeam) = true
isstable(p::AbstractBallAndBeam) = false
isasstable(p::AbstractBeamOrBallAndBeam) = false
sampletime(p::AbstractBeamOrBallAndBeam) = p.h
bias(p::AbstractBeamOrBallAndBeam) = p.bias
bias(p::BeamSimulator) = 0
bias(p::BallAndBeamSimulator) = 0
function control(p::AbstractBeamOrBallAndBeam, u::AbstractArray)
length(u) == 1 || error("Process $(typeof(p)) only accepts one control signal, tried to send u=$u.")
control(p,u[1])
end
control(p::AbstractBeamOrBallAndBeam, u::Number) = send(p.control,u)
control(p::BeamSimulator, u::Number) = p.s(u)
control(p::BallAndBeamSimulator, u::Number) = error("Not yet implemented")
measure(p::Beam) = read(p.measure)
measure(p::BallAndBeam) = [read(p.measure1), read(p.measure2)]
measure(p::BeamSimulator) = dot(p.s.sys.C,p.s.state)
measure(p::BallAndBeamSimulator) = error("Not yet implemented")
initialize(p::Beam) = nothing
initialize(p::BallAndBeam) = nothing
finalize(p::AbstractBeamOrBallAndBeam) = foreach(close, p.stream.devices)
initialize(p::BallAndBeamSimulator) = nothing
finalize(p::BallAndBeamSimulator) = nothing
initialize(p::BeamSimulator) = p.s.state .*= 0
finalize(p::BeamSimulator) = nothing
using ControlSystems, DSP
"""
beammodel, beamcontroller = define_beam_system(;doplot=false)
Return a continuous time model of the beam (`::StateSpace`) and a feedback controller that
gives ϕₘ = 56°, Aₘ = 4, Mₛ = 1.6.
Don't forget to discretize both before use. Call this method with
doplot = true to display a bunch of plots to convince yourself.
The numbers in the code have been selected based on the fra performed and reported in
`beamdetails.pdf`
"""
function define_beam_system(;doplot=false)
K = 5 # gain of internal velocity control
I1 = 1 # intertia of motor mass
I2 = 1 # inertia of beam
C = 5 # beam flexibility
D = 0.1 # beam damping
T = 0.01 # motor torque time constant
E = 1 # motor viscous damping
# transfer function from input signal to motor position
G1 = 2*tf([I2, D, C]*K,conv([T, 1],conv([I1, (D+E), C],[I2, D, C]) - [0; 0; conv([D, C],[D, C])]) + K*[0, 0, I2, D, C, 0])
G2 = 2*tf([D, C]*K,conv([T, 1],conv([I1, (D+E), C],[I2, D, C]) - [0; 0; conv([D, C],[D, C])]) + K*[0, 0, I2, D, C, 0])
# pzmap(G1)
# bodeplot([G1,G2], logspace(-2,3,500))
# Above model is super inaccurate and I have no idea where it comes from, let's define
# a simple but more accurate model // Bagge
G1 = zpk([-145],[0, -60, -100], 4.5*60*100/145)
ωz = 145
ζz = 0.01
ωp = 150
ζp = 0.005
G1 *= tf([1,2ζz*ωz, ωz^2]*ωp^2, [1,2ζp*ωp, ωp^2]*ωz^2)
sysFBc = ss(zpk([], [-40, -40], 2*40*40)) # Controller suggesten in assistant manual, works fine =)
# gives ϕₘ = 56°, Aₘ = 4, Mₛ = 1.6. Don't forget to discretize it before use. Call this method with
# doplot = true to display a bunch of plots to convince yourself
if doplot
ControlSystems.setPlotScale("log10")
bodeplot(tf(G1), logspace(0,log10(200),300))
bodeplot([tf(G1), sysFBc*G1], logspace(0,log10(200),300))
marginplot(sysFBc*tf(G1))
gangoffourplot(tf(G1), tf(sysFBc))
end
ss(G1), sysFBc
end
# Interface implementation Ball And Beam ====================================================
# There is a union type defined for convenience:
# AbstractETHHelicopter = Union{ETHHelicopter, ETHHelicopterSimulator}
# Although not Abstract per se, the names AbstractETHHelicopter etc. were chosen since this
# reflects the usage in dispatch.
export ETHHelicopter, ETHHelicopterSimulator, AbstractETHHelicopter
# @with_kw allows specification of default values for fields. If none is given, this value must be supplied by the user. replaces many constructors that would otherwise only supply default values.
# Call constructor like ETHHelicopter(bias=1.0) if you want a non-default value for bias
"""
ETHHelicopter(;kwargs...)
Physical ETH helicopter process
# Arguments (fields)
- `h::Float64 = 0.05`
- `bias::Float64 = 0.0`
- `stream::LabStream = ComediStream()`
- `measure1::AnalogInput10V = AnalogInput10V(0)`
- `measure2::AnalogInput10V = AnalogInput10V(1)`
- `control1::AnalogOutput10V = AnalogOutput10V(0)`
- `control2::AnalogOutput10V = AnalogOutput10V(1)`
"""
@with_kw struct ETHHelicopter <: PhysicalProcess
h::Float64
bias::Float64
stream::LabStream
measure1::AnalogInput10V
measure2::AnalogInput10V
control1::AnalogOutput10V
control2::AnalogOutput10V
end
function ETHHelicopter(;
h = 0.05,
bias = 0.,
stream = ComediStream(),
measure1::AnalogInput10V = AnalogInput10V(0),
measure2::AnalogInput10V = AnalogInput10V(1),
control1::AnalogOutput10V = AnalogOutput10V(0),
control2::AnalogOutput10V = AnalogOutput10V(1))
p = ETHHelicopter(h,bias,stream,measure1,measure2,control1,control2)
init_devices!(p.stream, p.measure1, p.measure2, p.control1, p.control2)
p
end
struct ETHHelicopterSimulator <: SimulatedProcess
h::Float64
bias::Float64
state::Vector{Float64}
end
ETHHelicopterSimulator() = ETHHelicopterSimulator(0.01, zeros(4))
const AbstractETHHelicopter = Union{ETHHelicopter, ETHHelicopterSimulator}
num_outputs(p::AbstractETHHelicopter) = 2
num_inputs(p::AbstractETHHelicopter) = 2
outputrange(p::AbstractETHHelicopter) = [(-10,10),(-10,10)]
inputrange(p::AbstractETHHelicopter) = [(-10,10),(-10,10)]
isstable(p::AbstractETHHelicopter) = false
isasstable(p::AbstractETHHelicopter) = false
sampletime(p::AbstractETHHelicopter) = p.h
bias(p::AbstractETHHelicopter) = p.bias
function control(p::ETHHelicopter, u)
send(p.control1,u[1])
send(p.control2,u[2])
end
measure(p::ETHHelicopter) = [read(p.measure1), read(p.measure2)]
control(p::ETHHelicopterSimulator, u) = error("Not yet implemented")
measure(p::ETHHelicopterSimulator) = error("Not yet implemented")
initialize(p::ETHHelicopter) = nothing
finalize(p::ETHHelicopter) = foreach(close, p.stream.devices)
initialize(p::ETHHelicopterSimulator) = nothing
finalize(p::ETHHelicopterSimulator) = nothing
export PRBSGenerator
#PRBS
"""
r = PRBSGenerator()
Generates a pseudo-random binary sequence. Call like `random_input = r()`.
"""
mutable struct PRBSGenerator
state::Int
end
PRBSGenerator() = PRBSGenerator(Int(1))
function (r::PRBSGenerator)(args...)
state = r.state
bit = ((state >> 0) (state >> 2) (state >> 3) (state >> 5) ) & 1
r.state = (state >> 1) | (bit << 15)
bit
end
export @periodically, init_sysfilter, sysfilter!, SysFilter
"""
@periodically(h, body)
Ensures that the body is run with an interval of `h >= 0.001` seconds.
"""
macro periodically(h, body)
quote
local start_time = time()
$(esc(body))
local execution_time = time()-start_time
Libc.systemsleep(max(0,$(esc(h))-execution_time))
end
end
"""
@periodically(h, simulation::Bool, body)
Ensures that the body is run with an interval of `h >= 0.001` seconds.
If `simulation == false`, no sleep is done
"""
macro periodically(h, simulation, body)
quote
local start_time = time()
$(esc(body))
local execution_time = time()-start_time
$(esc(simulation)) || Libc.systemsleep(max(0,$(esc(h))-execution_time))
end
end
"""
Csf = SysFilter(sys_discrete::StateSpace)
Csf = SysFilter(sys_continuous::StateSpace, sampletime)
Csf = SysFilter(sys::StateSpace, state::AbstractVector)
Returns an object used for filtering signals through LTI systems.
Create a SysFilter object that can be used to implement control loops and simulators
with LTI systems, i.e., `U(z) = C(z)E(z)`. To filter a signal `u` through the filter,
call like `y = Csf(u)`. Calculates the filtered output `y` in `y = Cx+Du, x'=Ax+Bu`
"""
struct SysFilter{T<:StateSpace}
sys::T
state::Vector{Float64}
function SysFilter(sys::StateSpace, state::AbstractVector)
@assert !ControlSystems.iscontinuous(sys) "Can not filter using continuous time model."
@assert length(state) == sys.nx "length(state) != sys.nx"
new{typeof(sys)}(sys, state)
end
function SysFilter(sys::StateSpace)
@assert !ControlSystems.iscontinuous(sys) "Can not filter using continuous time model. Supply sample time."
new{typeof(sys)}(sys, init_sysfilter(sys))
end
function SysFilter(sys::StateSpace, h::Real)
@assert ControlSystems.iscontinuous(sys) "Sample time supplied byt system model is already in discrete time."
sysd = c2d(sys, h)[1]
new{typeof(sysd)}(sysd, init_sysfilter(sysd))
end
end
(s::SysFilter)(input) = sysfilter!(s.state, s.sys, input)
"""
state = init_sysfilter(sys::StateSpace)
Use together with [`sysfilter!`](@ref)
"""
function init_sysfilter(sys::StateSpace)
zeros(sys.nx)
end
"""
output = sysfilter!(s::SysFilter, input)
output = sysfilter!(state, sys::StateSpace, input)
Returns the filtered output `y` in `y = Cx+Du, x'=Ax+Bu`
This function is used to implement control loops where a signal is filtered through a
dynamical system, i.e., `U(z) = C(z)E(z)`. Initialize `state` using [`init_sysfilter`](@ref).
"""
function sysfilter!(state::AbstractVector, sys::StateSpace, input)
state .= vec(sys.A*state + sys.B*input)
output = vec(sys.C*state + sys.D*input)
end
sysfilter!(s::SysFilter, input) = sysfilter!(s.state, s.sys, input)
using LabProcesses, ControlSystems, DSP
using Test
# Reference generators
r = PRBSGenerator(Int(4))
seq = [r() for i = 1:10]
@test all(seq .== [1,0,1,0,0,0,0,0,0,0])
foreach(r,1:10_000)
function test_sysfilter()
N = 10
u = randn(N)
b = [1, 1]
a = [1, 0.1, 1]
sys = ss(tf(b,a,1))
state = init_sysfilter(sys)
yf = filt(b,a,u)
yff = similar(yf)
for i in eachindex(u)
yff[i] = sysfilter!(state, sys, u[i])[1]
end
@test sum(abs,yf - yff) < (eps())
sysfilt = SysFilter(sys)
for i in eachindex(u)
yff[i] = sysfilter!(sysfilt, u[i])[1]
end
@test sum(abs,yf - yff) < (eps())
sysfilt = SysFilter(sys)
for i in eachindex(u)
yff[i] = sysfilt(u[i])[1]
end
@test sum(abs,yf - yff) < (eps())
end
test_sysfilter()