162 lines
3.5 KiB
Matlab
162 lines
3.5 KiB
Matlab
clc
|
|
clear all
|
|
close all
|
|
|
|
%% global variables
|
|
global q0 ref dref b tc K SATURATION tc tfin USE_PREDICTION PREDICTION_HORIZON PREDICTION_SATURATION_TOLERANCE;
|
|
|
|
%% variables
|
|
TRAJECTORY = 6
|
|
INITIAL_CONDITIONS = 1
|
|
USE_PREDICTION = false
|
|
PREDICTION_HORIZON = 5
|
|
% 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
|
|
|
|
tc = 0.1
|
|
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; 1];
|
|
PREDICTION_SATURATION_TOLERANCE = 0.0;
|
|
|
|
%% launch simulation
|
|
% initial state
|
|
% In order, [x, y, theta]
|
|
q0 = set_initial_conditions(INITIAL_CONDITIONS)
|
|
% trajectory to track
|
|
[ref, dref] = set_trajectory(TRAJECTORY)
|
|
|
|
global tu uu
|
|
|
|
figure(1)
|
|
PREDICTION_HORIZON = 0;
|
|
[t, q, ref_t, U] = simulate_discr(tfin);
|
|
plot_results(t, q, ref_t, U);
|
|
|
|
figure(2)
|
|
PREDICTION_HORIZON = 1;
|
|
[t1, q1, ref_t1, U1] = simulate_discr(tfin);
|
|
plot_results(t1, q1, ref_t1, U1);
|
|
|
|
figure(3)
|
|
PREDICTION_HORIZON = 2;
|
|
[t2, q2, ref_t2, U2] = simulate_discr(tfin);
|
|
plot_results(t2, q2, ref_t2, U2);
|
|
|
|
%figure(3)
|
|
%subplot(1, 2, 1)
|
|
%plot(tu, uu(1, :))
|
|
%subplot(1, 2, 2)
|
|
%plot(tu, uu(2, :))
|
|
%plot_results(t, x-x1, ref_t-ref_t1, U-U1);
|
|
|
|
|
|
|
|
%% FUNCTION DECLARATIONS
|
|
|
|
% Discrete-time simulation
|
|
function [t, q, ref_t, U] = simulate_discr(tfin)
|
|
global ref q0 u_discr tc
|
|
|
|
steps = tfin/tc
|
|
|
|
q = q0';
|
|
t = 0;
|
|
u_discr = control_act(t, q0);
|
|
U = u_discr';
|
|
|
|
for n = 1:steps
|
|
tspan = [(n-1)*tc n*tc];
|
|
z0 = q(end, :);
|
|
|
|
[v, z] = ode45(@sistema, tspan, z0);
|
|
|
|
q = [q; z];
|
|
t = [t; v];
|
|
|
|
u_discr = control_act(t(end), q(end, :));
|
|
U = [U; ones(length(v), 1)*u_discr'];
|
|
end
|
|
|
|
ref_t = double(subs(ref, t'))';
|
|
end
|
|
|
|
|
|
% Continuos-time simulation
|
|
function [t, q, ref, U] = simulate_cont(tfin)
|
|
global ref q0
|
|
|
|
% simulation time
|
|
tspan = linspace(0, tfin);
|
|
% execute simulation
|
|
[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), q(row, :));
|
|
end
|
|
|
|
% plot results
|
|
ref = double(subs(ref, t'))';
|
|
end
|
|
|
|
% Plots
|
|
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)
|
|
hold on
|
|
xlabel('t')
|
|
ylabel('x')
|
|
plot(t, ref(:, 1), "DisplayName", "X_{ref}");
|
|
plot(t, x(:, 1), "DisplayName", "X");
|
|
legend()
|
|
hold off
|
|
|
|
subplot(4,4,4)
|
|
plot(t, ref(:, 1) - x(:, 1));
|
|
xlabel('t')
|
|
ylabel('x error')
|
|
|
|
subplot(4,4,7)
|
|
hold on
|
|
xlabel('t')
|
|
ylabel('y')
|
|
plot(t, ref(:, 2), "DisplayName", "Y_{ref}");
|
|
plot(t, x(:, 2), "DisplayName", "Y");
|
|
legend()
|
|
hold off
|
|
|
|
subplot(4,4,8)
|
|
plot(t, ref(:, 2) - x(:, 2));
|
|
xlabel('t')
|
|
ylabel('y error')
|
|
end
|