furuta.jl

# 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
    phi::Float64
    phi_dot::Float64
    theta::Float64
    theta_dot::Float64
    theta_precise::Float64
    theta_dot_precise::Float64
    stream::LabStream
    measure_phi::AnalogInput10V
    measure_phi_dot::AnalogInput10V
    measure_theta::AnalogInput10V
    measure_theta_dot::AnalogInput10V
    measure_theta_precise::AnalogInput10V
    measure_theta_dot_precise::AnalogInput10V
    control::AnalogOutput10V
end
function Furuta(;
    h::Float64               = 0.01,
    bias::Float64            = 0.,
    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,
    stream::LabStream        = ComediStream(),
    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),
    control::AnalogOutput10V = AnalogOutput10V(0))
    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)
    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)
    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
num_inputs(p::AbstractFuruta)             = 6
outputrange(p::AbstractFuruta)             = [(-10,10)]
inputrange(p::AbstractFuruta)             =  [(-10,10),(-10,10),(-10,10),(-10,10),(-10,10),(-10,10)]
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.")
    a::Float64 = -1.4*u[1]
    control(p,a)
end

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


measure_phi(p::Furuta)                         = read(p.measure_phi)
phi(p::Furuta)                                      = measure_phi(p)*2.56
measure_phi_dot(p::Furuta)                         = read(p.measure_phi_dot)
phi_dot(p::Furuta)                                      = (measure_phi_dot(p) + 0.0708)*2.0
measure_theta(p::Furuta)                         = read(p.measure_theta)
theta(p::Furuta)                                      = (measure_theta(p) + 5.1763)*0.3091
measure_theta_dot(p::Furuta)                         = read(p.measure_theta_dot) 
theta_dot(p::Furuta)                                      = (measure_theta_dot(p) - 0.022)*3.76
measure_theta_precise(p::Furuta)                         = read(p.measure_theta_precise)
theta_precise(p::Furuta)                                      = (measure_theta_precise(p) + 0.7792)*0.058
measure_theta_dot_precise(p::Furuta)                         = read(p.measure_theta_dot_precise)
theta_dot_precise(p::Furuta)                                      = measure_theta_dot_precise(p)*0.68

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