function manga1_v5
%Versao 5

clear;
global ao ao1 dio_out dio_in;
initHardware;

setSpeed( 0 );
disp('Ligado');
pause;

%Determinaçao inicial do tamanho da imagem em uso
rgb_=vfm('grab',1);
[alturaImg larguraImg] = size(rgb_);

%-------------------------------

ganho=0.5;
comp=20;

yy1_xm =23;%47-comp
yy1_xM =37;%59-comp
yy2_xm =65;%47
yy2_xM =50;%59

%----------------------------------

manga_1=1;

while manga_1==1
%  tic

Vmotor=2.5;  %%%ISTO ha'-de ser mudado!!!!!!
setSpeed( Vmotor);    

passadeira1=test_passadeira(dio_in);

%%==============Aquisicao e binarizacao imagens
rgb_=vfm('grab',1);
rgb_1=(rgb_(:,:,1) >250);
rgb_2=(rgb_(:,:,2) >250);
% rgb_3=(rgb_(:,:,3) >250);
rgb_4 = rgb_1 & rgb_2;

img1=rgb_4(:,1:end/2);  %Imagem direita
img2=rgb_4(:,end/2:end); %Imagem esquerda

%===== INICIO ALGORITMO de GUIAMENTO

[x1 y1]=find(img1);
xx1=max(x1);  %ponto banco mais baixo da imagem direita

[x2 y2]=find(img2);
xx2=max(x2);  %idem esquerda


%****************************

if xx1 < (0.6 * alturaImg) continue  %Teste a 60% do topo da image
    
%=======================
%Ha' pixels Brancos abaixo dos 60 % da imagem

passadeira2=test_passadeira(dio_in);

%----- Determinar a largura da linha a 10 pixels acima do limite do branco
xx_M1=img1(xx1-10,:);
[x1_M y1_M]=find(xx_M1);

[yy1_m]=min(y1_M);
[yy1_M]=max(y1_M);
  
%************************
%% teste para ver se nao e' passadeira
if (yy1_M >= 1 & yy1_m >=1 ) & length(y1_M)<=15      
   Vref=(yy1_M-yy1_xM)*(-ganho/(yy1_xM-1))
   setDirection( Vref);
   continue
    
elseif (yy1_M~=[] & yy1_m~=[])
    yy1_M=yy1_m+10;
    Vref=(yy1_M-yy1_xM)*(-ganho/(yy1_xM-1))
    setDirection( Vref);
    continue
end
    
end

%%%%%%%%%%%% TESTE DA 2ª imagem (Esquerda)

if xx2 < 0.6 * alturaImg  continue % Se nao houver brancos abaixo dos 60% na img esq continua o ciclo
            
passadeira3=test_passadeira(dio_in)           

%----- Determinar a largura da linha a 10 pixels acima do limite do branco
xx_M2=img2(xx2-10,:);
[x2_M y2_M]=find(xx_M2);

[yy2_m]=min(y2_M);
[yy2_M]=max(y2_M);


%----------------------------
%Teste para ver se nao e' passadeira
if yy2_m>=1 & yy2_M>=1 & length(y2_M)<=15
    Vref=(yy2_m-yy2_xm)*(-ganho/(yy2_xm-1));
    setDirection( Vref );
           
elseif (yy2_M~=[] & yy2_m~=[]) % se e' passdeira (ou outra coisa estranha)
    yy2_m=yy2_M-10;
    Vref=(yy2_m-yy2_xm)*(-ganho/(yy2_xm-1));
    setDirection( Vref);
 end
end   
           
end % do ciclo while

%---------------------------------------

%   ENCERRAR COM OS CANAIS A ZERO.

setSpeed(0);

putvalue(dio_out, [0 0 0 0 0]);
stop(dio_out);
disp('concluido');

return

%================================

%   AUXILIARY FUNCTIONS

%==================================================
function emcima=test_passadeira(io_add)
global ao;
emcima = getvalue(io_add); 
if emcima == 1
    setSpeed(0);
    disp('+')
end

%======================================
function setSpeed( speed )
    global ao;
    putdata(ao, [speed]');
    start(ao);
    stop(ao);
end
%======================================
function setDirection( dir )
    global ao1;
    putdata(ao1, [dir]');
    start(ao1); 
    stop(ao1);
end

%==================================
function initHardware()

global ao ao1 dio_out dio_in;

ao=analogoutput('nidaq');
addchannel(ao, 0);
ch0=ao.Channel;

ao1=analogoutput('nidaq');
addchannel(ao1, 1);
ch1=ao1.Channel;

sampleRate=1000;

set(ao,'SampleRate',sampleRate);
set(ao1,'SampleRate',sampleRate);

set(ao, 'TriggerType', 'Immediate');
set(ao, 'RepeatOutput',0);

set(ao1, 'TriggerType', 'Immediate');
set(ao1, 'RepeatOutput', 0);

dio_out = digitalio('nidaq', 1);
addline(dio_out, 0:4, 0, 'Out');
putvalue(dio_out, [1 1 1 1 0]);

dio_in = digitalio('nidaq', 1);
addline(dio_in, 0, 1, 'In');

end

