simulate system in discrete time

control_act.m

@@ -1,4 +1,5 @@
 function u = control_act(t, x)
+
     global ref dref K b saturation
 
     ref_s = double(subs(ref, t));
@@ -12,7 +13,7 @@ function u = control_act(t, x)
 
     T_inv = [cos(theta), sin(theta); -sin(theta)/b, cos(theta)/b];
 
-    u = T_inv * u_nom;
+    u = T_inv * ( u_nom );
 
     % saturation
     u = min(saturation, max(-saturation, u));

sistema.m

@@ -1,11 +1,3 @@
 function x = sistema(t, x)
     x = unicycle(t, x, control_act(t, x));
 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

sistema_discr.m

@@ -0,0 +1,4 @@
+function x = sistema_discr(t, x)
+    global u_discr
+    x = unicycle(t, x, u_discr);
+end

tesiema.m

@@ -2,18 +2,24 @@ clc
 clear all
 close all
 
-global x0 ref dref b K saturation
+global x0 ref dref b K saturation tc tfin
 
-TRAJECTORY = 1
+TRAJECTORY = 0
-INITIAL_CONDITIONS = 0
+INITIAL_CONDITIONS = 1
 % 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.5
+K = eye(2)*2
+
+
 % saturation
-saturation = [5; 0.2];
+% HYP: a diff. drive robot with motors spinning at 100rpm -> 15.7 rad/s.
+% Radius of wheels 10cm. Wheels distanced 15cm from each other
+% applying transformation, v
+% saturation = [1.57, 20];
+saturation = [1.57; 20];
 
 
 % initial state
@@ -22,9 +28,42 @@ x0 = set_initial_conditions(INITIAL_CONDITIONS)
 % trajectory to track
 [ref, dref] = set_trajectory(TRAJECTORY)
 
+[t, x, ref_t, U] = simulate_discr(60, 0.1);
+plot_results(t, x, ref_t, U);
+
+function [t, x, ref_t, U] = simulate_discr(tfin, tc)
+    global ref x0 u_discr
+
+    steps = tfin/tc
+    
+    x = x0';
+    t = 0;
+    u_discr = control_act(t, x0);
+    U = u_discr';
+
+    
+    for n = 1:steps
+        tspan = [(n-1)*tc n*tc];
+        z0 = x(end, :);
+        
+        [v, z] = ode45(@sistema, tspan, z0);
+
+        x = [x; z];
+        t = [t; v];
+        
+        u_discr = control_act(t(end, :), x(end, :));
+        U = [U; ones(length(v), 1)*u_discr'];
+    end
+
+    ref_t = double(subs(ref, t'))';
+end
+
+
+function [t, x, ref, U] = simulate_cont(tfin)
+    global ref x0
 
     % simulation time
-tspan = 0:0.1:60;
+    tspan = linspace(0, tfin);
     % execute simulation
     [t, x] = ode45(@sistema, tspan, x0);
     
@@ -37,11 +76,13 @@ for row = 1:rows
     end
     
     % plot results
-ref_t = double(subs(ref, t'))';
+    ref = double(subs(ref, t'))';
+end
 
+function plot_results(t, x, ref, U)
     subplot(2,2,1)
     hold on
-plot(ref_t(:, 1), ref_t(:, 2), "DisplayName", "Ref")
+    plot(ref(:, 1), ref(:, 2), "DisplayName", "Ref")
     plot(x(:, 1), x(:, 2), "DisplayName", "state")
     xlabel('x')
     ylabel('y')
@@ -60,13 +101,13 @@ subplot(4,4,3)
     hold on
     xlabel('t')
     ylabel('x')
-plot(t, ref_t(:, 1), "DisplayName", "X_{ref}");
+    plot(t, ref(:, 1), "DisplayName", "X_{ref}");
     plot(t, x(:, 1), "DisplayName", "X");
     legend()
     hold off
     
     subplot(4,4,4)
-plot(t, ref_t(:, 1) - x(:, 1));
+    plot(t, ref(:, 1) - x(:, 1));
     xlabel('t')
     ylabel('x error')
     
@@ -74,12 +115,13 @@ subplot(4,4,7)
     hold on
     xlabel('t')
     ylabel('y')
-plot(t, ref_t(:, 2), "DisplayName", "Y_{ref}");
+    plot(t, ref(:, 2), "DisplayName", "Y_{ref}");
     plot(t, x(:, 2), "DisplayName", "Y");
     legend()
     hold off
     
     subplot(4,4,8)
-plot(t, ref_t(:, 2) - x(:, 2));
+    plot(t, ref(:, 2) - x(:, 2));
     xlabel('t')
     ylabel('y error')
+end

unicycle.m

@@ -0,0 +1,7 @@
+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