function manga3_v4
%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; 

initHardware;

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


while 1 %«««««««««««««««««««««« CICLO ENVOLVENTE »»»»»»»»»»»»»»»»»»»»»»»»
initHardware;    
    %************************************************
    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
                task=1;
                disp('tarefa=andar em circulos');
                break
            end
            
            if TURBO2 &~TURBO1
                task=2;
                disp('tarefa=andar em circulos para tras');
                break
            end
            
            if TURBO1 & TURBO2
                task=3;
                disp('tarefa=para a frente e para tr´as');
                break
            end
        end
    end
    %********************************************** 
    
    
    %**********************************************
    %Ciclo de espera pela ordem de arranque
    %
    while 1
        if  getvalue( dio_in.Line(linhaSW1) ) 
            break
        end
    end
    %*********************************************
    
    disp('Ja comecei...')
    
    
    pause(0.5) % Para ganhar tempo e poder voltar a interpretar o SW1 noutro contexto
    
    
    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             %*
        
        %******************************************************************************     
        if task==1
            Vref=2;
            setDirection( Vref);
            Vmotor=1;
            pause(1);
            setSpeed( Vmotor );
        end
        
        if task==2
            Vref=2;
            setDirection( Vref);
            Vmotor=-1;
            pause(1);
            setSpeed( Vmotor );
        end
        
        if task==3
            
            Vref=0;
            setDirection( Vref);
            pause(1)
            amp=2;
            ang=linspace(0,2*pi,6000);
            ang = ang(1:end-1);
            Vmotor=amp*sin(ang);
            putdata(ao, [Vmotor]');
            start(ao);
            pause;
            stop(ao);
%             setSpeed( Vmotor );
        end
        
    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 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, [1 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 stop_motor
global dio_out total_passagens_passadeira
setSpeed(0);  %Parar por software de o hardware ainda nao o fez!
%disp('stop');
return
