%----------------------------------------------------------------------------------------
% 			Project of automation e robótics   - 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
%----------------------------------------------------------------------------------------



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



% open the Real-Time file and set zero speed
%----------------------------------------------------------------------------------------
open_system('primeira_manga');
pause
set_param('primeira_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','vazio2','vazio3','vazio4',...
      'vazio5','vazio6'});
%----------------------------------------------------------------------------------------

%----------------------------------------------------------------------------------------
% 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;
   
   %try
   tic %time counting inicialization
   
   %---------------------------------------------------------------------------------
   % 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);
   %---------------------------------------------------------------------------------
   
   %---------------------------------------------------------------------------------      
   %error calculus for line tracking
   %---------------------------------------------------------------------------------
   erro = 80 -( min( find( linha_bin == 1)));      
   %---------------------------------------------------------------------------------
   
   %---------------------------------------------------------------------------------
   % Procedure for lap counting and robot's imobilization at the finish line
   %---------------------------------------------------------------------------------
   passadeira = getvalue(dio.passadeira); %logical read of line 9 (infra-red sensor)
   
   if i > contador;
      pode_detectar = 1;
   end
   
   if and( passadeira == 1 , pode_detectar == 1 ); 
      pode_detectar=0;
      contador = i + 25;
      num_voltas = num_voltas + 1;
      
      if num_voltas == 5;
         putvalue(dio.Line(1:2),[1 1]);
         disp('********* travei que nem um doido sou um ganda maluco **********');
         break; return; 
      end
   end;
   %--------------------------------------------------------------------------------
   
   %--------------------------------------------------------------------------------
   % Writing erro in Real Time Apllication
   %--------------------------------------------------------------------------------
   %if erro is no empty, actualize erro in the Real Time toolbox of Simulink
   if ~isempty(erro); 
      set_param('primeira_manga/Gain2','Gain','erro');
   end
   %--------------------------------------------------------------------------------
   
   %--------------------------------------------------------------------------------
   % Acceleration Ramp and variable speed operation control
   %--------------------------------------------------------------------------------
   if i < 11
      vel = -1 + ((1+vel_max) / 10 * i);
      set_param('primeira_manga/Gain','Gain','vel');
   end
   %--------------------------------------------------------------------------------
   
   tt(i)=toc; % i cycle period read
%   base_dados(i) = erro; % creating erro history

   
end

tempo_ciclo = sum(tt) / i; % cycle period calculus

frequencia = 1 / tempo_ciclo % main code iteration rating

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


