Update furuta.jl

src/interface_implementations/furuta.jl

@@ -87,18 +87,22 @@ 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 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
     if abs(a) < 0.5