128 lines
2.7 KiB
Matlab
128 lines
2.7 KiB
Matlab
clc
|
|
clear all
|
|
close all
|
|
|
|
global x0 ref dref b K saturation tc tfin
|
|
|
|
TRAJECTORY = 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
|
|
|
|
|
|
% 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];
|
|
|
|
|
|
% initial state
|
|
% In order, [x, y, theta]
|
|
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 = linspace(0, tfin);
|
|
% execute simulation
|
|
[t, x] = ode45(@sistema, tspan, x0);
|
|
|
|
% 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, :));
|
|
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)
|
|
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
|