Commit 91750c14 authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

Implement BeamSimulator

parent 127b12ad
......@@ -6,6 +6,7 @@ export num_outputs,
isstable,
isasstable,
sampletime,
bias,
control,
measure,
initialize,
......
......@@ -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
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
......@@ -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
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