/**************************************************************************
KitCar.c
	Programa do robo 1
	Versao: 1.0
	em 01/10/2002
        Versao 1.1
             -em 15/11/2002
             -incluido delays.h em kitcar.h
             -incluida leitura de varios canais do ADC, com delay antes do disparo
**************************************************************************/




//----------------------------------------------------------------------------
// Include files e macros
//----------------------------------------------------------------------------

#include "KitCar.h"

void main(void);					// programa principal

//-----------------------------------------------------------------------------
//definicoes de variaveis
//-----------------------------------------------------------------------------

extern int encoder;
char line[80];            //line de caracteres para mandar para serial
char buffer[8];           // buffer de caracteres p/ conversao inteiro/string   
char * ptr;
char leitura;
int i;

int contador =0;
signed char incremento =0;

/* ********************************************** Constantes ************************************************************* */

// Motor speed and direction

#define MFWD 3
#define MFLT 2
#define MOFF 1
#define MREV 0


// Distance thresholds
#define XL3 88 //80
#define XL2 86 //78
#define XL1 84 //76

#define XR1 80 //72
#define XR2 78 //70
#define XR3 76 //68

/* ********************************************** Fim das constantes ***************************************************** */

int value, sensor, dx;
int motl, motr;  // Motor da esquerda e da direita

/* ****************************************** Função LerSensor() ********************************************************* */

int LerSensor(void){
  int valorAD, frente;

  SetChannelADC(3);				// direciona entrada 0 para o ADC
  Delay10();
  ConvertADC(); 				// dispara conversao
  while( BusyADC()); 				// espera fim de conversao
  valorAD = ADRESH;

  SetChannelADC(0);				// direciona entrada 2 para o ADC
  Delay10();
  ConvertADC(); 				// dispara conversao
  while( BusyADC()); 				// espera fim de conversao
  frente = ADRESH;

  if(frente>35)
    valorAD = valorAD + 50;

  return valorAD;
}

int LerSensor4(void){
  int valorAD, frente;

  SetChannelADC(4);				// direciona entrada 0 para o ADC
  Delay10();
  ConvertADC(); 				// dispara conversao
  while( BusyADC()); 				// espera fim de conversao
  valorAD = ADRESH;

  return valorAD;
}


int LerSensorF(void){
  int frente;

  SetChannelADC(0);				// direciona entrada 2 para o ADC
  Delay10();
  ConvertADC(); 				// dispara conversao
  while( BusyADC()); 				// espera fim de conversao
  frente = ADRESH;

  return frente;
}

int SensorFinal(void)
{
	int DI0, DI1;
	char leitura;
	leitura = ReadDInputL();                	// Le entrada DI.0
		if(leitura & 0b00000001)
		  DI0 = 1;      // Faixa preta - retorna 1
		else
			DI0 = 0;   // Faixa branca - retorna 0
		if(leitura & 0b00000010)
			DI1 = 1;
		else
			DI1 = 0;

	if(DI0 && DI1) return 0;
		return 1;
}


void monitor(void)
{
         itoa(encoder, line);                   // transforma encoder em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  

		leitura = ReadDInputL();                // le entradas digitais altas ( 8 a 15)		
		if(leitura & 0b00000001) line[10] ='1';      // testa entrada 1 de portD e escreve valor na serial
		else              line[10] ='0';      // repare que entrada 1 alta = entrada 9

		leitura = ReadDInputL();                // le entradas digitais altas ( 8 a 15)		
		if(leitura & 0b00000010) line[15] ='1';      // testa entrada 1 de portD e escreve valor na serial
		else              line[15] ='0';      // repare que entrada 1 alta = entrada 9

                SetChannelADC(0);                       // direciona entrada 0 para o ADC
		Delay10();
		ConvertADC();				// dispara conversao 
		while( BusyADC());                      // espera fim de conversao

                itoa(ADRESH,&line[20]);                 // transforma leitura do ADC em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  
	
                SetChannelADC(1);                       // direciona entrada 1 para o ADC
		Delay10();
		ConvertADC();				// dispara conversao 
		while( BusyADC());                      // espera fim de conversao

                itoa(ADRES, &line[30]);                 // transforma leitura do ADC em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  

                SetChannelADC(2);                       // direciona entrada 2 para o ADC
		Delay10();
		ConvertADC();				// dispara conversao 
		while( BusyADC());                      // espera fim de conversao

                itoa(ADRESH, &line[40]);                 // transforma leitura do ADC em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  
	
                SetChannelADC(3);                       // direciona entrada 3 para o ADC
		Delay10();
		ConvertADC();				// dispara conversao 
		while( BusyADC());                      // espera fim de conversao
 
                itoa(ADRESH, &line[50]);                 // transforma leitura do ADC em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  

                puts (&line[0]);                       // escreve linha de catacteres no terminal
}



/* *********************************** Fim da função LerSensor() ********************************************************* */

/* ******************************************* Programa principal ******************************************************** */

void main()
{

	PORTCbits.RC4 = 1;                	//liga led 1

	while(TestaTeclaStart());           	// fica preso esperando soltar tecla start

	SystemInit();				// inicializa ports e perifericos do cartao

	putrsUSART("\r\n**  Equipe KIMAUANISSO  **\r\n"); // escreve algo na serial

// outras inicializacoes --------------------------------------------------------

	PORTCbits.RC4 = 1;                	      //liga led 1

//                  00000000001111111111222222222233333333334444444444555555555566666666667
//                  01234567890123456789012345678901234567890123456789012345678901234567890   
        putrsUSART("encoder=  DI0= DI1= Adc0=     Adc1=     Adc2=     Adc3=     \r\n");

// loop do programa principal ---------------------------------------------------
	/*while(1)
		{
		for(i=0; i<79; i++) line[i] = ' ';    // limpa line de caracteres
                line[78] = '\r';                      // volta linha
                line[79] = '\0';                       // coloca terminador de string

		leitura = ReadDInputL();                // le entradas digitais altas ( 8 a 15)		
		if(leitura & 0b00000001) line[10] ='1';      // testa entrada 1 de portD e escreve valor na serial
		  else line[10] ='0';      // repare que entrada 1 alta = entrada 9
	
		leitura = ReadDInputL();                // le entradas digitais altas ( 8 a 15)		
		if(leitura & 0b00000010) line[15] ='1';      // testa entrada 1 de portD e escreve valor na serial
		  else line[15] ='0';      // repare que entrada 1 alta = entrada 9

   puts (&line[0]);                       // escreve linha de catacteres no terminal
	}*/

	while(SensorFinal())
		{
		//if(TestaTeclaStop())break;		// se apertou stop, sai do loop

		for(i=0; i<79; i++) line[i] = ' ';    // limpa line de caracteres
                line[78] = '\r';                      // volta linha
                line[79] = '\0';                       // coloca terminador de string
 
                itoa(encoder, line);                   // transforma encoder em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  

		leitura = ReadDInputL();                // le entradas digitais altas ( 8 a 15)		
		if(leitura & 0b00000001) line[10] ='1';      // testa entrada 1 de portD e escreve valor na serial
		else              line[10] ='0';      // repare que entrada 1 alta = entrada 9

		leitura = ReadDInputL();                // le entradas digitais altas ( 8 a 15)		
		if(leitura & 0b00000010) line[15] ='1';      // testa entrada 1 de portD e escreve valor na serial
		else              line[15] ='0';      // repare que entrada 1 alta = entrada 9

                SetChannelADC(0);                       // direciona entrada 0 para o ADC
		Delay10();
		ConvertADC();				// dispara conversao 
		while( BusyADC());                      // espera fim de conversao

                itoa(ADRESH,&line[20]);                 // transforma leitura do ADC em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  
	
                SetChannelADC(1);                       // direciona entrada 1 para o ADC
		Delay10();
		ConvertADC();				// dispara conversao 
		while( BusyADC());                      // espera fim de conversao

                itoa(ADRES, &line[30]);                 // transforma leitura do ADC em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  

                SetChannelADC(2);                       // direciona entrada 2 para o ADC
		Delay10();
		ConvertADC();				// dispara conversao 
		while( BusyADC());                      // espera fim de conversao

                itoa(ADRESH, &line[40]);                 // transforma leitura do ADC em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  
	
                SetChannelADC(3);                       // direciona entrada 3 para o ADC
		Delay10();
		ConvertADC();				// dispara conversao 
		while( BusyADC());                      // espera fim de conversao
 
                itoa(ADRESH, &line[50]);                 // transforma leitura do ADC em string ascii
                *(line+strlen(line)) = ' ';            // tira terminador do string  

                puts (&line[0]);                       // escreve linha de catacteres no terminal


/* *********************************** Programa robô seguidor de parede esquerda ***************************************** */

    dx=sensor-LerSensor(); //Distance variarion
    sensor=LerSensor();
    
    motl=MFWD;
    motr=MFWD;

    if (sensor>=XL1) // Close to the wall
      {
      if (sensor>=XL3)
      	{
        motr=MREV; // Very close, turn in place
      	}
      else
	if (sensor>=XL2) 
      	  {
          motr=MOFF; // close enough, turn 
      	  }
      	else if (sensor>=XL1)
      	  {
          motr=MFLT; // a bit too close, shallow turn
       	  }
      }
    else // far from the wall
      {
      if (sensor<=XR3) // very far from wall
	  motl=MREV;
      else
        if (sensor<=XR2) // far from wall,
          {
          if (dx<2) // if small variation turn (avoids turning too short on salient angles)
            motl=MOFF;
          }
        else
	  if (sensor<=XR1)
            {
            motl=MFLT; // a bit too far, shallow turn
            }    
      }

/* **************************************** Decisões do motor da direita ************************************************* */
    
    switch(motr)
    {
      case MFWD:
			{
			if(LerSensorF()>40)
			  {
			  DirPWM2 = 0;
			  SetDCPWM2(180);  //175
			  }
			else
   			  {
			  DirPWM2 = 0;
			  SetDCPWM2(250); //250
			  }
			}
      break;
      case MFLT:
	{
	DirPWM2 = 0;
        SetDCPWM2(100);
	}
      break;

      case MOFF:
        SetDCPWM2(0);
      break;

      case MREV:
	{
	DirPWM2 = 1;
  SetDCPWM2(220);
	DirPWM1 = 0;
	SetDCPWM1(220);
	}
      break;
    }

/* **************************************** Fim das decisões do motor da direita ***************************************** */

/* **************************************** Decisões do motor da esquerda ************************************************ */

    switch(motl)
    {
      case MFWD:
			{
			if(LerSensorF()>40)
  			  {
 			  DirPWM1 = 0;
			  SetDCPWM1(180);
			  }
			else
   			  {
			  DirPWM1 = 0;
			  SetDCPWM1(250);
			  }
			}
      break;
      case MFLT:
	{
	DirPWM1 = 0;
        SetDCPWM1(100);
	}
      break;
      
      case MOFF:
        SetDCPWM1(0);
      break;
      
      case MREV:
	{
	DirPWM1 = 0;
  SetDCPWM1(155);
	DirPWM2 = 0;
  SetDCPWM2(300);
	}
      break;
    }

/* ******************************** Fim das decisões do motor da esquerda ************************************************ */


		}  // fim do loop principal
    monitor();
	//while(LerSensor()>=79 || LerSensor()<=85)
	while(LerSensor()!=82) //43
      {
	  if(LerSensor()>82)
	  {
      DirPWM1 = 0;
  	  SetDCPWM1(200);
	  DirPWM2 = 1;
      SetDCPWM2(200);
      }
	  if(LerSensor()<82)
	  {
      DirPWM1 = 1;
  	  SetDCPWM1(200);
	  DirPWM2 = 0;
      SetDCPWM2(200);
      }
}
    SetDCPWM1(0);
    SetDCPWM2(0);
    for(i=0; i<20000; i++)
    	Delay10();
  	MOTOR3 = 1;
  	for(i=0; i<1000; i++)
    	Delay10();
    	monitor();
	MOTOR3 = 0;
    while(1) monitor();
	ShutDown();
} // fim do main

/* ******************************************* Fim do programa principal ************************************************* */