%Regulation complete
%On donne un une position finale qui sert à 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
x0=50; 
y0=20; 
teta0=0; 

%Consigne
teta_final= 30*pi/180 ; %30degres
x_final=100*cos(teta_final); 
y_final=100*sin(teta_final); 

%Configuration du controleur PI de la vitesse
ki_uc=0.1/100000;
%ki_uc=0;
kp_uc=5/100;

%Configuration du controleur PI de l'angle 
alpha=1/l;
omega=1;
ksi=1;
ki_teta=omega*omega/alpha;
kp_teta=2*ksi*omega/alpha;

%reduire la vitesse quand on fait une correction d'orientation
k=0;

%Lancer la simulation
[time, out]=sim('robot_v4', 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_final,y_final, '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