clc
clear all
close all

xr_robot=0;
yr_robot=0;
zr_robot=0;

xg_robot=0;
yg_robot=0;
zg_robot=0;

xb_robot=0;
yb_robot=0;
zb_robot=0;

%%%     INICIA CAMARA     %%%

cm_por_pixel = 25/1280;

video = videoinput('macvideo',1);
set(video,'ReturnedColorSpace','RGB');
preview(video)

% Medidas del robot

a1=10;
a2=8.5;
a3=10.2;

a=arduino('/dev/cu.wchusbserial1410');
a.servoAttach(9)    % theta 1
a.servoAttach(10)   % theta 2
a.servoAttach(11)   % theta 3
a.servoAttach(3)    % efector

% Posicion inicial
a.servoWrite(9,90);
a.servoWrite(10,70);
a.servoWrite(11,90);
a.servoWrite(3,180);
pause(1)

Stop=1;
uicontrol('Style','Pushbutton','String','Detener','Callback','Stop=0')

while Stop
    
%%%     PROCESAMIENTO DE IMAGEN     %%%

imagen = getsnapshot(video);

rojo = imagen(:,:,1);
verde = imagen(:,:,2);
azul = imagen(:,:,3);

resta1 = rojo-verde-azul;
resta1 = resta1;
resta2 = verde-azul-rojo;
resta2 = resta2*10;
resta3 = azul-verde-rojo;
resta3 = resta3*10;

%%%     ENCONTRAR PUNTO ROJO     %%%

suma_colr = sum(resta1);
num_colr = [1:1:1280];
mult_colr = suma_colr.*num_colr;
totalcr = sum(mult_colr);
location_colr = totalcr/sum(sum(resta1));

suma_rowr = sum(resta1'); 
num_rowr = [1:1:720];
mult_rowr = suma_rowr.*num_rowr;
totalrr = sum(mult_rowr);
location_rowr = totalrr/sum(sum(resta1));

%%%     ENCONTRAR PUNTO VERDE     %%%

suma_colg = sum(resta2);
num_colg = [1:1:1280];
mult_colg = suma_colg.*num_colg;
totalcg = sum(mult_colg);
location_colg = totalcg/sum(sum(resta2));

suma_rowg = sum(resta2'); 
num_rowg = [1:1:720];
mult_rowg = suma_rowg.*num_rowg;
totalrg = sum(mult_rowg);
location_rowg = totalrg/sum(sum(resta2));

%%%     ENCONTRAR PUNTO AZUL     %%%

suma_colb = sum(resta3);
num_colb = [1:1:1280];
mult_colb = suma_colb.*num_colb;
totalcb = sum(mult_colb);
location_colb = totalcb/sum(sum(resta3));

suma_rowb = sum(resta3'); 
num_rowb = [1:1:720];
mult_rowb = suma_rowb.*num_rowb;
totalrb = sum(mult_rowb);
location_rowb = totalrb/sum(sum(resta3));

%%%     PLOT PIEZAS     %%%

%figure
imshow(imagen)
hold on
plot(location_colr,location_rowr,'xw')
plot(location_colg,location_rowg,'ow')
plot(location_colb,location_rowb,'ow')
hold off


%%%     CINEMATICA INVERSA ROJO     %%%

if totalcr > 30000000 %Umbral de deteccion varia segun iluminacion
 
    xr_location = location_colr * cm_por_pixel;
    yr_location = location_rowr * cm_por_pixel;

    xr_robot = yr_location;
    yr_robot = xr_location;

    Dr=sqrt(xr_robot^2+yr_robot^2);
    Br=sqrt(Dr^2+zr_robot^2);
    alphar=atand(zr_robot/Dr);
    Cr=sqrt(a1^2+Br^2-2*a1*Br*cosd(90-alphar));
    phi1r=(acosd((a3^2-a2^2-Cr^2)/(-2*a2*Cr)));
    phi2r=(acosd((Br^2-a1^2-Cr^2)/(-2*a1*Cr)));
    phi3r=(acosd((Cr^2-a2^2-a3^2)/(-2*a2*a3)));

    T1r=round(atand(yr_robot/xr_robot))+90;
    T2r=round(180-phi1r-phi2r);
    T3r=round(phi3r-90);
    
    %Ajuste para evitar errores
    if isreal(T2r-5)
        T2r = T2r
    else 
        T2r = 90
    end
    
    if 173 < T3r
        T3r = 180 
    else 
        T3r = T3r    
    end
    
    % Ajuste de angulos y recoleccion de pieza
    a.servoWrite(9,T1r-6);
    pause(0.5)
    a.servoWrite(10,T2r-5);
    pause(0.5)
    a.servoWrite(11,T3r+13);
    pause(0.5)
    a.servoWrite(3,90);
    pause(0.5)
    
    % Posicion de entrega rojo
    a.servoWrite(10,47)
    pause(0.5)
    a.servoWrite(9,52)
    pause(0.5)
    a.servoWrite(11,5)
    pause(0.5)
    a.servoWrite(10,72)
    pause(0.5)
    a.servoWrite(3,180)
    pause(1)
    
    a.servoWrite(9,90);
    a.servoWrite(10,70);
    a.servoWrite(11,90);
    a.servoWrite(3,180);
    pause(1.5)
    

%%%     CINEMATICA INVERSA VERDE     %%%

elseif totalcg > 1000000 %Umbral de deteccion varia segun iluminacion
   
 
    xg_location = location_colg * cm_por_pixel;
    yg_location = location_rowg * cm_por_pixel;

    xg_robot = yg_location;
    yg_robot = xg_location;

    Dg=sqrt(xg_robot^2+yg_robot^2);
    Bg=sqrt(Dg^2+zg_robot^2);
    alphag=atand(zg_robot/Dg);
    Cg=sqrt(a1^2+Bg^2-2*a1*Bg*cosd(90-alphag));
    phi1g=(acosd((a3^2-a2^2-Cg^2)/(-2*a2*Cg)));
    phi2g=(acosd((Bg^2-a1^2-Cg^2)/(-2*a1*Cg)));
    phi3g=(acosd((Cg^2-a2^2-a3^2)/(-2*a2*a3)));
    
    
    T1g=round(atand(yg_robot/xg_robot))+90;
    T2g=round(180-phi1g-phi2g);
    T3g=round(phi3g-90);
    
    
     % Ajuste de angulos y recoleccion de pieza
    if isreal(T2g)
        T2g = T2g
    else 
        T2g = 90
    end
    
    if isreal(T3g)
        T3g = T3g
    else 
        T3g = 0
    end

    % Ajuste de angulos y recoleccion de pieza
    a.servoWrite(9,T1g-3);
    pause(0.5)
    a.servoWrite(10,T2g-5);
    pause(0.5)
    a.servoWrite(11,T3g+7);
    pause(0.5)
    a.servoWrite(3,90);
    pause(1)

    % Posicion de entrega verde
    a.servoWrite(10,47)
    pause(0.5)
    a.servoWrite(9,35)
    pause(0.5)
    a.servoWrite(11,15)
    a.servoWrite(10,68)
    pause(0.5)
    a.servoWrite(3,180)
    pause(1)
    
    a.servoWrite(9,90);
    a.servoWrite(10,70);
    a.servoWrite(11,90);
    a.servoWrite(3,180);
    pause(1.5)

 
%%%     CINEMATICA INVERSA AZUL     %%%

elseif totalcb > 30000000 %Umbral de deteccion varia segun iluminacion
    
    xb_location = location_colb * cm_por_pixel;
    yb_location = location_rowb * cm_por_pixel;

    xb_robot = yb_location;
    yb_robot = xb_location;

    Db=sqrt(xb_robot^2+yb_robot^2);
    Bb=sqrt(Db^2+zb_robot^2);
    alphab=atand(zb_robot/Db);
    Cb=sqrt(a1^2+Bb^2-2*a1*Bb*cosd(90-alphab));
    phi1b=(acosd((a3^2-a2^2-Cb^2)/(-2*a2*Cb)));
    phi2b=(acosd((Bb^2-a1^2-Cb^2)/(-2*a1*Cb)));
    phi3b=(acosd((Cb^2-a2^2-a3^2)/(-2*a2*a3)));

    T1b=round(atand(yb_robot/xb_robot))+90;
    T2b=round(180-phi1b-phi2b);
    T3b=round(phi3b-90);
    
    if isreal(T2b)
        T2b = T2b
    else 
        T2b = 90
    end
    
    if 180 > T3b > 0
        T3b = T3b
        
    elseif isreal(T3b+5)
        T3b=T3b
    else
        T3b = 90
    end
    
    % Ajuste de angulos y recoleccion de pieza
    a.servoWrite(9,T1b-3);
    pause(0.5)
    a.servoWrite(10,T2b-5);
    pause(0.5)
    a.servoWrite(11,T3b+8);
    pause(0.5)
    a.servoWrite(3,90);
    pause(1)
    
    % Posicion entrega azul
    a.servoWrite(10,60)
    pause(0.5)
    a.servoWrite(9,55)
    pause(0.5)
    a.servoWrite(11,38)
    a.servoWrite(10,75)
    pause(0.5)
    a.servoWrite(3,180)
    pause(1)
    
    a.servoWrite(9,90);
    a.servoWrite(10,70);
    a.servoWrite(11,90);
    a.servoWrite(3,180);
    pause(1.5)
 %}   
else

    % Posicion inicial
    a.servoWrite(9,90);
    a.servoWrite(10,70);
    a.servoWrite(11,90);
    a.servoWrite(3,180);
    pause(1)

end
   
end










