37 lines
1.7 KiB
Matlab
37 lines
1.7 KiB
Matlab
|
omni = omnidirectional(0.94, 0.96, 0.01, 2.2, 1.5,0.099,0.1,1.5,0.025,30,0.035,19,0.00011,1.69,0.0059,0.0059,0.9,9.1,6,'./');
|
||
|
|
|
||
|
|
A = kron(eye(3), [zeros(size(omni.origin.syscl.a)) eye(size(omni.origin.syscl.a)); zeros(size(omni.origin.syscl.a)) omni.origin.syscl.a]);
|
||
|
|
B = kron(eye(3),kron([0;1],omni.origin.syscl.b));
|
||
|
|
R = @(the) [[cos(deg2rad(the)) -sin(deg2rad(the)) 0; sin(deg2rad(the)) cos(deg2rad(the)) 0; [0 0 1];] zeros(size(omni.origin.syscl.a)); zeros(size(omni.origin.syscl.a)) zeros(size(omni.origin.syscl.a))];
|
||
|
|
C = @(the1,the2, the3) kron(diag([1 0 0]), R(the1)) + kron(diag([0 1 0]), R(the2)) + kron(diag([0 0 1]), R(the3));
|
||
|
|
fvref_cw = fncSpeedRef('cw',4,6,3);
|
||
|
|
fvref_ysin = fncSpeedRef('sysin',8,6,3);
|
||
|
|
fvref_xsin = fncSpeedRef('sxsin',8,6,3);
|
||
|
|
finput = @(t) [fvref_cw(t)+[0 0 90*sin(deg2rad(5*t))]'; fvref_xsin(t); fvref_ysin(t);]
|
||
|
|
|
||
|
|
sys = @(st, input) A*st + B* input;
|
||
|
|
dydt = @(t, y) sys(y, finput(t));
|
||
|
|
RX = @(x,y,the) x*cos(deg2rad(the))-y*sin(deg2rad(the));
|
||
|
|
RY = @(x,y,the) y*cos(deg2rad(the))+x*sin(deg2rad(the));
|
||
|
|
|
||
|
|
init = [0 0 0 0 0 0 5 7 45 0 0 0 -8 4 270 0 0 0]';
|
||
|
|
tSpan = 1:.1:50;
|
||
|
|
[t, y] = ode45(dydt,tSpan,init);
|
||
|
|
for i=1:size(t,1)
|
||
|
|
out(i,:) = C(y(i,3),y(i,9)+y(i,3),y(i,15)+y(i,3))*y(i,:)';
|
||
|
|
u(i,:) = finput(t(i))';
|
||
|
|
d1(i,1) = hypot(out(i,7)-out(i,1),out(i,8)-out(i,2));
|
||
|
|
d2(i,1) = hypot(out(i,13)-out(i,1),out(i,14)-out(i,2));
|
||
|
|
endfor
|
||
|
|
|
||
|
|
close all
|
||
|
|
figure(1)
|
||
|
|
plot(out(:,1), out(:,2), out(:,7), out(:,8), out(:,13), out(:,14));
|
||
|
|
figure(2)
|
||
|
|
plot(t(:,1), d1(:,1), t(:,1), d2(:,1));
|
||
|
|
figure(3)
|
||
|
|
plot(t(:,1), u(:,1), t(:,1), u(:,2), t(:,1), u(:,3));
|
||
|
|
% figure(4)
|
||
|
|
% plot(t(:,1), u(:,4), t(:,1), u(:,5),t(:,1), u(:,6));
|
||
|
|
% figure(5)
|
||
|
|
% plot(t(:,1), u(:,7), t(:,1), u(:,8),t(:,1), u(:,9));
|