WIP: Started furuta implementation

src/interface_implementations/furuta.jl

@@ -104,19 +104,20 @@ function phi(p::Furuta)
     return a
 end
 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
+   # 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
+    #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
+    #if abs(a) < 0.5
+     #   b = measure_theta_dot_precise(p)*0.68
+    #end
     return b
 end