function manga3_v2
%Versao 3
%11 Maio 2002 -

warning off;
clear;

global ai ao ao1 dio_out dio_in;
global img1 img2 Kd;
global yy1_xM yy1_xm yy2_xm yy2_xM;
global manga_1 ganho Vmotor desvio_ant;
global linhaPassadeira linhaSW1 linhaSW2 linhaSW3 linhaSW4;
global tempo_ultima_passagem total_passagens_passadeira TEMPO_VALIDAR_PASSAGENS

linhaPassadeira = 1;
linhaSW1 = 2; linhaSW2 = 3; linhaSW3 = 4; linhaSW4 = 5; 

total_passagens_passadeira=0;
TEMPO_VALIDAR_PASSAGENS = 8;   %8 segundo...
initHardware;

stop_motor;
disp('Ligado');
% pause;

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


while 1 %«««««««««««««««««««««« CICLO ENVOLVENTE »»»»»»»»»»»»»»»»»»»»»»»»

total_passagens_passadeira=0;
  
%************************************************
while 1
if getvalue( dio_in.Line(linhaSW2)) & ~getvalue( dio_in.Line(linhaSW1)) %& ~getvalue( dio_in.Line(linhaSW3)) &~getvalue( dio_in.Line(linhaSW4));
      
      TURBO1=getvalue( dio_in.Line(linhaSW3));
      TURBO2=getvalue( dio_in.Line(linhaSW4));
      
      if TURBO1 &~TURBO2
%         comp=-15;
%         yy1_xm =34+comp;
%         yy1_xM =49+comp;
%         yy2_xm =36+comp;
%         yy2_xM =49+comp;
        
        Vmotor=2.5;
%         ganho=0.6;
%         Kd=-0.012;
        disp('Vmotor = 2.5');
%        setSpeed( Vmotor );         % Selecao da velocidade
        break
      end

      if TURBO2 &~TURBO1
%         comp=-15;
%         yy1_xm =34+comp;
%         yy1_xM =49+comp;
%         yy2_xm =36+comp;
%         yy2_xM =49+comp;
%         
        Vmotor=3;
%         ganho=0.58;
%         Kd=-0.012;
        disp('Vmotor = 3');
%         setSpeed( Vmotor );
        break
      end

      if TURBO1 & TURBO2
%         comp=-15;
%         yy1_xm =34+comp;
%         yy1_xM =49+comp;
%         yy2_xm =36+comp;
%         yy2_xM =49+comp;
%           
        Vmotor=1.5;
%         ganho=0.57;
%         Kd=-0.012;
        disp('Vmotor = 1.5');
%         setSpeed( Vmotor );
        break
      end
end
end
%********************************************** 
  
  
%**********************************************
%Ciclo de espera pela ordem de arranque
%
while 1
  if  getvalue( dio_in.Line(linhaSW1) ) 
      enable_motor(dio_out,1) 
      break
   end
end
%*********************************************

disp('Ja comecei...')

clear_flip_flop( dio_out);

pause(0.5) % Para ganhar tempo e poder voltar a interpretar o SW1 noutro contexto
tic;
tempo_ultima_passagem = toc;


while 1 %»»»»»»»»»»»»»»»»%re-INICIO ap´os paragem na passadeira»»»»»»»»»»»»»»»»»»»»»


reset_ciclo=getvalue(dio_in.Line(linhaSW1));    
if reset_ciclo  %***************************************************
    break       %* Começar de novo!!
end             %*

ESCOLHA=semaforo(total_passagens_passadeira); %/////////////ESCOLHA DA OPERAÇ~AO A EFECTUAR\\\\\\\\\\\\\\\

switch ESCOLHA
    case '001', %parar
        disp('parar')
        stop_motor;
%******************************************************************************     
    case '010', %segue em frente
        disp('segue em frente')
        if Vmotor==2.5
            comp=-13;
            yy1_xm =44+comp;
            yy1_xM =57+comp;
            yy2_xm =24+comp;
            yy2_xM =38+comp;
            Vmotor=2.5;
            ganho=0.75;
            Kd=-0.012;
            setSpeed( Vmotor );
            enable_motor(dio_out,1)
        end
        
        if Vmotor==3
            comp=-13;
            yy1_xm =44+comp;
            yy1_xM =57+comp;
            yy2_xm =24+comp;
            yy2_xM =38+comp;
            Vmotor=3;
            ganho=0.7;
            Kd=-0.015;
            setSpeed( Vmotor );
            enable_motor(dio_out,1)
        end
         
        if Vmotor==1.5
            comp=-13;
            yy1_xm =44+comp;
            yy1_xM =57+comp;
            yy2_xm =24+comp;
            yy2_xM =38+comp;
            Vmotor=1.5;
            ganho=0.88;
            Kd=-0.012;
            setSpeed( Vmotor );
            enable_motor(dio_out,1)
        end
%         tic;
%         tempo_ultima_passagem = toc;
        while 1
            if getvalue(dio_in.Line(linhaSW1))
                break;
            end
            
            [em_tunel v_esq v_dir] = test_tunel;
            
            if( em_tunel )
                newDir = calc_dir_tunel(v_esq,v_dir);
                if newDir ~= -100
                    setDirection(newDir);
                end
                clear_flip_flop( dio_out)
                %disp('*')
            else            

                disp('p1')
                
                em_cima= test_passadeira(dio_in);
                
                disp('p2')
                %______________________________
                if em_cima
                    if test_passagem_valida
                        stop_motor
                        break
                    end
                end         
                 disp('p3')
               %______________________________
                [xx1, xx2,dif1,dif2]=Calc_img;
                %______________________________
                                disp('p4')

                em_cima=test_passadeira(dio_in);
                                disp('p5')

                %Testa passadeira outra vez...
                if em_cima
                    if test_passagem_valida
                        stop_motor
                        break
                    end
                end
                                disp('p6')

                %______________________________
                go_frente(xx1,xx2,dif1,dif2,alturaImg)   
                                disp('p7')

                %disp('f')
            end %Fim da alternativa ao tunel
            %disp('.')
        end  % Do while dentro do case de seguir em frente
        
 %******************************************************************************       
        case '100', %segue pela esquerda
        disp('segue pela esquerda')
        if Vmotor==2.5
            comp=+13;
            yy1_xm =44+comp;
            yy1_xM =57+comp;
            yy2_xm =24+comp;
            yy2_xM =38+comp;
            Vmotor=2.5;
            ganho=0.78;
            Kd=-0.012;
            setSpeed( Vmotor );
            disp('vmotor a 2.5')
            enable_motor(dio_out,1)
        end
        
        if Vmotor==3
            comp=+13;
            yy1_xm =44+comp;
            yy1_xM =57+comp;
            yy2_xm =24+comp;
            yy2_xM =38+comp;
            Vmotor=3;
            ganho=0.75;
            Kd=-0.012;
            setSpeed( Vmotor );
            enable_motor(dio_out,1)
        end
        
        if Vmotor==1.5
            comp=+13;
            yy1_xm =44+comp;
            yy1_xM =57+comp;
            yy2_xm =24+comp;
            yy2_xM =38+comp;
            Vmotor=1.5;
            ganho=0.9;
            Kd=-0.01;
            setSpeed( Vmotor );
            enable_motor(dio_out,1)
        end

        while 1
            
            if getvalue(dio_in.Line(linhaSW1))
                break;
            end
            
            [em_tunel v_esq v_dir] = test_tunel;
            
            if( em_tunel )
                newDir = calc_dir_tunel(v_esq,v_dir);
               if newDir ~= -100
                    setDirection(newDir);
               end
               clear_flip_flop( dio_out)
               %disp('*')
           else
                 %______________________________
                em_cima=test_passadeira(dio_in);
                        
                if em_cima
                    if test_passagem_valida
                        stop_motor
                        break
                    end
                end
                %______________________________
                [xx1, xx2,dif1,dif2]=Calc_img;
                %______________________________
                %Testa passadeira outra vez...
                em_cima=test_passadeira(dio_in);
                
                if em_cima
                    if test_passagem_valida
                        stop_motor
                        break
                    end
                end
                %______________________________        
                go_esquerda(xx1,xx2,dif1,dif2,alturaImg)  
            end
        end
 %******************************************************************************         
        case '111', % fim de passagem
        disp('fim de passagem')
        setSpeed(0);
 %****************************************************************************** 
        case '110', %segue para o parque
        disp('segue para o parque')
        setSpeed(1);
        enable_motor(dio_out,1)
        pause(4)
        stop_motor;
        Vref=2;
        setDirection( Vref);
        setSpeed(1);
        pause(1.4)
        Vref=0;
        setDirection( Vref);
        setSpeed(1);
        pause(2);
        stop_motor;
        break            

    end  %end do switch-case

    disp('Fim ciclo')
end %»»»»»»»»»»»FIM DA 2ªMANGA»»»»»»»»»»»»»»»»»»

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

%   ENCERRAR COM OS CANAIS A ZERO.

stop_motor;

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

end  %«««««««««««««««««««««« CICLO ENVOLVENTE »»»»»»»»»»»»»»»»»»»»»»»»

return




%================================
%================================
%   AUXILIARY FUNCTIONS

%==================================================
function [emcima]=test_passadeira(io_add)
    global linhaPassadeira linhaSW1 linhaSW2 linhaSW3 linhaSW4;
    emcima = getvalue(io_add.Line(linhaPassadeira));
    return
    
%======================================
function setSpeed( speed )
global ao;
    putdata(ao, [speed]');
    start(ao);
    stop(ao);
    return
%======================================
function setDirection( dir )
    global ao1;
    putdata(ao1, [dir]');
    start(ao1); 
    stop(ao1);
    return
%==================================
function initHardware()

global ai ao ao1 dio_out dio_in;


%================= DIGITAL =============
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, [0 0 0 0 0]);
%pause(0.01)
dio_in = digitalio('nidaq', 1);
addline(dio_in, 0:7, 1, 'In');

%======================== ANALOG==================
ai = analoginput('nidaq', 1); %Abrir dispositivo entradas analógico 1
ai.InputType = 'SingleEnded'; %Impor entradas não-diferenciais
ai.SampleRate = 5000;        %Ajustar frequência de amostragem
%ai.TriggerRepeat = Inf;       %Trigger contínuo (sempre a ler)
ai.SamplesPerTrigger = 5;
addchannel(ai,[0 1 2],{'Ch0', 'Sens1', 'Sens2'});
start(ai)         %Iniciar o processo de aquisição/escrita analógica

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

return


%************************************
function [xx1,xx2,xx1_min,xx2_min]=Calc_img()
global img1 img2;
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
xx1_min=min(x1);
dif1=xx1-xx1_min;

[x2 y2]=find(img2);
xx2=max(x2);  %idem esquerda
xx2_min=min(x2);
dif2=xx2-xx2_min;
return

%************************************
function [ret,Vref]=calc_traj1(xx1,xx1_min)
global img1 img2 Kd;
global yy1_xM ganho Vmotor desvio_ant;
%----- Determinar a largura da linha a 20 pixels acima do limite do branco
xx_M1=img1(xx1-20,:);
[x1_M y1_M]=find(xx_M1);


[yy1_m]=min(y1_M);
[yy1_M]=max(y1_M);

desvio_inst=(yy1_M-yy1_xM);
   

%************************
%% teste para ver se nao e' passadeira
if (yy1_M >= 1 & yy1_m >=1 ) & length(y1_M)<=15 %& dif1>=38     
   var_desv=(desvio_inst-desvio_ant);
   Vref=desvio_inst*(-ganho/(yy1_xM-1))+Kd*var_desv;
   desvio_ant=(yy1_M-yy1_xM);
   setDirection( Vref);
   ret=1;
   return;
    
elseif yy1_M~=[] & yy1_m~=[] %& dif1>=40 %27 deste modo o robot mantem a trajectoria
    yy1_M=yy1_m+15; 
    var_desv=((yy1_M-yy1_xM)-desvio_ant);                      % que tinha ao entrar na interseccao e           
    Vref=(yy1_M-yy1_xM)*(-ganho/(yy1_xM-1))+Kd*var_desv;     % ignora a linha que vai encontrar!!!
    desvio_ant=(yy1_M-yy1_xM);    
    setDirection( Vref);
    ret=1;
    return;
    
elseif length(y1_M)>15 & xx1_min>=120
    return;
end
ret = 0;

return

%***************************************
function [ret2,Vref]=calc_traj2(xx2,xx2_min)
global img1 img2 Kd;
global yy2_xM yy2_xm ganho Vmotor desvio_ant;
%----- Determinar a largura da linha a 20 pixels acima do limite do branco
xx_M2=img2(xx2-20,:);
[x2_M y2_M]=find(xx_M2);

[yy2_m]=min(y2_M);
[yy2_M]=max(y2_M);
desvio_inst=(yy2_m-yy2_xm);
 
%----------------------------
%Teste para ver se nao e' passadeira
if yy2_m>=1 & yy2_M>=1 & length(y2_M)<=15 %& dif2>=38
    var_desv=(desvio_inst-desvio_ant);
    Vref=desvio_inst*(-ganho/(yy2_xm-1))+Kd*var_desv;
    desvio_ant=(yy2_m-yy2_xm);
    setDirection( Vref );
    ret2=1;
    return;
    
elseif yy2_M~=[] & yy2_m~=[] %& dif2>=40% 27 se e' passdeira (ou outra coisa estranha)
    yy2_m=yy2_M-15;
    var_desv=((yy2_m-yy2_xm)-desvio_ant);
    Vref=(yy2_m-yy2_xm)*(-ganho/(yy2_xm-1))+Kd*var_desv;
    desvio_ant=(yy2_m-yy2_xm);
    setDirection( Vref);
    ret2=1;
    return;
 
elseif length(y2_M)>15 & xx2_min>=120
    return
    
end 

return

%===================================0
function clear_flip_flop( dio_out )
        putvalue(dio_out, [1 0 0 0 0]);
        putvalue(dio_out, [1 1 0 0 0]);
return

%*************************************
function enable_motor(dio_out,state)

if state==0
    putvalue(dio_out.Line(3),0);
else 
    putvalue(dio_out.Line(3),1);
end            
return    
%*************************************

function ESCOLHA=semaforo( count)
global dio_in

espera_por_semaforo = 0; %por a 1 para usar o semaforo
tempo_inicial = toc;

if espera_por_semaforo
    while 1
        temp_down = toc-tempo_inicial;
        temp = ~getvalue( dio_in.Line([6 7 8]));
        ESCOLHA = strrep( num2str(temp) ,' ','')
         if ESCOLHA == '000' & temp_down > 5
           ESCOLHA ='100';
          break
         end
    end
else %% nao espera por semaforo e aguarda 5 segundos
    while toc-tempo_inicial < 5; end
    if count >=4
        ESCOLHA = '110';  %Vai para o parque
    else
        %ESCOLHA ='010';  % Vai em fremte 
        ESCOLHA ='100';  % Vai para a esquerda 
    
    end
end

return

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

function go_frente(xx1,xx2,dif1,dif2,alturaImg)

if xx1 < (0.5 * alturaImg)  %Teste a 50% do topo da image
    return
end
if calc_traj1(xx1,dif1) == 1    %Correu bem!
    return
end

if xx2 < 0.5 * alturaImg 
    return % Se nao houver brancos abaixo dos 50% na img esq continua o ciclo
end 
if calc_traj2(xx2,dif2) == 1    %Correu bem!
    return
end

return
%***********************


function go_esquerda(xx1,xx2,dif1,dif2,alturaImg)

if xx2 < (0.5 * alturaImg)  %Teste a 50% do topo da image
    return
end
if calc_traj2(xx2,dif2) == 1    %Correu bem!
    return
end

if xx1 < 0.5 * alturaImg 
    return % Se nao houver brancos abaixo dos 50% na img esq continua o ciclo
end 
if calc_traj1(xx1,dif1) == 1    %Correu bem!
    return
end

return
%***********************

function ret = test_passagem_valida
global tempo_ultima_passagem total_passagens_passadeira dio_out TEMPO_VALIDAR_PASSAGENS
clear_flip_flop( dio_out);
tmp_passadeira = toc;
%disp('Test passadeira...');
if (tmp_passadeira - tempo_ultima_passagem) > TEMPO_VALIDAR_PASSAGENS
    disp(['Passagem valida aos' num2str(toc) ' segundos']);
    total_passagens_passadeira = total_passagens_passadeira+1;
    tempo_ultima_passagem = tmp_passadeira;
    ret=1;
    return
else
%    disp('Desde a ultima passagem'); tmp_passadeira - tempo_ultima_passagem
    ret = 0;
%    disp('... nao valida');
end
return

%===========================
function stop_motor
global dio_out total_passagens_passadeira
    setSpeed(0);  %Parar por software de o hardware ainda nao o fez!
    enable_motor(dio_out,0)
    %disp('stop');
    disp(['stop: ' 'total_passagens_passadeira= ' num2str(total_passagens_passadeira)])
return

%================================================
function [em_tunel, v_esq ,v_dir] = test_tunel
global ai

sinal = round( 100*mean(getdata(ai)));
start(ai);

if sinal(1,3) > 50  %Valor optimo para determinar o tunel a 1 metro
        em_tunel = 1;
    else
        em_tunel = 0;
    end
v_esq = sinal(1,2);
v_dir = sinal(1,1);
return

%=======================      
function newDir = calc_dir_tunel(v_esq, v_dir)

desvio_actual = v_esq - v_dir;
if( 0 ) %v_esq < 50 | v_dir < 50)
    newDir = -100; %nao fazer nada
else
    newDir = desvio_actual * 0.02;
end

return
