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,64 +28,100 @@ 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);
 
-% simulation time
+function [t, x, ref_t, U] = simulate_discr(tfin, tc)
-tspan = 0:0.1:60;
+    global ref x0 u_discr
-% execute simulation
-[t, x] = ode45(@sistema, tspan, x0);
 
-% recalc and save input at each timestep
+    steps = tfin/tc
-ts = size(t);
+    
-rows = ts(1);
+    x = x0';
-U = zeros(rows, 2);
+    t = 0;
-for row = 1:rows
+    u_discr = control_act(t, x0);
-    U(row, :) = control_act(t(row), x(row, :));
+    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
 
-% plot results
-ref_t = double(subs(ref, t'))';
 
-subplot(2,2,1)
+function [t, x, ref, U] = simulate_cont(tfin)
-hold on
+    global ref x0
-plot(ref_t(:, 1), ref_t(:, 2), "DisplayName", "Ref")
+
-plot(x(:, 1), x(:, 2), "DisplayName", "state")
+    % simulation time
-xlabel('x')
+    tspan = linspace(0, tfin);
-ylabel('y')
+    % execute simulation
-legend()
+    [t, x] = ode45(@sistema, tspan, x0);
-subplot(2,2,3)
+    
-plot(t, U(:, 1))
+    % recalc and save input at each timestep
-xlabel('t')
+    ts = size(t);
-ylabel('input v')
+    rows = ts(1);
-subplot(2,2,4)
+    U = zeros(rows, 2);
-plot(t, U(:, 2))
+    for row = 1:rows
-xlabel('t')
+        U(row, :) = control_act(t(row), x(row, :));
-ylabel('input w')
+    end
+    
+    % plot results
+    ref = double(subs(ref, t'))';
+end
+
+function plot_results(t, x, ref, U)
+    subplot(2,2,1)
+    hold on
+    plot(ref(:, 1), ref(:, 2), "DisplayName", "Ref")
+    plot(x(:, 1), x(:, 2), "DisplayName", "state")
+    xlabel('x')
+    ylabel('y')
+    legend()
+    subplot(2,2,3)
+    plot(t, U(:, 1))
+    xlabel('t')
+    ylabel('input v')
+    subplot(2,2,4)
+    plot(t, U(:, 2))
+    xlabel('t')
+    ylabel('input w')
     
     
-subplot(4,4,3)
+    subplot(4,4,3)
-hold on
+    hold on
-xlabel('t')
+    xlabel('t')
-ylabel('x')
+    ylabel('x')
-plot(t, ref_t(:, 1), "DisplayName", "X_{ref}");
+    plot(t, ref(:, 1), "DisplayName", "X_{ref}");
-plot(t, x(:, 1), "DisplayName", "X");
+    plot(t, x(:, 1), "DisplayName", "X");
-legend()
+    legend()
-hold off
+    hold off
     
-subplot(4,4,4)
+    subplot(4,4,4)
-plot(t, ref_t(:, 1) - x(:, 1));
+    plot(t, ref(:, 1) - x(:, 1));
-xlabel('t')
+    xlabel('t')
-ylabel('x error')
+    ylabel('x error')
     
-subplot(4,4,7)
+    subplot(4,4,7)
-hold on
+    hold on
-xlabel('t')
+    xlabel('t')
-ylabel('y')
+    ylabel('y')
-plot(t, ref_t(:, 2), "DisplayName", "Y_{ref}");
+    plot(t, ref(:, 2), "DisplayName", "Y_{ref}");
-plot(t, x(:, 2), "DisplayName", "Y");
+    plot(t, x(:, 2), "DisplayName", "Y");
-legend()
+    legend()
-hold off
+    hold off
     
-subplot(4,4,8)
+    subplot(4,4,8)
-plot(t, ref_t(:, 2) - x(:, 2));
+    plot(t, ref(:, 2) - x(:, 2));
-xlabel('t')
+    xlabel('t')
-ylabel('y error')
+    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