/*ROBOT   14/04/04   */
#include<pic.h>
#include"biosdem.h"

#define MAX 0xF100
#define MotG CCPR2L
#define MotD CCPR1L
#define fincourse RB0
__CONFIG(0x3D39);

void Detection_Piste(void);
char masque=0, SEUIL=100;
char CaptDroit=0,CaptCentre=0, CaptGauche=0, CaptExtrmDroit=0, CaptExtrmGauche=0/*, racourcis=0, priorite=0*/;
unsigned char i,n;
void main(void)
	{
	CCP2CON=0x0C;
	MotG=000; 				/*moteur gauche à l'arrêt*/
	CCP1CON=0x0C;
	MotD=000;				/*moteur droit à l'arrêt*/
	TRISD=0xF1;			 	/*RD4 à RD7 et RD0 en entrée*/
	TRISC=0x00;				/*port C en sortie*/
	TMR2ON=1;				/*mise en route du timer 2*/
	TRISB=0xFD;				/*RB2 en sortie*/
	PORTD=8;				/*choix du sens de rotation des moteurs*/
	PR2=99;					
	adc_on();				
	TRISB0=1;				/*RB0 en entrée*/
	TRISB1=0;				/*RB1 en sortie*/
	TRISB2=0;				/*RB2 en sortie*/
	TRISB4=0;				/*RB4 en sortie*/
	TRISB5=0;				/*RB5 en sortie*/
	while(RD0==0);
	for(;;)
		{
			if(fincourse == 0 )					/***********************************/
				{								/*contrôle du capteur fin de course*/								   
					MotG=0;					/*qui va arrêter les moteur lorsque*/
					MotD=0;					/*le robot tapera la barre		   */
					for( ;	; );				/*								   */
				}		 						/***********************************/
		n=0;
												/*switch sur la position 1*/
				
			Detection_Piste();					/*appel du sous programme detection piste*/
			switch ( n )
				{
				case 0:							/*********************************/
				case 5:							/*								 */
				case 7:							/*								 */
				case 9:							/*							   	 */
				case 10:						/*							   	 */
				case 11:						/*								 */
				case 12:						/*possibilité que les capteurs ne*/
				case 13:						/*peuvent pas appliquées		 */
				case 14:						/*								 */
				case 15:						/*********************************/
				case 17:
				case 18:
				case 19:
				case 20:
				case 21:
				case 22:
				case 23:
				case 24:
				case 25:
				case 26:
				case 27:
				case 28:
				case 29:
				case 30:
				if (MotG==0 && MotD == 0)		/*si etat precedent est arret*/
					{
					MotD=100;					/*il va a toco*/
					MotG=100;
					}
					break;
				case 1:
					MotD=42;		/*capteur droit*/
					MotG=100;
					break;
				case 2:
					MotD=100;		/*capteur milieu*/
					MotG=100;
					break;
				case 3:
					MotD=63;		/*capteurs milieu et droit*/
					MotG=100;
					break;
				case 4:
					MotD=100;		/*capteur gauche*/
					MotG=42;
					break;
				case 6:
					MotD=100;		/*capteurs milieu et gauche*/
					MotG=63;
					break;
				case 8:				/*capteur extreme droite*/
					if ( MotG==100 && MotD == 42 )
						{
						MotD=30;
						MotG=100;
						}
					break;
/*				case 13:
					priorite=1;
					a travailler pour sonar,activer un timer pour qu'il fasse une distance de 1.30m avec le sonar activé
					break;*/
				case 16:			/*capteur extreme gauche*/
					if ( MotG==42 && MotD == 100 )
						{
						MotD=100;
						MotG=30;
						}
					break;
/*				case 22:
					if ( racourcis==0 )
						racourcis=1;		activer un timer
					else
						{
						MotD=100;
						MotG=30;
						}
					a travailler pour ptet racourcis
					break;*/
				case 31:
//					racourcis=0;
//					priorite=0;
					break;
/*				if ( timer==(50cm parcouru) && racourcis==1 )
					{
					racourcis=0;
					arret du timer
					}*/
/*				if ( timer==(1m30 parcouru) && priorite==1 )
					{
					priorite=0;
					arret du timer
					}*/
						
				}
		
		if ( RD6==1 )		
			{
			CCPR1L=100;				/* test pour voir si le robot va droit avec les*/
			CCPR2L=100;				/*deux moteurs à fond						   */
			}
	
	}
}



void Detection_Piste(void)
{
	
	CaptDroit=adc_read_8b(1);				//capteur Droit
	CaptCentre=adc_read_8b(3);				//capteur Centre 
	CaptGauche=adc_read_8b(2);				//capteur Gauche
	CaptExtrmDroit=adc_read_8b(0);			//capteur extreme Droit
	CaptExtrmGauche=adc_read_8b(4);			//capteur extreme Gauche

	TRISA=0xFF;
	 masque = 0;
	if(CaptDroit<SEUIL)	
		{
		masque = 0x01;
		RB1=1;
		n=n+1;
		}
	else RB1=0;
	if(CaptCentre<SEUIL)
		{
		masque = masque | 0x02;
		RB4=1;
		n=n+2;
		}								/************************************/
	else RB4=0;							/*ce sous programme sert à la 		*/
	if(CaptGauche<SEUIL)				/*programmation des capteurs		*/
		{								/*pour que le robot puisse suivre	*/
		masque = masque | 0x04;			/*la bande réfléchissante. Pour cela*/
		RB5=1;							/*on utilise un seuil qui est ici de*/
		n=n+4;							/*100								*/
		}								/************************************/
	else RB5=0;
	if(CaptExtrmDroit<SEUIL)
		{
		masque = masque | 0x08;
		RB2=1;
		n=n+8;
		}
	else RB2=0;
	if(CaptExtrmGauche<SEUIL)
		{
		masque = masque | 0x0F;
		RC0=1;
		n=n+16;
		}
	else RC0=0;


}