%----------------------------------------------------------------------------------------
% 			Project of automation and robotics   - degree in Mechanical Engineering - 
%
% Autor:			Pedro da Costa Maia & Rui Freitas Ferreira
% Date:			11Abr2002
% Description:	Autonomous Control  of a mobil robot using vision and others sensors (I-R)
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
%utilities/ variables inicialization 
%----------------------------------------------------------------------------------------
i = 1;
erro = 0;
vel = -1;
vel_max = 0.3;
num_voltas = 0;
pode_detectar = 1;
start_move = 0;
contador = 0;
travei = 0;
num_passagens = 0 ;
base_dados = 0 ;
acumulaerroparairascegasemcurva = 0;
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
% open the Real-Time file and set zero speed
%----------------------------------------------------------------------------------------
open_system('segunda_manga');
pause
set_param('segunda_manga/Gain','Gain','vel');
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
% Inicialization of 16 digital lines ( 8 output/ 8 input )
%----------------------------------------------------------------------------------------
dio = digitalio('nidaq', 1);
linhas_out = addline(dio, 0:7, 0, 'out');
linhas_in = addline(dio, 0:7, 1, 'in',{'passadeira','start','vazio1','amarelo','vazio2',...
      'vazio3','vazio4','vermelho'});
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
% Actuation of digital output for robot's imobilization - Output Disable Input
putvalue(dio.Line(1:2),[1 1]);
putvalue(dio.Line(4:5),[0 0]);%leds Indication off
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
% inicialization of vfm - image capture tool
%----------------------------------------------------------------------------------------
vfm('grab',1);
vfm('show',0);
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
% Manual Start  
%----------------------------------------------------------------------------------------
while start_move == 0;
   start_move = getvalue(dio.start);
   pause(0.1)
end
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
% Actuation of digital output for robot's movement sinalization - green led
%----------------------------------------------------------------------------------------
putvalue(dio.Line(4:5),[1 0]);
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
% Actuation of digital output for robot's movement
%----------------------------------------------------------------------------------------
putvalue(dio.Line(1:2),[0 0]);
%----------------------------------------------------------------------------------------


clc
%-----------------------------------------------------------------------------------------
% ##### Main Code ######
%-----------------------------------------------------------------------------------------
while 1 == 1;
   
   i = i + 1;
   
   tic %time counting inicialization
   
   %##########################################################################################
   %---------------------------------------------------------------------------------------
   % Get information of passadeira state
   %---------------------------------------------------------------------------------------

   passadeira = getvalue(dio.passadeira);
   if passadeira == 1;
      
      if i > contador;
         pode_detectar = 1;
      end
      
      if and( passadeira == 1 , pode_detectar == 1 ); 
         pode_detectar = 0;
         contador = i + 25;
         num_passagens = num_passagens + 1;
         
         putvalue(dio.Line(1:2),[1 1]); %actuation of Output Disable Input         
         disp(['********* Travei que nem um Doido ********** na passagem pela zebra nº: ' num2str(num_passagens)]);
         ver_semaforos = 1;
         travei = 1;
         traveiparaarampa = 1;
         
         if num_passagens == 5;
            putvalue(dio.Line(1:2),[1 1]);
            disp('********** Terminei a Prova ***********');         
            break; return;
         end
         
      end;
   end%passadeira==1
   %###########################################################################################
   
   %###########################################################################################
   %----------------------------------------------------------------------------------------
   % Traffic lights Status  
   %----------------------------------------------------------------------------------------

   if ver_semaforos == 1;
                  
      ver_semaforos = 0;
     
      if getvalue(dio.vermelho) == 1;
         
         while getvalue(dio.vermelho) == 1;
            pause(0.01)
         end
         
      else
         pause(5)
      end
      
      luz_amarela = getvalue(dio.amarelo);
      
      if luz_amarela == 1;
         direcao_frente = 0;
      else
         direcao_frente = 1;
      end
      
      putvalue(dio.Line(1:2),[0 0]);
      
   end%ver_semaforos
   %##########################################################################################
   
   %---------------------------------------------------------------------------------
   % Capture of one frame, image treatment, binarization 
   %---------------------------------------------------------------------------------
   im = vfm('grab',1);
   linha_gray = rgb2gray(im(80,:,:));
   linha_adjust = imadjust(linha_gray,[0.2 1],[]);
   linha_bin = im2bw(linha_adjust);
   %---------------------------------------------------------------------------------
   
   %##########################################################################################
   %---------------------------------------------------------------------------------
   % Analize what following direction
   %---------------------------------------------------------------------------------

   if direcao_frente == 1;
      tic
      e = 0;
      vel_max = 0.3;
      
      while toc < 4;
         %--------------------------------------------------------------------------------
         %aceleration ramp
         e = e + 1;
         if e < 10
            vel = -1 + ((1 + vel_max) / 10 * e);
            set_param('segunda_manga/Gain','Gain','vel');
         end
         %--------------------------------------------------------------------------------
         
         %---------------------------------------------------------------------------------
         % Capture of one frame, image treatment, binarization 
         %---------------------------------------------------------------------------------
         im = vfm('grab',1);
         linha_gray = rgb2gray(im(80,:,:));
         linha_adjust = imadjust(linha_gray,[0.2 1],[]);
         linha_bin = im2bw(linha_adjust);
         %---------------------------------------------------------------------------------

         erro = 80 -( min(find( linha_bin ==1 )));
         
         if ~isempty(erro)
            set_param('segunda_manga/Gain2','Gain','erro');
         end
      end %while toc<4 - this garantee the initial alignment
      
      erro = 0;      
      set_param('segunda_manga/Gain2','Gain','erro');
      
      pause(3) %this forced the line loose
      
      %---------------------------------------------------------------------------------
      % Capture of one frame, image treatment, binarization 
      %---------------------------------------------------------------------------------
      im = vfm('grab',1);
      linha_gray = rgb2gray(im(80,:,:));
      linha_adjust = imadjust(linha_gray,[0.2 1],[]);
      linha_bin = im2bw(linha_adjust);
      %---------------------------------------------------------------------------------
      
      %find linha
      while isempty(find( linha_bin ==1 )) 
         im = vfm('grab',1);
   		linha_gray = rgb2gray(im(80,:,:));
   		linha_adjust = imadjust(linha_gray,[0.2 1],[]);
   		linha_bin = im2bw(linha_adjust);
      end
      	
      
      while ~isempty(find( linha_bin ==1 )) 
         %---------------------------------------------------------------------------------
         % Capture of one frame, image treatment, binarization 
         %---------------------------------------------------------------------------------
         im = vfm('grab',1);
   		linha_gray = rgb2gray(im(80,:,:));
   		linha_adjust = imadjust(linha_gray,[0.2 1],[]);
         linha_bin = im2bw(linha_adjust);
         %---------------------------------------------------------------------------------
         
         erro = 95 -( max(find( linha_bin ==1 )));   
         
         if ~isempty(erro);
            set_param('segunda_manga/Gain2','Gain','erro');
         end         
         
         acumulaerroparairascegasemcurva = [acumulaerroparairascegasemcurva erro];
         
      end %while ~isempty
      
      erro = mean(acumulaerroparairascegasemcurva(end-7:end));
      set_param('segunda_manga/Gain2','Gain','erro');
      
      while isempty(find( linha_bin ==1 )) 
         %---------------------------------------------------------------------------------
         % Capture of one frame, image treatment, binarization 
         %---------------------------------------------------------------------------------
         im = vfm('grab',1);
   		linha_gray = rgb2gray(im(80,:,:));
   		linha_adjust = imadjust(linha_gray,[0.2 1],[]);
         linha_bin = im2bw(linha_adjust);
         %---------------------------------------------------------------------------------
      end
      
      pause(0.5)
      
      while getvalue(dio.passadeira)==0;
         %---------------------------------------------------------------------------------
         % Capture of one frame, image treatment, binarization 
         %---------------------------------------------------------------------------------
         im = vfm('grab',1);
         linha_gray = rgb2gray(im(80,:,:));
         linha_adjust = imadjust(linha_gray,[0.2 1],[]);
         linha_bin = im2bw(linha_adjust);
         %---------------------------------------------------------------------------------
         
         erro = 95 -( max(find( linha_bin ==1 )));   
         
         if ~isempty(erro);
            set_param('segunda_manga/Gain2','Gain','erro');
         end
         
      end
      
      putvalue(dio.Line(1:2),[1 1]);
      
      if getvalue(dio.passadeira)==0;
         ver_semaforos = 1;
         travei = 1;
         traveiparaarampa = 1;
         contador = i + 25;
      end
      
      direcao_frente = 0;
      
   else %direcao_frente == 1
      
      erro = 80 -( min(find( linha_bin ==1 )));
      
      %-------------------------------------------------------------------------------------
      %aceleration ramp
      if  traveiparaarampa == 1;
         if i < 11;
            vel = -1 + ((1 + vel_max) / 10 * i);
            set_param('segunda_manga/Gain','Gain','vel');         
         else
            if i < contador-15 ;  
               vel = -1 + ((1 + vel_max) / 10 * (i - contador + 26  ));
               set_param('segunda_manga/Gain','Gain','vel');
            else
               traveiparaarampa = 0;
            end
         end %i<11
      end% end aceleration ramp
      %------------------------------------------------------------------------------------
      
   end%direcao_frente==1
   %##########################################################################################
   
   %##########################################################################################
   if ~isempty(erro); 
      set_param('segunda_manga/Gain2','Gain','erro');
   end
   %##########################################################################################
     
   tt(i)=toc; % i cycle period read
   
end %-------END OF MAIN CODE----------------------------------------------------------------
%##########################################################################################

%##########################################################################################
%----------------------------------------------------------------------------------------
% Actuation of digital output for robot's movement sinalization - red led
%----------------------------------------------------------------------------------------
putvalue(dio.Line(4:5),[0 1]);
%----------------------------------------------------------------------------------------
%##########################################################################################