From 91750c14e2558f52dc95add89a7354d29013b567 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <cont-frb@ulund.org>
Date: Tue, 22 Aug 2017 20:18:47 +0200
Subject: [PATCH] Implement BeamSimulator
---
src/interface.jl | 1 +
src/interface_implementations/ballandbeam.jl | 23 ++++++--
.../define_beam_system.jl | 52 +++++++++++++++++++
src/utilities.jl | 8 +--
4 files changed, 76 insertions(+), 8 deletions(-)
create mode 100644 src/interface_implementations/define_beam_system.jl
diff --git a/src/interface.jl b/src/interface.jl
index db99995..44a7b84 100644
--- a/src/interface.jl
+++ b/src/interface.jl
@@ -6,6 +6,7 @@ export num_outputs,
isstable,
isasstable,
sampletime,
+ bias,
control,
measure,
initialize,
diff --git a/src/interface_implementations/ballandbeam.jl b/src/interface_implementations/ballandbeam.jl
index d91092f..05543f5 100644
--- a/src/interface_implementations/ballandbeam.jl
+++ b/src/interface_implementations/ballandbeam.jl
@@ -17,11 +17,16 @@ struct Beam <: PhysicalProcess
end
Beam() = Beam(0.01, 0.0)
+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
- state::Vector{Float64} # angle,ω
+ state::Vector{Float64} # states defined by the file define_beam_system
+ sys::StateSpace
+ BeamSimulator() = new(0.01, init_sysfilter(beam_system), c2d(beam_system, 0.01)[1])
+ BeamSimulator(h::Real) = new(Float64(h), init_sysfilter(beam_system), c2d(beam_system, h)[1])
end
-BeamSimulator() = BeamSimulator(0.01, zeros(2))
struct BallAndBeam <: PhysicalProcess
h::Float64
@@ -52,18 +57,26 @@ 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
control(p::AbstractBeamOrBallAndBeam, u) = ccall((:comedi_write, comedipath),Int32,
(Int32,Int32,Int32,Int32),0,1,1,num2io(u[1]+p.bias))
+
+function control(p::BeamSimulator, u)
+ sysfilter!(p.state, p.sys, u)
+end
+control(p::BallAndBeamSimulator, u) = error("Not yet implemented")
+
measure(p::Beam) = io2num(ccall((:comedi_read,comedipath), Int32,
(Int32,Int32,Int32), 0,0,0))
measure(p::BallAndBeam) = [io2num(ccall((:comedi_read,comedipath), Int32,
(Int32,Int32,Int32), 0,0,i)) for i = 0:1]
-control(p::BallAndBeamSimulator, u) = error("Not yet implemented")
-measure(p::BallAndBeamSimulator) = [p.state[1], p.state[3]]
+measure(p::BeamSimulator) = vecdot(p.sys.C,p.state)
+measure(p::BallAndBeamSimulator) = error("Not yet implemented")
+
const comedipath = Pkg.dir("LabProcesses","c","comedi_bridge.so")
const conversion = 65535/20
@@ -74,3 +87,5 @@ initialize(p::AbstractBeamOrBallAndBeam) = ccall((:comedi_start, comedipath),Int
finalize(p::AbstractBeamOrBallAndBeam) = (control(p,0);ccall((:comedi_stop, comedipath),Int32,(Int32,), 0))
initialize(p::BallAndBeamSimulator) = nothing
finalize(p::BallAndBeamSimulator) = nothing
+initialize(p::BeamSimulator) = nothing
+finalize(p::BeamSimulator) = nothing
diff --git a/src/interface_implementations/define_beam_system.jl b/src/interface_implementations/define_beam_system.jl
new file mode 100644
index 0000000..5aba53b
--- /dev/null
+++ b/src/interface_implementations/define_beam_system.jl
@@ -0,0 +1,52 @@
+using ControlSystems
+
+"""
+ 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
diff --git a/src/utilities.jl b/src/utilities.jl
index 14373fc..7cff5ca 100644
--- a/src/utilities.jl
+++ b/src/utilities.jl
@@ -18,7 +18,7 @@ end
Use together with [`sysfilter!`](@ref)
"""
function init_sysfilter(sys::StateSpace)
- zeros(sys.nx,1)
+ zeros(sys.nx)
end
"""
@@ -28,7 +28,7 @@ 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, sys::StateSpace, input)
- state .= sys.A*state + sys.B*input
- output = sys.C*state + sys.D*input
+function sysfilter!(state::AbstractVector, sys::StateSpace, input)
+ state .= vec(sys.A*state + sys.B*input)
+ output = vec(sys.C*state + sys.D*input)
end
--
GitLab