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
+ 44
15
Compare changes
  • Side-by-side
  • Inline
@@ -18,12 +18,12 @@ 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
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
@@ -36,12 +36,12 @@ 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,
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),
@@ -50,8 +50,8 @@ function Furuta(;
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)
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
@@ -78,7 +78,8 @@ 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)
@@ -87,11 +88,39 @@ control(p::FurutaSimulator, u::Number) = p.s(u)
measure_phi(p::Furuta) = read(p.measure_phi)
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)
measure_theta_dot(p::Furuta) = read(p.measure_theta_dot)
measure_theta_precise(p::Furuta) = read(p.measure_theta_precise)
measure_theta_dot_precise(p::Furuta) = read(p.measure_theta_dot_precise)
function phi(p::Furuta)
a = rem(measure_phi(p)*2.56,2*pi)
if a > pi
a = a - 2*pi
elseif a < pi
a = a + 2*pi
end
return a
end
function theta(p::Furuta)
# a = (measure_theta(p) + 5.1763)*0.3091
a = (measure_theta(p) - 2.43)*0.627
#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