From 6d84703c629cb2f54d9ed999b64726c574582f65 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@tutanota.com>
Date: Fri, 12 Jul 2024 19:14:53 +0200
Subject: [PATCH] isolate control in its own function

---
 control_act.m | 26 ++++++++++++++++++++++++++
 sistema.m     | 33 ++-------------------------------
 2 files changed, 28 insertions(+), 31 deletions(-)
 create mode 100644 control_act.m

diff --git a/control_act.m b/control_act.m
new file mode 100644
index 0000000..9df21fc
--- /dev/null
+++ b/control_act.m
@@ -0,0 +1,26 @@
+function u = control_act(t, x)
+    global ref dref K b saturation
+
+    ref_s = double(subs(ref, t));
+    dref_s = double(subs(dref, t));
+
+    
+    err = ref_s - feedback(x);
+    u_nom = dref_s + K*err;
+    
+    theta = x(3);
+
+    T_inv = [cos(theta), sin(theta); -sin(theta)/b, cos(theta)/b];
+
+    u = T_inv * u_nom;
+
+    % saturation
+    u = min(saturation, max(-saturation, u));
+end
+
+function x_track = feedback(x)
+    global b
+    x_track = [ x(1) + b*cos(x(3)); x(2) + b*sin(x(3))  ];
+end
+
+
diff --git a/sistema.m b/sistema.m
index d07bed6..7d2b285 100644
--- a/sistema.m
+++ b/sistema.m
@@ -1,27 +1,5 @@
-function [x, u_] = sistema(t, x)
-    global ref dref K b saturation U tc
-
-    ref_s = double(subs(ref, t));
-    dref_s = double(subs(dref, t));
-
-    
-    err = ref_s - feedback(x);
-    u_nom = dref_s + K*err;
-    
-    theta = x(3);
-
-    T_inv = [cos(theta), sin(theta); -sin(theta)/b, cos(theta)/b];
-
-    u = T_inv * u_nom;
-
-    % saturation
-    u = min(saturation, max(-saturation, u));
-    
-    i = int8(1.5 + t/tc);
-    % save input
-    U(i, :) = reshape(u, [1, 2]);
-
-    x = unicycle(t, x, u);
+function x = sistema(t, x)
+    x = unicycle(t, x, control_act(t, x));
 end
 
 function dx = unicycle(t, x, u)
@@ -31,10 +9,3 @@ function dx = unicycle(t, x, u)
     G_x = [cos(theta), 0; sin(theta), 0; 0, 1];
     dx = G_x*u;
 end
-
-function x_track = feedback(x)
-    global b
-    x_track = [ x(1) + b*cos(x(3)); x(2) + b*sin(x(3))  ];
-end
-
-