41 lines
773 B
Matlab
41 lines
773 B
Matlab
|
function [x, u_] = sistema(t, x)
|
||
|
|
global ref dref K b saturation U tc
|
||
|
|
|
||
|
|
ref_s = double(subs(ref, t));
|
||
|
|
dref_s = double(subs(dref, t));
|
||
|
|
|
||
|
|
|
||
|
|
err = ref_s - feedback(x);
|
||
|
|
u_nom = dref_s + K*err;
|
||
|
|
|
||
|
|
theta = x(3);
|
||
|
|
|
||
|
|
T_inv = [cos(theta), sin(theta); -sin(theta)/b, cos(theta)/b];
|
||
|
|
|
||
|
|
u = T_inv * u_nom;
|
||
|
|
|
||
|
|
% saturation
|
||
|
|
u = min(saturation, max(-saturation, u));
|
||
|
|
|
||
|
|
i = int8(1.5 + t/tc);
|
||
|
|
% save input
|
||
|
|
U(i, :) = reshape(u, [1, 2]);
|
||
|
|
|
||
|
|
x = unicycle(t, x, u);
|
||
|
|
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
|
||
|
|
|
||
|
|
function x_track = feedback(x)
|
||
|
|
global b
|
||
|
|
x_track = [ x(1) + b*cos(x(3)); x(2) + b*sin(x(3)) ];
|
||
|
|
end
|
||
|
|
|
||
|
|
|