%% Kinematyka robota mobilnego z uwzgl�dnieniem kinematyki ko�a
clear all; clc; close all;

%% Parametry symulacji
dt=0.1; % krok ca�kowania
ts=50; % czas symulacji
t=0:dt:ts; % time span

%% Parametry fizyczne robota mobilnego (w zale�no�ci od promienia ko�a, jak i rozmiaru ramy, zale�e� b�dzie promie� skr�tu i inne parametry ruchu)
a=5; % promie� ko�a
d_w=20; % odleg�o�� ko�a od �rodka robota (wzd�u� osi y) - dla ko�a r�nicowego
l_w=30; % odleg�o�� ko�a od �rodka robota - dla ko�a wielokierunkowego

%% Warunki pocz�tkowe
x0=50.;
y0=50.;
psi0=0;

eta0=[x0;y0;psi0];

eta(: , 1) = eta0;

%% czasy jazdy                 
t1=1; % obr�t
t2=12; % czas jazdy do g�ry
t3=13; % obr�t
t4=18; % czas jazdy w bok
t5=19; % obr�t
t6=28; % czas jazdy w d�
t7=29; % obr�t
t8=33; % czas jazdy w bok
t9=34; % obr�t
t10=35; % czas jazdy do g�ry
t11=36; % obr�t
t12=46; % czas jazdy w bok
t13=47; % obr�t
t14=50; % czas jazdy w d�

%% Start p�tli

for i=1:length(t)
    
    act_t=i*dt;

    psi = eta(3,i); %aktualna orientacja w rad
    
    %Jakobian macierzy:
    J_psi=[cos(psi),-sin(psi),0;
           sin(psi),cos(psi),0;
           0,0,1];

       
    %% Nap�d r�nicowy
      
    if (act_t<=t1)
        omega_1=-(d_w*pi/2)*2*pi/(2*pi*a);
        omega_2=(d_w*pi/2)*2*pi/(2*pi*a);
    elseif (act_t>t1) && (act_t<=t2) 
        omega_1=(330/(2*pi*a))*2*pi/(t2-t1);
        omega_2=(330/(2*pi*a))*2*pi/(t2-t1);
    elseif (act_t>t2) && (act_t<=t3) 
        omega_1=(d_w*pi/2)*2*pi/(2*pi*a)*(t3-t2);
        omega_2=-(d_w*pi/2)*2*pi/(2*pi*a)*(t3-t2);
    elseif (act_t>t3) && (act_t<=t4)
        omega_1=(150/(2*pi*a))*2*pi/(t4-t3);
        omega_2=(150/(2*pi*a))*2*pi/(t4-t3);
    elseif (act_t>t4) && (act_t<=t5)
        omega_1=(d_w*pi/2)*2*pi/(2*pi*a)*(t5-t4);
        omega_2=-(d_w*pi/2)*2*pi/(2*pi*a)*(t5-t4);
    elseif (act_t>t5) && (act_t<=t6)
       omega_1=(270/(2*pi*a))*2*pi/(t6-t5);
        omega_2=(270/(2*pi*a))*2*pi/(t6-t5);
    elseif (act_t>t6) && (act_t<=t7)
       omega_1=-(d_w*pi/2)*2*pi/(2*pi*a)*(t7-t6);
        omega_2=(d_w*pi/2)*2*pi/(2*pi*a)*(t7-t6);
    elseif (act_t>t7) && (act_t<=t8)
      omega_1=(120/(2*pi*a))*2*pi/(t8-t7);
        omega_2=(120/(2*pi*a))*2*pi/(t8-t7);
    elseif (act_t>t8) && (act_t<=t9)
      omega_1=-(d_w*pi/2)*2*pi/(2*pi*a)*(t9-t8);
        omega_2=(d_w*pi/2)*2*pi/(2*pi*a)*(t9-t8);
   elseif (act_t>t9) && (act_t<=t10)
       omega_1=(30/(2*pi*a))*2*pi/(t10-t9);
        omega_2=(30/(2*pi*a))*2*pi/(t10-t9);
   elseif (act_t>t10) && (act_t<=t11)
       omega_1=(d_w*pi/2)*2*pi/(2*pi*a)*(t11-t10);
        omega_2=-(d_w*pi/2)*2*pi/(2*pi*a)*(t11-t10);
     elseif (act_t>t11) && (act_t<=t12)
        omega_1=(130/(2*pi*a))*2*pi/(t12-t11);
        omega_2=(130/(2*pi*a))*2*pi/(t12-t11);
    elseif (act_t>t12) && (act_t<=t13)
       omega_1=(d_w*pi/2)*2*pi/(2*pi*a)*(t13-t12);
        omega_2=-(d_w*pi/2)*2*pi/(2*pi*a)*(t13-t12);
    elseif (act_t>t13) && (act_t<=t14)
        omega_1=(90/(2*pi*a))*2*pi/(t14-t13);
        omega_2=(90/(2*pi*a))*2*pi/(t14-t13);
    end


    omega=[omega_1; omega_2];

    % Macierz konfiguracji ko�a 

    W=[a/2 a/2;
        0 0;
        -a/(2*d_w) a/(2*d_w)];

    
    %% Wektor pr�dko�ci wej�ciowych
    zeta(:,i)=W*omega;
       
    eta_dot(:,i) = J_psi*zeta(:,i); % Pochodna czasowa po wsp�rz�dnych uog�lnionych
       
    eta(:,i+1) = eta(:,i)+dt*eta_dot(:,i); % metoda Eulera
end

%% Wydruk funkcji
% figure
% plot(t, eta(1,1:i),'r-');
% set(gca,'fontsize',12)
% xlabel('t[s]')
% ylabel('x[m]')
% 
% figure
% plot(t, eta(2,1:i),'b-');
% set(gca,'fontsize',12)
% xlabel('t[s]')
% ylabel('y[m]')
% 
% figure
% plot(t, eta(3,1:i),'g-');
% set(gca,'fontsize',12)
% xlabel('t[s]')
% ylabel('\psi[rad]')

%% Animacja ruchu robota mobilnego
l=2*l_w; % d�ugo�� platformy
w=2*d_w; % szeroko�� platformy - wa�na w przypadku rozpatrywania kinematyki ko�a

% Wsp�rz�dne opisuj�ce kszta� platformy
mr_co=[-l/2,l/2,l/2,-l/2,-l/2;
       -w/2,-w/2,w/2,w/2,-w/2];

figure
for i=1:5:length(t) %pocz�tek animacji

  %% Mapa
    x1 = [120 140 140 120];
    y1 = [0 0 330 330];
    fill(x1,y1,'r')
    hold on;

    x2 = [240 260 260 240];
    y2 = [135 135 380 380];
    fill(x2,y2,'r')
    hold on;

    x3 = [240 260 260 240];
    y3 = [445 445 500 500];
    fill(x3,y3,'r')
    hold on;

    x4 = [360 380 380 360];
    y4 = [0 0 95 95];
    fill(x4,y4,'r')
    hold on;

    x5 = [340 500 500 340];
    y5 = [285 285 305 305];
    fill(x5,y5,'r')
    hold on;

    plot(50,50,'b')
    hold on;

    plot(50,50,'.','MarkerSize',24,'MarkerEdge','b');
    hold on;

    plot(450,50,'.','MarkerSize',24,'MarkerEdge','b');
    hold on;

    text(30,30,'1',FontSize=20);
    hold on;

    text(470,30,'2',FontSize=20);
    hold on;

    psi= eta(3,i);
    R_psi=[cos(psi), -sin(psi);
           sin(psi),cos(psi)]; % macierz rotacji
    v_pos=R_psi*mr_co;
    
    fill(v_pos(1,:)+eta(1,i), v_pos(2,:)+eta(2,i),'g')
    hold on, grid on; axis([0 500 0 500]), axis square
    
    plot(eta(1,1:i),eta(2,1:i),'b-');
    legend('MR','Path'), set(gca,'fontsize',12)
    xlabel('x[m]'); ylabel('y[m]');
    pause(0.01);
    hold off
end % koniec animacji