diff --git a/control_act.m b/control_act.m new file mode 100644 index 0000000..9df21fc --- /dev/null +++ b/control_act.m @@ -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 + + diff --git a/sistema.m b/sistema.m index d07bed6..7d2b285 100644 --- a/sistema.m +++ b/sistema.m @@ -1,27 +1,5 @@ -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); +function x = sistema(t, x) + x = unicycle(t, x, control_act(t, x)); end 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]; 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 - -