EmaMaker
/
thesis
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

isolate control in its own function

master
EmaMaker 2024-07-12 19:14:53 +02:00
parent 10fa0382a6
commit 6d84703c62
2 changed files with 28 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
control_act.m Normal file
View File

@ -0,0 +1,26 @@
function u = control_act(t, x)
global ref dref K b saturation
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));
end
function x_track = feedback(x)
global b
x_track = [ x(1) + b*cos(x(3)); x(2) + b*sin(x(3)) ];
end

33
sistema.m
View File

@ -1,27 +1,5 @@
function [x, u_] = sistema(t, x) function x = sistema(t, x)
global ref dref K b saturation U tc x = unicycle(t, x, control_act(t, x));
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 end
function dx = unicycle(t, x, u) function dx = unicycle(t, x, u)
@ -31,10 +9,3 @@ function dx = unicycle(t, x, u)
G_x = [cos(theta), 0; sin(theta), 0; 0, 1]; G_x = [cos(theta), 0; sin(theta), 0; 0, 1];
dx = G_x*u; dx = G_x*u;
end end
function x_track = feedback(x)
global b
x_track = [ x(1) + b*cos(x(3)); x(2) + b*sin(x(3)) ];
end