% Regulation complete
% On donne un une position finale qui sert a reguler l'orientation du
% chariot et sa vitesse

close all
clear

attr=['r-'; 'g-'; 'b-'; 'c-'; 'm-'; 'k-'];
Tmax=100;

%parametre du robot 
l=0.2; %axe inter-roue

%Conditions initiales
x=50; 
y=20; 
teta=0; 

%Consigne
teta_tmp = 30*pi/180 ; %30degres
x_consigne=100*cos(teta_tmp); 
y_consigne=100*sin(teta_tmp); 

%Configuration du controleur PI de la vitesse (a completer)
ki_uc=0; % TODO
kp_uc=0; % TODO

%Configuration du controleur PI de l'angle (a completer)
alpha=0; % TODO
omega=0; % TODO
ksi=0; % TODO
ki_teta=omega*omega/alpha;
kp_teta=2*ksi*omega/alpha;

%Lancer la simulation
[time, out]=sim('robot_v5', Tmax);

%Recuperer les position des roues pour le graphique
L=20*l; % pour le graphique
point_g(:,1)=out(:,1)-L*cos(pi/2-out(:,3))/2; 
point_d(:,1)=out(:,1)+L*cos(pi/2-out(:,3))/2; 
point_g(:,2)=out(:,2)+L*sin(pi/2-out(:,3))/2; 
point_d(:,2)=out(:,2)-L*sin(pi/2-out(:,3))/2; 

figure
hold on
grid on
axis xy
axis('square')
xlabel('x')
ylabel('y')

plot(out(:,1), out(:,2), 'g.-')
plot(x_consigne,y_consigne, 'rd')
   
%Dessin du charriot
for i=1:length(time)
    plot([point_g(i, 1) point_d(i, 1)]', [point_g(i, 2) point_d(i, 2)]', 'b-')
end
