From 10fa0382a6fe06932779d72aaa929ff80b5b1ba4 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker@tutanota.com>
Date: Fri, 12 Jul 2024 19:13:29 +0200
Subject: [PATCH] initial simulations

model a unicycle with exact i/o linearization. Plot the desired trajectory on each axis and the one took by the robot
---
 set_initial_conditions.m |  8 +++++++
 set_trajectory.m         | 20 ++++++++++++++++
 sistema.m                | 40 ++++++++++++++++++++++++++++++++
 tesiema.m                | 49 ++++++++++++++++++++++++++++++++++++++++
 4 files changed, 117 insertions(+)
 create mode 100644 set_initial_conditions.m
 create mode 100644 set_trajectory.m
 create mode 100644 sistema.m
 create mode 100644 tesiema.m

diff --git a/set_initial_conditions.m b/set_initial_conditions.m
new file mode 100644
index 0000000..0a2e21f
--- /dev/null
+++ b/set_initial_conditions.m
@@ -0,0 +1,8 @@
+function x0 = set_initial_conditions(i)
+switch i
+    case 0
+        x0 = [0; 0; 0]
+    case 1
+        x0 = [0; 0; PI]
+end
+end
\ No newline at end of file
diff --git a/set_trajectory.m b/set_trajectory.m
new file mode 100644
index 0000000..5411e3e
--- /dev/null
+++ b/set_trajectory.m
@@ -0,0 +1,20 @@
+function [ref, dref] = set_trajectory(i)
+syms s
+
+switch i
+    case 0   
+        % a straigth line trajectory at v=0.5m/s
+        xref = 0.5*s;
+        yref = 0;
+    case 1
+        % a straigth line trajectory at v=10 m/s
+        xref = 10*s;
+        yref = 0;
+    case 2
+        xref = 5*cos(s);
+        yref = 5*sin(s);
+end
+
+ref = [xref; yref];
+dref = diff(ref, s);
+end
\ No newline at end of file
diff --git a/sistema.m b/sistema.m
new file mode 100644
index 0000000..d07bed6
--- /dev/null
+++ b/sistema.m
@@ -0,0 +1,40 @@
+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);
+end
+
+function dx = unicycle(t, x, u)
+    % u is (v;w)
+    % x is (x; y; theta)
+    theta = x(3);
+    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
+
+
diff --git a/tesiema.m b/tesiema.m
new file mode 100644
index 0000000..58566e4
--- /dev/null
+++ b/tesiema.m
@@ -0,0 +1,49 @@
+clc
+clear all
+close all
+
+global x0 ref dref b K saturation
+
+TRAJECTORY = 0
+INITIAL_CONDITIONS = 0
+% distance from the center of the unicycle to the point being tracked
+% ATTENZIONE! CI SARA' SEMPRE UN ERRORE COSTANTE DOVUTO A b. Minore b,
+% minore l'errore
+b = 0.2
+% proportional gain
+K = eye(2)*2
+% saturation
+saturation = [5; 0.2];
+
+
+% initial state
+% In order, [x, y, theta]
+x0 = set_initial_conditions(INITIAL_CONDITIONS)
+% trajectory to track
+[ref, dref] = set_trajectory(TRAJECTORY)
+
+
+% simulation time
+tspan = 0:0.1:10;
+% execute simulation
+
+[t, x] = ode45(@sistema, tspan, x0)
+
+% plot results
+ref_t = reshape(double(subs(ref, t)), [101,2]);
+
+subplot(3,2,1)
+hold on
+plot(t, ref_t(:, 1));
+plot(t, x(:, 1));
+legend()
+subplot(3,2,2)
+plot(t, ref_t(:, 1) - x(:, 1));
+subplot(3,2,3)
+hold on
+plot(t, ref_t(:, 2));
+plot(t, x(:, 2));
+legend()
+subplot(3,2,4)
+plot(t, ref_t(:, 2) - x(:, 2));
+