From 97b9c56b6fe5a4e66c2f3dc8fdafb30fe805139e Mon Sep 17 00:00:00 2001 From: Julian Salt <juliansaltducaju@gmail.com> Date: Mon, 26 Aug 2019 14:09:04 +0200 Subject: [PATCH] Update furuta.jl --- src/interface_implementations/furuta.jl | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/src/interface_implementations/furuta.jl b/src/interface_implementations/furuta.jl index 2f2ee62..fb98742 100644 --- a/src/interface_implementations/furuta.jl +++ b/src/interface_implementations/furuta.jl @@ -91,13 +91,30 @@ 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 +#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 +#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 +#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 +#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 -- GitLab