Implement BeamSimulator

src/interface.jl

@@ -6,6 +6,7 @@ export  num_outputs,
         isstable,
         isasstable,
         sampletime,
+        bias,
         control,
         measure,
         initialize,

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

src/interface_implementations/define_beam_system.jl

+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

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