From e0f22a402bba5c04aeb72ef76b5570c970acd27f Mon Sep 17 00:00:00 2001
From: Julian Salt <juliansaltducaju@gmail.com>
Date: Mon, 26 Aug 2019 14:32:57 +0200
Subject: [PATCH] Update furuta.jl
---
src/interface_implementations/furuta.jl | 14 +++++++++-----
1 file changed, 9 insertions(+), 5 deletions(-)
diff --git a/src/interface_implementations/furuta.jl b/src/interface_implementations/furuta.jl
index fb98742..4f2e62c 100644
--- a/src/interface_implementations/furuta.jl
+++ b/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
--
GitLab