#include<pic.h>
#include <stdlib.h>


#include "delay.h"
#include "i2c.h"

#define MG 0
#define MD 1
#define DS1621   0x48<<1 /* adresse i2c du ds1621 */
#define SONAR    0xE0    /* adresse i2c du module sonar */
 
void waitRAT(void);
void InitCan(void);
unsigned char ReadCan8bit(unsigned char );
void InitPwm(void);
void SetDutyCycle(unsigned char ,unsigned char );
void WriteSonar(unsigned char location, unsigned char byte);
unsigned char ReadSonar(unsigned char location);
int sonar(void);
void raccourci(void);
void Seuil(void);
void Marche_Arriere(void);
void test_mode(unsigned char, unsigned char);
void ARRET(void);

char capg2,capd2,capg,capd,capc; /* capteur numerisé gauche droite centre */ 
char seuilg,seuild,seuilc,seuilg2,seuild2; /* seuil des capteurs */ 
char n;  /* combinaision des capteurs */
char vitesse, vitesseg, vitessed,vitesseg2,vitessed2 ;
char delta_v1 ;	/*premier difference de vitesse*/
char delta_v2 ;	/*deuxieme difference de vitesse*/
char mode=0;
int i,distance;		/*compteurs*/
char j,d,k;

void main(void)
{

TRISB0=0;		/* leb vert RB0 et rouge RB1 */
TRISB1=0;
TRISD3=0;		/*moteurs*/
TRISD1=0;
TRISD2=0;
TRISD0=1;
RD3=0;
InitCan();
InitPwm();
seuilg = 150 ;
seuild = 150 ;
seuilc = 150 ;
seuilg2 = 150 ;
seuild2 = 150 ;
vitesse = 100 ;
delta_v1 = 10 ;
delta_v2 = 85 ;
TRISC0=1;
TRISC3=0;

 /*************** initialize i2c module *****************/
   	#ifdef I2C_MODULE
   	  SSPMode(MASTER_MODE);
   	  SSPEN = 1;
   	  CKP = 1;
   	#else
   		    #error "I2C NON INITIALISE"
   	#endif
 /*******************************************************/

do
{
if (RD0>0)
{RD3=1;} 			/* autorise le demarrage des moteurs */
else
{RD3=0;}
if (RC0>0)
{ARRET();}				/*Fin de course*/


/* lire les 3 capteurs */

Seuil();


/*Raccourci*/

if ((capg2==1)&&(capd2==0)&&(capg==1))
{
raccourci();
}


/*Sonar*/

/*if ((capd2==1)&&(capd==1)&&(capg2==0)&&(capc==1)&&(capg==0))
{mode=3;
distance=sonar();
if (distance<50)
RD3=0;
while(distance<50);
RD3=1;
}*/


/* fabriquer une variable comprise entre 0 et 7 */
/*0 000 - 1 001 - 2 010 - 3 011 - 4 100 - 5 101 - 6 110 - 7 111 */

n=(capg*4+capc*2+capd);	
    
   switch(n)
	{
          case 0 :
		   Marche_Arriere();
		   break;
	  case 7 :
	  case 5 :
	  case 2 :
		   test_mode(vitesse, vitesse);
		   break;
          case 1 :
		   test_mode(vitesse-delta_v2, vitesse);
		   break; 		
 	  case 6 :
		   test_mode(vitesse, vitesse-delta_v1);
 		   break; 
	  case 3 :
		   test_mode(vitesse-delta_v1, vitesse);
 		   break; 
	  case 4 :
		   test_mode(vitesse, vitesse-delta_v2);
 		   break;
}
		   
}
while(1);
}/*main */



/**************************************************************************************/
void InitCan(void)
	{
	  ADCON1 = 0x02 ; /*Port E digital , Port A analogique , justification gauche*/
	  ADCON0 = 0x41 ; /*Tad = 8*Tosc = 8 * 0.25µs = 2µs > 1.6µs , ADON ;*/
	  ADIF = 0 ;
        }


/**************************************************************************************/
unsigned char ReadCan8bit(unsigned char ch)	
	{
	  ADCON0 &= 0xC7 ;  /* RAZ CHSx */
			    /* Sélection du canal */
	  if ( ch & 1 )
            {
             CHS0  = 1 ;
	    }
	  if ( ch & 2 )
            {
             CHS1  = 1 ;
	    }

	  if ( ch & 4 )
            {
             CHS2  = 1 ;
	    }

	  waitRAT();            /* Attendre le temps d'acquisition */
	
    	  ADGO = 1 ;            /* Démarre la conversion */
				/* Attend la fin de conversion */
	  while (ADGO && ADON)  /* Par sécurité on teste aussi ADON */
		;		/* car sinon on rentre dans une boucle infinie */
	  return ADRESH ;
	}


/************************************************************************
 * Required acquisition time for A/D module form Max. Heise 2002	*
 * p. 115 equation 11-1 Tacq=19,72,e-6					*
 * seconds, approx. 2,e-5 seconds.					*
 * One instruction is executed in one					*
 * cycle, one cycle at 4MHz takes 2,5,e-7				*
 * seconds. Thus we must idle for 80					*
 * instructions								*
 * Formula for this loop						*
 * Inst(I)=1+1+( (I-1)*(1+2) )+2+2=6+3I-3=3I+3 				*
 * +2 Inst.Cycles for the call of this function				*
 * => Inst(I)=3I+5 							*
 * Inst(I) must be 80							*
 * 80=3I+5								*
 * => I=25								*
 * Timing critical, so this is still in assembler.			*
 ************************************************************************/
char counter ;

void waitRAT(void) {

#asm
  I EQU 50      ; Note this is actually twice the required value of I, just to be sure
  movlw  I	; 1 Inst.Cycle
  movwf  _counter    ; 1 Inst.Cycle
WaitRAT_Loop
  decfsz _counter,f  ; 1 Inst.Cycle if I!=0, 2 Inst.Cycles if I==0
  goto   WaitRAT_Loop ; 2 Inst.Cycles
#endasm
}


/****************************************************************
/* Fonction : InitPwm()						*
/* Cette fonction initialise les deux modules CCP1 et CCP2 en 	*
/* mode pwm pour une fréquence de 10kHz avec un quartz de 4Mhz	*
/* les deux rapports cycliques sont mis à 0 			*
/****************************************************************/
 
void InitPwm(void)
	{
	PR2 = 99 ;
        CCPR1L = 0 ; 
	CCP1CON = 0x0C ; /* PWM mode , bit 5 et 4 à 0 */
	TRISC2 = 0 ;
	CCPR2L = 0 ; 
        TRISC1 = 0 ;
	CCP2CON = 0x0C;
	T2CON = 0x04 ;	/* TMR2ON prescaler = 1 */         
	}


/****************************************************************
/* Fonction : SetDutyCycle()					*
/* Cette fonction programme le rapport cyclique DutyCycle sur  	*
/* le canal ch (0 ou 1)						*
/* Le rapport est fourni sous la forme 0<DutyCycle<100		*
/* pour 0% < r <100% 						*
/* Si le canal n'est pas 0 ni 1 les deux signaux sont mis à 0 	*
/* Le rapport est limité à 100% 				*
/****************************************************************/

void SetDutyCycle(unsigned char ch,unsigned char DutyCycle)
	{

		if ( DutyCycle > 100)
		   {
		  	DutyCycle = 100 ;
		   }
		DutyCycle = ( (unsigned int) PR2 * (unsigned int)DutyCycle )/100 ;
		if ( ch == 0 )
		  {
 			CCPR1L = DutyCycle ;
			return ;
		  }
		if ( ch == 1 )
		  {
			CCPR2L = DutyCycle ;
			return ;
		  }
			
			CCPR1L = 0 ;
			CCPR2L = 0 ;
			return ;
	}


/* ecrit un octet sur le sonar ************************************************************/ 
   void WriteSonar(unsigned char location, unsigned char byte)
 {
      i2c_WriteTo(SONAR);
      i2c_PutByte(location);
      i2c_PutByte(byte);
   }


/* lit un octet sur le sonar **************************************************************/
   unsigned char ReadSonar(unsigned char location)
 {
      i2c_WriteTo(SONAR);
      i2c_PutByte(location);
      i2c_ReadFrom(SONAR);
      return i2c_GetByte(I2C_LAST);
   }

	
/******************************************************************************************/
  int sonar(void)
   {
   
      unsigned char light,distH,distL;
      int dist;
   
   	/* Sonar */
   	/*Lecture luminosité pour s'assurer que le sonar répond */
             
	WriteSonar(0,0x51);
	DelayMs(25);
      
/*   do {
     light=ReadSonar(0);}
     while(light==255);
*/

   	/* Lecture Distance en cm */

      DelayMs(25);
      distH=ReadSonar(2);
      DelayMs(25);
      distL=ReadSonar(3);
      dist = (distH<<8) + distL ;
   
     	DelayMs(25);

          return dist;
   }


/************************************************************************************
C'est dangeureux !! Il suffit que la première fois il ne détecte pas le raccourci et que la seconde fois oui pour que le robot soit totalement hors piste !!
*/
void raccourci (void)
{
if (k!=3)
mode = 2;
SetDutyCycle(MG,100);
SetDutyCycle(MD,60);
for(k=0;k<2;k++)
DelayMs(100);
SetDutyCycle(MG,100);
SetDutyCycle(MD,100);
k=3;
while(mode==2)
{
if (RC0>0)
{ARRET();}

Seuil();

if ((capg==1)||(capd==1)||(capc==1))
mode=1;
}
}	


/***********************Binarisation avec seuils*********************************/
void Seuil(void)
{
if(ReadCan8bit(0)<seuilg)		/* numerisation */
{capg=1;}
else {capg=0;}

if(ReadCan8bit(1)<seuild)		/* numerisation */
{capd=1;}
else {capd=0;}

if(ReadCan8bit(3)<seuilc)		/* numerisation */
{capc=1;}
else {capc=0;}

if(ReadCan8bit(4)<seuilg2)		/* numerisation */
{capg2=1;}
else {capg2=0;}

if(ReadCan8bit(5)<seuild2)		/* numerisation */
{capd2=1;}
else {capd2=0;}
}


/******************************Marche Arrière***********************************/
void Marche_Arriere(void)
{
		   if (mode==1)
		   {
			i++;
			if ((i>2000)&&(i<6000))
		   	{
			vitesseg=100-vitesseg;
		   	vitessed=100-vitessed;
		        RD1=1;
		        RD2=1;
 		   	}
		   		
			if((i>=6000)&&(i<12000))
			{
			  vitesseg=vitesse;
			  vitessed=vitesse;
			  RD1=0;
		   	  RD2=0;
 		   	}
		         if(i>=12000)
			  {
			  vitesseg=40;
			  vitessed=vitesse;
			  RD1=0;
		   	  RD2=0;
 		   	  }
		    }    
		    if (mode==0)
		    {
			vitesseg=vitesse;
			vitessed=vitesse;
			RD1=0;
			RD2=0;
		    }
if((vitesseg2!=vitesseg)||(vitessed2!=vitessed))
{
vitesseg2=vitesseg;
vitessed2=vitessed;

SetDutyCycle(MG,vitesseg);
SetDutyCycle(MD,vitessed);
}
}


/*************************************************************************/
void test_mode(unsigned char g, unsigned char d)
{
  vitessed=d;
  vitesseg=g;

if (mode==0)
{mode=1;}
if((vitesseg2!=vitesseg)||(vitessed2!=vitessed))
{
RD1=0;
RD2=0;
i=0;
vitesseg2=vitesseg;
vitessed2=vitessed;
SetDutyCycle(MG,vitesseg);
SetDutyCycle(MD,vitessed);
}
}


/***************************Fin de course********************************/

void ARRET(void)
{
RD3=0;
SetDutyCycle(MG,0);
SetDutyCycle(MD,0);
while (1);
}