ballandbeam.jl 3.78 KB
Newer Older
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
1
2
# Interface implementation Ball And Beam ====================================================

3
4
5
6
7
8
9
10
11
12
# 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
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
13
14
15
16
17
18
19

struct Beam <: PhysicalProcess
    h::Float64
    bias::Float64
end
Beam() = Beam(0.01, 0.0)

20
21
22
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
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
23
24
struct BeamSimulator <: SimulatedProcess
    h::Float64
25
26
27
28
    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])
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
29
end
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
30
31
32

struct BallAndBeam <: PhysicalProcess
    h::Float64
33
    bias::Float64
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
34
end
35
BallAndBeam() = BallAndBeam(0.01, 0.0)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
36
37
38

struct BallAndBeamSimulator <: SimulatedProcess
    h::Float64
39
    state::Vector{Float64} # pos,vel,angle,ω
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
40
end
41
BallAndBeamSimulator() = BallAndBeamSimulator(0.01, zeros(4))
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
42

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
43

44
45
46
const AbstractBeam              = Union{Beam, BeamSimulator}
const AbstractBallAndBeam       = Union{BallAndBeam, BallAndBeamSimulator}
const AbstractBeamOrBallAndBeam = Union{AbstractBeam, AbstractBallAndBeam}
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
47
48


49
50
51
52
53
54
55
56
57
58
59
num_outputs(p::AbstractBeam)             = 1
num_outputs(p::AbstractBallAndBeam)      = 2
num_inputs(p::AbstractBeamOrBallAndBeam) = 1
outputrange(p::Beam)                     = [(-10,10)]
outputrange(p::AbstractBallAndBeam)      = [(-10,10),(-1,1)] # Beam angle, Ball position
inputrange(p::AbstractBeamOrBallAndBeam) = [(-10,10)]
isstable(p::Beam)                        = true
isstable(p::AbstractBallAndBeam)         = false
isasstable(p::AbstractBeamOrBallAndBeam) = false
sampletime(p::AbstractBeamOrBallAndBeam) = p.h
bias(p::AbstractBeamOrBallAndBeam)       = p.bias
60
bias(p::BeamSimulator)                   = 0
61
bias(p::BallAndBeamSimulator)            = 0
62

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
63

64
control(p::AbstractBeamOrBallAndBeam, u) = ccall((:comedi_write, comedipath),Int32,
65
                                        (Int32,Int32,Int32,Int32),0,1,1,num2io(u[1]+p.bias))
66
67
68
69
70
71

function control(p::BeamSimulator, u)
    sysfilter!(p.state, p.sys, u)
end
control(p::BallAndBeamSimulator, u)  = error("Not yet implemented")

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
72
measure(p::Beam) = io2num(ccall((:comedi_read,comedipath), Int32,
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
73
                                        (Int32,Int32,Int32), 0,0,0))
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
74
75
measure(p::BallAndBeam) = [io2num(ccall((:comedi_read,comedipath), Int32,
(Int32,Int32,Int32), 0,0,i)) for i = 0:1]
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
76

77
78
79
measure(p::BeamSimulator)        = vecdot(p.sys.C,p.state)
measure(p::BallAndBeamSimulator) = error("Not yet implemented")

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
80

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
81
const comedipath = Pkg.dir("LabProcesses","c","comedi_bridge.so")
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
82
83
const conversion = 65535/20
io2num(x) = x/conversion -10            # Converts from io to float
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
84
num2io(x) = round(Int32,x*100 + 2048)   # Converts from regular number to io
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
85

86
87
88
89
initialize(p::AbstractBeamOrBallAndBeam) = ccall((:comedi_start, comedipath),Int32,(Int32,), 0)
finalize(p::AbstractBeamOrBallAndBeam)   = (control(p,0);ccall((:comedi_stop, comedipath),Int32,(Int32,), 0))
initialize(p::BallAndBeamSimulator)      = nothing
finalize(p::BallAndBeamSimulator)        = nothing
90
91
initialize(p::BeamSimulator)             = nothing
finalize(p::BeamSimulator)               = nothing