% Below, time is "normalized". You will definitely need
% to SLOW DOWN the actual motion, which cannot possibly
% be accomplished in 1 second!
t = 0:0.005:1;  % normalized units

%Target location
x0 = 1;  % 1 meter from start position, in x
y0 = 0;  % aligned with initial axis the laser points

%Generate the ellipse
LongWidth =  4*12*.0254;
% The x direction must avoid the "obstacle", but is otherwise 
% a width you may set as desired.
x = (LongWidth/4)*cos(2*pi*t-pi/2); % You may wish to adjust this
x=x-x(1);  % ensure we start at "x=0" when t=0
% The goal is to travel the distance "LongWidth" in y. However,
% you may wish to adjust the y trajectory below to "warp" the
% command to end in the desired location.
y = (LongWidth/2)*sin(2*pi*t-pi/2);
y=y-y(1);

%Calculate phi to orient robot toward target at all times
phi = atan2(y0-y, x0-x);

figure(501); clf
plot(x,y,'b--','LineWidth',2);
hold on;
plot(x0,y0,'ko','MarkerFaceColor','r')
plot(x(1),y(1),'ks','MarkerFaceColor',[0 .7 0])
plot(x(1),y(1)+LongWidth,'k>','MarkerFaceColor',[.9 .6 0])
plot(x(1)+[0 0 -1 -1 0]*12*.0254,...
    y(1) + [1 3 3 1 1]*12*.0254,'c-','LineWidth',3)
p2 = plot([x(1) x0],[y(1),y0],'r'); 
legend('trajectory','target (1 meter from start)','start (for robot body)',...
    'end (4 feet from start)','No-go zone','laser beam',...
    'Location','EastOutside')
axis image;
grid on
axis([-.5 1.2 -.5 1.7])
set(gca,'XTick',[-10:10]*(12*.0254),'YTick',[-10:10]*(12*.0254));
text(0.05,-.2,'Grid at 1-foot spacing')
title('Lab 5 Contest Schematic')
xlabel('x direction (meters)')
ylabel('y direction (meters)')

p1 = draw_omni_lab5([x(1); y(1); phi(1)]);
drawnow
for n=length(x)/3
    pause(.02);
    p1 = draw_omni_lab5([x(n); y(n); phi(n)],p1);
    set(p2,'XData',[x(n),x0],'YData',[y(n),y0]);
    drawnow
end
%p1 = draw_omni_lab5([x(end);y(end);phi(end)],p1);
%drawnow

figure(502); clf
subplot(3,1,1);
plot(t,x);
ylabel('X_b (m)'); xlabel('Time (normalized)'); grid on

subplot(3,1,2);
plot(t,y);
ylabel('Y_b (m)'); xlabel('Time (normalized)'); grid on

subplot(3,1,3);
plot(t, unwrap(phi)*180/pi);
ylabel('\phi_b (deg)'); xlabel('Time (normalized)'); grid on
