WIP: Started furuta implementation

src/interface_implementations/furuta.jl

@@ -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