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.45;
b=0.3;
c=0.25;
R=0.3; % Radio de la llnata 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

%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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)

hx(1) = x1(1)-c*cos(phi(1)); % Posicion en el punto de control del robot en el eje x
hy(1) = y1(1)-c*sin(phi(1)); % Posicion en el punto de control del robot en el eje y

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

Wd1=0.5*ones(1,N);
Wd2=1.5*ones(1,N);
Wd3=0.5*ones(1,N);
Wd4=-1.5*ones(1,N);

uf = zeros(1,N); % velocidad lineal frontal (eje x)
ul = zeros(1,N); % velocidad lineal lateral (eje y)
w  = zeros(1,N);  % velocidad angular

% Matriz de transformación
T=(1/R)*[1 -1 -(a+b);...
     1  1  (a+b);...
     1  1  -(a+b);...
     1 -1  (a+b)];  
 
invT=pinv(T);


%%%%%%%%%%%%%%%%%%%%%%%%% BUCLE DE CONTROL %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=1:N
    
    Wd=[Wd1(k);Wd2(k);Wd3(k);Wd4(k)];

    qp=invT*Wd; % Transformar velocidades de cada motor a velocidades globales

    uf(k)=qp(1); % velocidad lineal frontal (eje x)
    ul(k)=qp(2); % velocidad lineal lateral (eje y)
    w(k)=qp(3);  % velocidad angular

   
    %%%%%%%%%%%%%%%%%%%%% MODELO CINEMATICO %%%%%%%%%%%%%%%%%%%%%%%%%
    x1p = uf(k)*cos(phi(k))-ul(k)*sin(phi(k));
    y1p = uf(k)*sin(phi(k))+ul(k)*cos(phi(k));
    
    % Integral numerica (metodo de Euler)
    x1(k+1)= x1(k)+ts*x1p;
    y1(k+1)= y1(k)+ts*y1p;
    phi(k+1)= phi(k)+ts*w(k);
    
    hx(k+1)=x1(k+1)-c*cos(phi(k+1)); 
    hy(k+1)=y1(k+1)-c*sin(phi(k+1)); 
    
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
axis equal; % Establece la relación de aspecto para que las unidades de datos sean las mismas en todas las direcciones.
axis([-5 5 -5 5 0 1]); % Ingresar limites minimos y maximos en los ejes x y z [minX maxX minY maxY minZ maxZ]
view([-40 30]); % Orientacion de la figura
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 ejes
camlight right % Luz para la escena

% b) Graficar robots en la posicion inicial
Dimension_Omni;
H1=Plot_Omni(x1(1),y1(1),phi(1));hold on;

% c) Graficar Trayectorias
H2=plot3(hx(1),hy(1),0,'r','lineWidth',2);

% d) Bucle de simulacion de movimiento del robot

step=10; % pasos para simulacion

for k=1:step:N

    delete(H1);    
    delete(H2);
    
    H1=Plot_Omni(x1(k),y1(k),phi(k));
    H2=plot3(hx(1:k),hy(1:k),zeros(1,k),'r','lineWidth',2);
    
    pause(ts);

end

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




