furuta.jl 4.89 KB
Newer Older
Julian Salt's avatar
Julian Salt committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
# Interface implementation Furuta Pendulum ====================================================

export Furuta, FurutaSimulator, AbstractFuruta

# @with_kw allows specification of default values for fields. If none is given, this value must be supplied by the user. replaces many constructors that would otherwise only supply default values.
# Call constructor like Furuta(bias=1.0) if you want a non-default value for bias

#    Furuta(;kwargs...)
#Physical Furuta process

#Arguments (fields)
#- `h::Float64 = 0.01`
#- `bias::Float64 = 0.0`
#- `stream::LabStream = ComediStream()`
#- `measure::AnalogInput10V = AnalogInput10V(0)`
#- `control::AnalogOutput10V = AnalogOutput10V(1)`

struct Furuta <: PhysicalProcess
    h::Float64
    bias::Float64
Julian Salt's avatar
Julian Salt committed
21
22
23
24
25
26
    phi::Float64
    phi_dot::Float64
    theta::Float64
    theta_dot::Float64
    theta_precise::Float64
    theta_dot_precise::Float64
Julian Salt's avatar
Julian Salt committed
27
    stream::LabStream
Julian Salt's avatar
Julian Salt committed
28
29
30
31
32
33
    measure_phi::AnalogInput10V
    measure_phi_dot::AnalogInput10V
    measure_theta::AnalogInput10V
    measure_theta_dot::AnalogInput10V
    measure_theta_precise::AnalogInput10V
    measure_theta_dot_precise::AnalogInput10V
Julian Salt's avatar
Julian Salt committed
34
35
    control::AnalogOutput10V
end
Julian Salt's avatar
Julian Salt committed
36
function Furuta(;
Julian Salt's avatar
Julian Salt committed
37
38
    h::Float64               = 0.01,
    bias::Float64            = 0.,
Julian Salt's avatar
Julian Salt committed
39
40
41
42
43
44
    phi::Float64            = 0.0,
    phi_dot::Float64            = 0.0,
    theta::Float64            = 0.0,
    theta_dot::Float64            = 0.0,
    theta_precise::Float64            = 0.0,
    theta_dot_precise::Float64            = 0.0,
Julian Salt's avatar
Julian Salt committed
45
    stream::LabStream        = ComediStream(),
Julian Salt's avatar
Julian Salt committed
46
47
48
49
50
51
    measure_phi::AnalogInput10V  = AnalogInput10V(4),
    measure_phi_dot::AnalogInput10V  = AnalogInput10V(5),
    measure_theta::AnalogInput10V  = AnalogInput10V(6),
    measure_theta_dot::AnalogInput10V  = AnalogInput10V(7),
    measure_theta_precise::AnalogInput10V  = AnalogInput10V(2),
    measure_theta_dot_precise::AnalogInput10V  = AnalogInput10V(3),
Julian Salt's avatar
Julian Salt committed
52
    control::AnalogOutput10V = AnalogOutput10V(0))
Julian Salt's avatar
Julian Salt committed
53
54
    p = Furuta(Float64(h),Float64(bias),Float64(phi),Float64(phi_dot),Float64(theta),Float64(theta_dot),Float64(theta_precise),Float64(theta_dot_precise),stream,measure_phi,measure_phi_dot,measure_theta,measure_theta_dot,measure_theta_precise,measure_theta_dot_precise,control)
  # p = Furuta(Float64(h),Float64(bias),stream,measure_phi,measure_phi_dot,measure_theta,measure_theta_dot,measure_theta_precise,measure_theta_dot_precise,control)
Julian Salt's avatar
Julian Salt committed
55
    init_devices!(p.stream, p.measure_phi, p.measure_phi_dot, p.measure_theta, p.measure_theta_dot, p.measure_theta_precise, p.measure_theta_dot_precise, p.control)
Julian Salt's avatar
Julian Salt committed
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
    p
end

include("define_furuta_system.jl")
const furuta_system = define_furuta_system()

struct FurutaSimulator <: SimulatedProcess
    h::Float64
    s::SysFilter
    FurutaSimulator(;h::Real = 0.01, bias=0) = new(Float64(h), SysFilter(furuta_system, h))
end
#FurutaSimulator() = FurutaSimulator()

const AbstractFuruta              = Union{Furuta, FurutaSimulator}
num_outputs(p::AbstractFuruta)             = 1
Julian Salt's avatar
Julian Salt committed
71
num_inputs(p::AbstractFuruta)             = 6
Julian Salt's avatar
Julian Salt committed
72
outputrange(p::AbstractFuruta)             = [(-10,10)]
Julian Salt's avatar
Julian Salt committed
73
inputrange(p::AbstractFuruta)             =  [(-10,10),(-10,10),(-10,10),(-10,10),(-10,10),(-10,10)]
Julian Salt's avatar
Julian Salt committed
74
75
76
77
78
79
80
isstable(p::AbstractFuruta)    = false
isasstable(p::AbstractFuruta)  = false
sampletime(p::AbstractFuruta)  = p.h
bias(p::AbstractFuruta)        = p.bias

function control(p::AbstractFuruta, u::AbstractArray)
    length(u) == 1 || error("Process $(typeof(p)) only accepts one control signal, tried to send u=$u.")
Julian Salt's avatar
Julian Salt committed
81
82
    a::Float64 = -1.4*u[1]
    control(p,a)
Julian Salt's avatar
Julian Salt committed
83
84
85
86
87
88
end

control(p::AbstractFuruta, u::Number) = send(p.control,u)
control(p::FurutaSimulator, u::Number)             = p.s(u)


Julian Salt's avatar
Julian Salt committed
89
measure_phi(p::Furuta)                         = read(p.measure_phi)
Julian Salt's avatar
Julian Salt committed
90
phi(p::Furuta)                                      = measure_phi(p)*2.56
Julian Salt's avatar
Julian Salt committed
91
measure_phi_dot(p::Furuta)                         = read(p.measure_phi_dot)
Julian Salt's avatar
Julian Salt committed
92
phi_dot(p::Furuta)                                      = (measure_phi_dot(p) + 0.0708)*2.0
Julian Salt's avatar
Julian Salt committed
93
measure_theta(p::Furuta)                         = read(p.measure_theta)
Julian Salt's avatar
Julian Salt committed
94
theta(p::Furuta)                                      = (measure_theta(p) + 5.1763)*0.3091
Julian Salt's avatar
Julian Salt committed
95
measure_theta_dot(p::Furuta)                         = read(p.measure_theta_dot) 
Julian Salt's avatar
Julian Salt committed
96
theta_dot(p::Furuta)                                      = (measure_theta_dot(p) - 0.022)*3.76
Julian Salt's avatar
Julian Salt committed
97
measure_theta_precise(p::Furuta)                         = read(p.measure_theta_precise)
Julian Salt's avatar
Julian Salt committed
98
theta_precise(p::Furuta)                                      = (measure_theta_precise(p) + 0.7792)*0.058
Julian Salt's avatar
Julian Salt committed
99
measure_theta_dot_precise(p::Furuta)                         = read(p.measure_theta_dot_precise)
Julian Salt's avatar
Julian Salt committed
100
theta_dot_precise(p::Furuta)                                      = measure_theta_dot_precise(p)*0.68
Julian Salt's avatar
Julian Salt committed
101

Julian Salt's avatar
Julian Salt committed
102
103
104
105
106
107
measure(p::FurutaSimulator)                = p.s.sys.C*p.s.state

initialize(p::Furuta)                    = nothing
finalize(p::Furuta)             = foreach(close, p.stream.devices)
initialize(p::FurutaSimulator)           = nothing
finalize(p::FurutaSimulator)             = nothing