ballandbeam.jl 1.82 KB
Newer Older
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
1
2
3
4
5
6
# Interface implementation Ball And Beam ====================================================

export BallAndBeam, BallAndBeamSimulator, BallAndBeamType

struct BallAndBeam <: PhysicalProcess
    h::Float64
7
    bias::Float64
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
8
end
9
BallAndBeam() = BallAndBeam(0.01, 0.0)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
10
11
12

struct BallAndBeamSimulator <: SimulatedProcess
    h::Float64
13
    state::Vector{Float64} # pos,vel,angle,ω
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
14
end
15
BallAndBeamSimulator() = BallAndBeamSimulator(0.01, zeros(4))
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
16
17
18
19
20
21
22

const BallAndBeamType = Union{BallAndBeam, BallAndBeamSimulator}
num_outputs(p::BallAndBeamType) = 2
num_inputs(p::BallAndBeamType)  = 1
outputrange(p::BallAndBeamType) = [(-10,10),(-1,1)] # Beam angle, Ball position
inputrange(p::BallAndBeamType)  = [(-10,10)]
isstable(p::BallAndBeamType)    = false
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
23
isasstable(p::BallAndBeamType)  = false
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
24
sampletime(p::BallAndBeamType)  = p.h
25
26
27
bias(p::BallAndBeam)            = p.bias
bias(p::BallAndBeamSimulator)   = 0

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
28
29

control(p::BallAndBeam, u)  = ccall((:comedi_write, comedipath),Int32,
30
                                        (Int32,Int32,Int32,Int32),0,1,1,num2io(u[1]+p.bias))
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
31
32
33
34
measure(p::BallAndBeam)     = io2num(ccall((:comedi_read,comedipath), Int32,
                                        (Int32,Int32,Int32), 0,0,0))

control(p::BallAndBeamSimulator, u)  = error("Not yet implemented")
35
measure(p::BallAndBeamSimulator)     = [p.state[1], p.state[3]]
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
36

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

initialize(p::BallAndBeam)  = ccall((:comedi_start, comedipath),Int32,(Int32,), 0)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
43
finalize(p::BallAndBeam)    = (control(p,0);ccall((:comedi_stop, comedipath),Int32,(Int32,), 0))
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
44
45
initialize(p::BallAndBeamSimulator)  = nothing
finalize(p::BallAndBeamSimulator)    = nothing