rename state x to q

control_act.m

@@ -1,14 +1,14 @@
-function u = control_act(t, x)
+function u = control_act(t, q)
     global ref dref K b SATURATION PREDICTION_SATURATION_TOLERANCE USE_PREDICTION PREDICTION_STEPS
 
     ref_s = double(subs(ref, t));
     dref_s = double(subs(dref, t));
 
     
-    err = ref_s - feedback(x);
+    err = ref_s - feedback(q);
     u_track = dref_s + K*err;
     
-    theta = x(3);
+    theta = q(3);
 
     T_inv = [cos(theta), sin(theta); -sin(theta)/b, cos(theta)/b];
     
@@ -19,7 +19,7 @@ function u = control_act(t, x)
         % quadprog solves the problem in the form
         % min 1/2 x'Hx +f'x
         % where x is u_corr. Since u_corr is (v_corr; w_corr), and I want
-        % to minimize u'u (norm squared of the function itself) H must be
+        % to minimize u'u (norm squared of u_corr itself) H must be
         % the identity matrix of size 2
         H = eye(2)*2;
         % no linear of constant terms, so 
@@ -61,9 +61,9 @@ function u = control_act(t, x)
     u = min(SATURATION, max(-SATURATION, u));
 end
 
-function x_track = feedback(x)
+function q_track = feedback(q)
     global b
-    x_track = [ x(1) + b*cos(x(3)); x(2) + b*sin(x(3))  ];
+    q_track = [ q(1) + b*cos(q(3)); q(2) + b*sin(q(3))  ];
 end
 
 

set_trajectory.m

@@ -25,8 +25,11 @@ switch i
         xref = 15*cos(s);
         yref = 15*sin(s);
     case 6
-        xref = 5*cos(0.15*s);
-        yref = 5*sin(0.15*s);
+        xref = 0.4*s; 
+        yref = cos(0.4*s);
+    case 7
+        xref = 5*cos(0.05*s);
+        yref = 5*sin(0.05*s);
 end
 
 ref = [xref; yref];

sistema.m

@@ -1,3 +1,3 @@
-function x = sistema(t, x)
-    x = unicycle(t, x, control_act(t, x));
+function q = sistema(t, q)
+    q = unicycle(t, q, control_act(t, q));
 end

sistema_discr.m

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

tesiema.m

@@ -3,7 +3,7 @@ clear all
 close all
 
 %% global variables
-global x0 ref dref b K SATURATION tc tfin USE_PREDICTION PREDICTION_STEP PREDICTION_SATURATION_TOLERANCE;
+global q0 ref dref b K SATURATION tc tfin USE_PREDICTION PREDICTION_STEP PREDICTION_SATURATION_TOLERANCE;
 
 %% variables
 TRAJECTORY = 6
@@ -17,34 +17,34 @@ b = 0.2
 % proportional gain
 K = eye(2)*2
 
-tfin=10
+tfin=30
 
 % saturation
 % 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];
-PREDICTION_SATURATION_TOLERANCE = 0.1;
+SATURATION = [1; 1];
+PREDICTION_SATURATION_TOLERANCE = 0.0;
 
 %% launch simulation
 % initial state
 % In order, [x, y, theta]
-x0 = set_initial_conditions(INITIAL_CONDITIONS)
+q0 = set_initial_conditions(INITIAL_CONDITIONS)
 % trajectory to track
 [ref, dref] = set_trajectory(TRAJECTORY)
 
 global tu uu
 
-%figure(1)
-%USE_PREDICTION = false;
-%[t, x, ref_t, U] = simulate_discr(tfin, 0.05);
-%plot_results(t, x, ref_t, U);
+figure(1)
+USE_PREDICTION = false;
+[t, q, ref_t, U] = simulate_discr(tfin, 0.1);
+plot_results(t, q, ref_t, U);
 
 figure(2)
 USE_PREDICTION = true;
-[t1, x1, ref_t1, U1] = simulate_discr(tfin, 0.05);
-plot_results(t1, x1, ref_t1, U1);
+[t1, q1, ref_t1, U1] = simulate_discr(tfin, 0.1);
+plot_results(t1, q1, ref_t1, U1);
 
 figure(3)
 subplot(1, 2, 1)
@@ -58,26 +58,26 @@ plot(tu, uu(2, :))
 %% FUNCTION DECLARATIONS
 
 % Discrete-time simulation
-function [t, x, ref_t, U] = simulate_discr(tfin, tc)
-    global ref x0 u_discr
+function [t, q, ref_t, U] = simulate_discr(tfin, tc)
+    global ref q0 u_discr
 
     steps = tfin/tc
     
-    x = x0';
+    q = q0';
     t = 0;
-    u_discr = control_act(t, x0);
+    u_discr = control_act(t, q0);
     U = u_discr';
     
     for n = 1:steps
         tspan = [(n-1)*tc n*tc];
-        z0 = x(end, :);
+        z0 = q(end, :);
         
         [v, z] = ode45(@sistema, tspan, z0);
 
-        x = [x; z];
+        q = [q; z];
         t = [t; v];
         
-        u_discr = control_act(t(end), x(end, :));
+        u_discr = control_act(t(end), q(end, :));
         U = [U; ones(length(v), 1)*u_discr'];
     end
 
@@ -86,20 +86,20 @@ end
 
 
 % Continuos-time simulation
-function [t, x, ref, U] = simulate_cont(tfin)
-    global ref x0
+function [t, q, ref, U] = simulate_cont(tfin)
+    global ref q0
 
     % simulation time
     tspan = linspace(0, tfin);
     % execute simulation
-    [t, x] = ode45(@sistema, tspan, x0);
+    [t, q] = ode45(@sistema, tspan, q0);
     
     % recalc and save input at each timestep
     ts = size(t);
     rows = ts(1);
     U = zeros(rows, 2);
     for row = 1:rows
-        U(row, :) = control_act(t(row), x(row, :));
+        U(row, :) = control_act(t(row), q(row, :));
     end
     
     % plot results