WIP: Started furuta implementation

src/interface_implementations/furuta.jl

@@ -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)
@@ -86,11 +87,34 @@ 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
+
+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