%Equações da Direção

maxdir=0.192;
mindir=-0.192;
vel=linspace(0, 100, 100);
%vel=60;

% secure_dist=2*vel;
secure_dist=0.015*vel+0.6;

plot(vel, secure_dist, 'r')
legend('Secure Dist')
xlabel('Velocidade (0~100)');
ylabel('Distância (m)');



min_dist=0.5;
x=linspace(0,secure_dist,100);
% x=linspace(0,100,100);

dir1=-(maxdir/(secure_dist-min_dist)).*x+(maxdir*secure_dist/(secure_dist-min_dist));
dir2=(maxdir*secure_dist/(secure_dist-min_dist))*cos(x/(secure_dist/(pi/2)));
dir3=(maxdir/(secure_dist-min_dist)).*x-(maxdir*secure_dist/(secure_dist-min_dist));



plot(x,dir1,'b')

hold on;
plot(x,dir2,'g')
plot(x,dir3,'r')
plot(x,-dir2,'g')

legend('dir1', 'dir2', 'dir3')
xlabel('Distância (m)');
ylabel('Direcção (Radianos)');

axis([0 2 -0.4 0.4]);
%[X, Y]=meshgrid(x, secure_dist, dir1);