clear
close all
clc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% TIEMPO %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tf = 40;             % Tiempo de simulacion en segundos (s)
ts = 0.1;            % Tiempo de muestreo en segundos (s)
t = 0: ts: tf;       % Vector de tiempo
N = length(t);       % Muestras

%%%%%%%%%%%%%%%%%%%%%%%% PARAMETROS DEL ROBOT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a=0.2;   % distancia hacia la base del manipulador
b=0.25;  % altura de la plataforma móvil
c=0.25;  % altura de la base del manipulador
l1=0.5;  % longitud del eslabon 1 en metros (m)
l2=0.55; % longitud del eslabon 2 en metros (m)

%%%%%%%%%%%%%%%%%%%%%%%% CONDICIONES INICIALES %%%%%%%%%%%%%%%%%%%%%%%%%%%%
x1 = zeros(1, N+1);  % Posicion en el centro del robot (eje x) en metros (m)
y1 = zeros(1, N+1);  % Posicion en el centro del robot (eje y) en metros (m)
phi = zeros(1, N+1); % Orientacion del robot en radianes (rad)

x1(1)=0; % Posicion inicial eje x
y1(1)=0; % Posicion inicial eje y
phi(1)=0; % Orientacion inicial del robot

q1 = zeros(1, N+1); % Posición angular de la base del manipulador
q2 = zeros(1, N+1); % Posición angular del eslabón del manipulador

q1(1) =  10*(pi/180);  % Posición angular inicial q1           
q2(1) =  0*(pi/180);   % Posición angular inicial q2  

%%%%%%%%%%%%%%%%%%%%%%%%%%%% PUNTO DE CONTROL %%%%%%%%%%%%%%%%%%%%%%%%%%%%
hx = zeros(1, N+1);  % Posicion en el punto de control (eje x) en metros (m)
hy = zeros(1, N+1);  % Posicion en el punto de control (eje y) en metros (m)
hz = zeros(1, N+1);  % Posicion en el punto de control (eje z) en metros (m)

% Robot diferencial
xp = zeros(1, N+1);  % Posicion de la base del manipulador (eje x) en metros (m)
yp = zeros(1, N+1);  % Posicion de la base del manipulador (eje y) en metros (m)

xp(1)=x1(1)+a*cos(phi(1));
yp(1)=y1(1)+a*sin(phi(1));

hx(1)=xp(1)+l1*cos(q1(1))*cos(phi(1))+l2*cos(q1(1)+q2(1))*cos(phi(1));   
hy(1)=yp(1)+l1*cos(q1(1))*sin(phi(1))+l2*cos(q1(1)+q2(1))*sin(phi(1));   
hz(1)=b+c+l1*sin(q1(1))+l2*sin(q1(1)+q2(1)); 


%%%%%%%%%%%%%%%%%%%%%% VELOCIDADES DE REFERENCIA %%%%%%%%%%%%%%%%%%%%%%%%%%

u = 0.15*ones(1,size(t,2));  % velocidad lineal (m/s)
w = 0.1*ones(1,size(t,2));   % velocidad angular (rad/s)
q1p = 0.1*sin(0.3*t);  % Velocidad angular del eslabón 1 del manipulador (rad/s)
q2p = 0.1*cos(0.2*t);  % Velocidad angular del eslabón 2 del manipulador (rad/s)



%%%%%%%%%%%%%%%%%%%%%%%%% BUCLE DE SIMULACION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for k=1:N

 
        %%%%%%%%%%%%%%%%%%%%% MODELO CINEMATICO %%%%%%%%%%%%%%%%%%%%%%%%%

        % Jacobiano
        J11 = cos(phi(k));
        J12 = -a*sin(phi(k))-l1*cos(q1(k))*sin(phi(k))-l2*cos(q1(k)+q2(k))*sin(phi(k));
        J13 = -l1*sin(q1(k))*cos(phi(k))-l2*sin(q1(k)+q2(k))*cos(phi(k));
        J14 = -l2*sin(q1(k)+q2(k))*cos(phi(k));
                
        J21 = sin(phi(k));
        J22 = a*cos(phi(k))+l1*cos(q1(k))*cos(phi(k))+l2*cos(q1(k)+q2(k))*cos(phi(k));
        J23 = -l1*sin(q1(k))*sin(phi(k))-l2*sin(q1(k)+q2(k))*sin(phi(k));
        J24 = -l2*sin(q1(k)+q2(k))*sin(phi(k));
        
        J31 = 0;
        J32 = 0;
        J33 = l1*cos(q1(k))+l2*cos(q1(k)+q2(k));
        J34 = l2*cos(q1(k)+q2(k));
        
        

        J=[J11 J12 J13 J14;...
           J21 J22 J23 J24;...
           J31 J32 J33 J34];

        qp = [u(k) w(k) q1p(k) q2p(k)]'; % vector de acciones de control
        
        hp = J*qp;
   
        phi(k+1)=phi(k)+ts*w(k); 
        x1p = u(k)*cos(phi(k+1)); 
        y1p = u(k)*sin(phi(k+1)); 
        
        x1(k+1)=x1(k)+ts*x1p;  
        y1(k+1)=y1(k)+ts*y1p; 
        
        xp(k+1)=x1(k+1)+a*cos(phi(k+1));
        yp(k+1)=y1(k+1)+a*sin(phi(k+1));

        q1(k+1)=q1(k)+ts*q1p(k);  
        q2(k+1)=q2(k)+ts*q2p(k);       

        hx(k+1)=hx(k)+ts*hp(1); 
        hy(k+1)=hy(k)+ts*hp(2);
        hz(k+1)=hz(k)+ts*hp(3); 
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SIMULACION VIRTUAL 3D %%%%%%%%%%%%%%%%%%%%%%%%%%%%

% a) Configuracion de escena

scene=figure;  % Crear figura (Escena)
set(scene,'Color','white'); % Color del fondo de la escena
set(gca,'FontWeight','bold') ;% Negrilla en los ejes y etiquetas
sizeScreen=get(0,'ScreenSize'); % Retorna el tamaño de la pantalla del computador
set(scene,'position',sizeScreen); % Congigurar tamaño de la figura
camlight('headlight'); % Luz para la escena
axis equal; % Establece la relación de aspecto para que las unidades de datos sean las mismas en todas las direcciones.
grid on; % Mostrar líneas de cuadrícula en los ejes
box on; % Mostrar contorno de ejes
xlabel('x(m)'); ylabel('y(m)'); zlabel('z(m)'); % Etiqueta de los eje

view([-40 30]); % Orientacion de la figura
axis([-5 5 -5 5 0 2]); % Ingresar limites minimos y maximos en los ejes x y z [minX maxX minY maxY minZ maxZ]

% b) Graficar robots en la posicion inicial
MobileRobot;  
H1=MobilePlot(x1(1),y1(1),phi(1));hold on;
Arm_Parameters(1); % Parameters of arm robot
H2=Arm_Plot3D(xp(1),yp(1),b,0,0,phi(1),q1(1),q2(1),0);

% c) Graficar Trayectorias
H3=plot3(hx(1),hy(1),hz(1),'b','LineWidth',2); 

% d) Bucle de simulacion de movimiento del robot

step=10; % pasos para simulacion

for k=1:step:N
    
     delete (H1) 
     delete (H2) 
     delete (H3)
     
     H1=MobilePlot(x1(k),y1(k),phi(k)); hold on; 
     H2=plot3(hx(1:k),hy(1:k),hz(1:k),'b','LineWidth',2); 
     H3=Arm_Plot3D(xp(k),yp(k),b,0,0,phi(k),q1(k),q2(k),0);
     
    pause(ts)
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Graficas %%%%%%%%%%%%%%%%%%%%%%%%%%%%
graph=figure;  % Crear figura (Escena)
title("Plataforma movil")
set(graph,'position',sizeScreen); % Congigurar tamaño de la figura
subplot(211)
plot(t,u,'b','LineWidth',2),grid('on'),xlabel('Tiempo [s]'),ylabel('[m/s]'),legend('u');
subplot(212)
plot(t,w,'r','LineWidth',2),grid('on'),xlabel('Tiempo [s]'),ylabel('[rad/s]'),legend('w');

graph1=figure;  % Crear figura (Escena)
title("Manipulador")
set(graph1,'position',sizeScreen); % Congigurar tamaño de la figura
plot(t,q1p,'b','LineWidth',2),grid('on'),xlabel('Tiempo [s]'),ylabel('[m/s]'),hold on;
plot(t,q2p,'r','LineWidth',2),grid('on'),xlabel('Tiempo [s]'),ylabel('[m/s]'),legend('q1p','q2p');
