function preditivo

warning off;

global  ao ao1 dio_out dio_in;
global  img Kd Kp comp_img dist_offset;
global  declive_recta velocidade;
global  Vmotor;
global  linhaSW1 linhaSW2 linhaSW3 linhaSW4;


linhaSW1 = 2; linhaSW2 = 3; linhaSW3 = 4; linhaSW4 = 5; 
comp_img=1.4; %comprimento da imagem 240px<==>1.4metros
dist_offset=0.6; %metros
declive_recta =0.8235;
Kp=0.5;
flag=0;
Vref_anterior=0;
    
initHardware;


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

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

while 1
   
  
%************************************************
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
        Vmotor=2.5
        ganho=0.75;
        Kd=-0.012;
        disp('Vmotor = 2.5');
                % Selecao da velocidade
        break
      end

      if TURBO2 &~TURBO1
        Vmotor=1;
        ganho=0.75;
        Kd=-0.012;
        disp('Vmotor = 1');
        
        break
      end

      if TURBO1 & TURBO2
        
        Vmotor=3.5;
        ganho=0.57;
        Kd=-0.012;
        disp('Vmotor = 3.5');
        
        break
      end
end
end
%********************************************** 


%**********************************************
%Ciclo de espera pela ordem de arranque
%
while 1
  if  getvalue( dio_in.Line(linhaSW1) ) 
      setSpeed( Vmotor );
      break
   end
end
%*********************************************
  
velocidade=(Vmotor*1.8)/5 %m/s

while 1
    [xx2_max,xx_med,xx2_min,declive1,declive2]=Calc_img
    if Calc_img==1
        continue
    end
    [dist_0,dist_1,dist_2,teta1,teta2]=calc_dist(xx2_max,xx_med,xx2_min,declive1,declive2)
   
    
    [tempo_0,tempo_1,tempo_2]=calc_tempo(dist_0,dist_1,dist_2)
    
    
    flag=0;  
    tic
    tmp=0;
    while tmp<=tempo_0
        Vref=Vref_anterior;
        if ~flag
            setDirection(Vref)
            flag=1;
        end
        tmp=toc;
    end
    
    flag=0;
    tic
    tmp=0;
    while tmp<=tempo_1
        Vref=teta1*(6/pi)*Kp;
        if ~flag
            setDirection(Vref)
            flag=1;
        end
        tmp=toc;
    end
    
    flag=0;
    tic
    tmp=0;
    while tmp<=tempo_2
        Vref=teta2*(6/pi)*Kp;
        if ~flag
           setDirection(Vref)
           flag=1;
        end
        tmp=toc;
    end  
    Vref_anterior=Vref;
    
end
    
    
    
    
    
end    
    
    
    
    
    
    

    
%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11FUNCTIONS!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!111  

%==================================
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 0 0 0 0]);

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


%************************************
function [xx2_max,xx_med,xx2_min,declive1,declive2]=Calc_img()
global img1 img2;
rgb_=vfm('grab',1);
% imshow(rgb_)
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);

img=rgb_4(:,:);
if img==[]
    Calc_img=1;
    return
end
[x2 y2]=find(img);
xx_max=max(x2);
xx_min=min(x2);

xx_Max=img(xx_max,:);
[x_Max y_Max]=find(xx_Max);
[y_Mmin]=min(y_Max);
[y_Mmax)=max(y_Max);
    

xx_min=img(xx_min,:);
[x_min y_min]=find(xx_min);
[y_mmin]=min(y_min);
[y_mmax]=max(y_min);


while 1
if (y_Mmax-y_Mmin)<=40
    xx_max=xx_max-10;
    xx_Max=img(xx_max,:);
    [x_Max y_Max]=find(xx_Max);
    [y_Mmin]=min(y_Max);
    [y_Mmax)=max(y_Max);
elseif (y_Mmax-y_Mmin)>40
    break
end

while 1
if (y_mmax-y_mmin)<=40
    xx_min=xx_min+10;
    xx_min=img(xx_min,:);
    [x_min y_min]=find(xx_min);
    [y_mmin]=min(y_min);
    [y_mmax]=max(y_min);
elseif (y_mmax-y_mmin)>40
    break
end


xx_med=(xx_max-xx_min)/2;
xxmed=img(xx_med,:);
[x2_med y2_med]=find(xxmed);
[yymed]=min(y2_med);


xx_M2=img(xx2_max,:);
[x2_M y2_M]=find(xx_M2);
[yy2_xm]=min(y2_M)


declive1=(yy2_xm-yymed)/(xx2_max-xx_med);
declive2=(yymed-yy2min_m)/(xx_med-xx2_min);
if declive1==[] | declive2==[]
    Calc_img=1;
    return
end


end


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

function [dist_0,dist_1,dist_2,teta1,teta2]=calc_dist(xx2_max,xx_med,xx2_min,declive1,declive2)

    global declive_recta comp_img dist_offset;

    teta1=atan(declive_recta-declive1);
    teta2=atan(declive_recta-declive2);


    dist_0=(240-xx2_max)*(comp_img/240);
    dist_1=(xx2_max-xx_med)*(comp_img/240);
    dist_2=(xx_med-xx2_min)*(comp_img/240);
   
end

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

function [tempo_0,tempo_1,tempo_2]=calc_tempo(dist_0,dist_1,dist_2)

global velocidade;
   
   tempo_0=dist_0/velocidade;
   tempo_1=dist_1/velocidade;
   tempo_2=dist_2/velocidade;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
function setDirection( dir )

global ao1;
    putdata(ao1, [dir]');
    start(ao1); 
    stop(ao1);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5

function setSpeed( speed )
global ao;
    putdata(ao, [speed]');
    start(ao);
    stop(ao);

end
%======================================