% simulazione di carellino a ruote differenziali (controllo su traiettoria pianificata)
% presenta un errore a causa del non corretto valutazione dell'errore
% angolare
clear all
clf

% dati carellino
R=0.05;     % raggio ruote
L=0.5;      % dirtanza tra le ruote

% posizione iniziale del robot (x, y, theta)
pos_iniziale=[0.5,1,0.3]';

% dati controllo
k_1=0.1;     % guadagno relativo allo spostaemnto in avanti
k_2=0.1;     % guadagno relativo all'agiustamento laterale
k_3=0.1;     % guadagno relativo al controllo della posizione angolare

% dati traiettoria
t_tot=500;      % tempo finale dell traiettoria
n=1000;          % tratti in cui dividere la traiettoria
inc_t=t_tot/n;  % step tempo


% disegno traiettoria desiderata in funzione del tempo

vet_pos_desiderata(1,1)=0;   % traiettoria a caso (x ed y)
vet_pos_desiderata(2,1)=0;
for cont=2:n
    vet_t(cont)=cont*inc_t;
    
    vet_pos_desiderata(1,cont)=4*sin(cont*pi/n);   % traiettoria a caso /x/
    vet_pos_desiderata(2,cont)=3*sin(2*cont*pi/n); %                    /y/
    
    % posizione angolare allineata secondo la traiettoria identificata
    % dalle due precedenti istruzioni       /theta/
    vet_pos_desiderata(3,cont)=atan2(vet_pos_desiderata(2,cont)-vet_pos_desiderata(2,cont-1),vet_pos_desiderata(1,cont)-vet_pos_desiderata(1,cont-1));                                                    % la posizione angoalre
                                                    % deriva di conseguenza
                                                    % (robot allineato secondo la traiettoria)
end
hold on
plot(vet_pos_desiderata(1,:),vet_pos_desiderata(2,:))

% implementazione del sistema di controllo
pos_robot=pos_iniziale;
for cont2=1:n
    
    vet_pos_robot(:,cont2)=pos_robot;
    % posizione angolare del robot
    theta=pos_robot(3);
    
    
    
    % calcolo errore 
    errore=vet_pos_desiderata(:,cont2)-pos_robot;
    %vet_errore(cont2)=errore(3);
    
    % correzioen dell'errore quando si è vicini ai 2*pi radiani
    if errore(3)>pi
        errore(3)=errore(3)-2*pi;    
    end
    
    
    % errore nel sistema di riferimento del robot
    mat_rotazione=[  cos(theta),     sin(theta),            0;...
                     -sin(theta),    cos(theta),            0;...
                                0,              0,          1];
                            
    errore_relativo_al_robot=mat_rotazione*errore;
    
    % azione di controllo
    vel_traslazione=k_1*errore_relativo_al_robot(1);
    
    vel_rotazione=k_2*errore_relativo_al_robot(2)+k_3*errore_relativo_al_robot(3);
    
    % a questo punto le velocità di traslazione e rotazione devono essere 
    % trasformate in felocità di rotazione delle ruote, ma tale azione
    % riguarda il robot reale, non la simulazione
    
    % simulazione
    % spostamento e rotazione simulato
    spostamento=vel_traslazione*inc_t;
    delta_rotazione=vel_rotazione*inc_t;
    
    pos_robot=pos_robot+inv(mat_rotazione)*[spostamento,0,delta_rotazione]';
    
    
end

plot(vet_pos_robot(1,:),vet_pos_robot(2,:),'r.')