Skip to content
Snippets Groups Projects

WIP: Started furuta implementation

Open Julian Salt requested to merge furuta into julia1
Compare and Show latest version
1 file
+ 61
8
Compare changes
  • Side-by-side
  • Inline
@@ -18,18 +18,41 @@ export Furuta, FurutaSimulator, AbstractFuruta
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::AnalogInput10V
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::AnalogInput10V = AnalogInput10V(1),
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),stream,measure,control)
init_devices!(p.stream, p.measure, p.control)
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
@@ -45,9 +68,9 @@ end
const AbstractFuruta = Union{Furuta, FurutaSimulator}
num_outputs(p::AbstractFuruta) = 1
num_inputs(p::AbstractFuruta) = 1
num_inputs(p::AbstractFuruta) = 6
outputrange(p::AbstractFuruta) = [(-10,10)]
inputrange(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
@@ -55,14 +78,44 @@ 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.")
control(p,u[1])
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(p::Furuta) = read(p.measure)
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
function theta(p::Furuta)
a = (measure_theta(p) + 5.1763)*0.3091
if abs(a) < 0.5
a = (measure_theta_precise(p) + 0.7792)*0.058
end
return a
end
function theta_dot(p::Furuta)
a = (measure_theta(p) + 5.1763)*0.3091
b = (measure_theta_dot(p) - 0.022)*3.76
if abs(a) < 0.5
b = measure_theta_dot_precise(p)*0.68
end
return b
end
measure(p::FurutaSimulator) = p.s.sys.C*p.s.state
initialize(p::Furuta) = nothing
Loading