//*************************************
// IRBOT
//
// Auteur : YANISSE Daniel
//*************************************


#include <18F252.h>                        // Inclusion du Header correspondant au µC utilisé
#fuses HS,NOWDT,NOPROTECT,NOBROWNOUT,NOLVP  // Paramétrage des fusibles du microcontrôleur
#use delay(clock=20000000)
#use rs232(baud=115200, xmit=PIN_C6,rcv=PIN_C7)

//**********************
// Paramètres
//**********************

//**********************
// Defines
//**********************
#define DEBUT_TIMER0 65284    // (65534-65284)/(20000000/4) = 50 µs
#define SERVO_G      PIN_B3
#define SERVO_D      PIN_B2
#define SWITCH_G     PIN_C2
#define SWITCH_D     PIN_C3
#define LED_GAUCHE   PIN_C1
#define LED_DROITE   PIN_C0
#define CAPTEUR_IR   PIN_A0

//**********************
// Variables globales
//**********************
int nbtimers_droite_commande = 0;
int nbtimers_gauche_commande = 0;
int moteur_droit_actif = 0;
int moteur_gauche_actif = 0;
long nbtimers_effectue = 400;
int etalon = 180;

//**********************
// Fonctions
//**********************
void moteur_droit_avancer()  { nbtimers_droite_commande = 16; moteur_droit_actif  = 1; }
void moteur_droit_reculer()  { nbtimers_droite_commande =  6; moteur_droit_actif  = 1; }
void moteur_droit_arreter()  { nbtimers_droite_commande = 11; moteur_droit_actif  = 0; }
void moteur_gauche_avancer() { nbtimers_gauche_commande =  6; moteur_gauche_actif = 1; }
void moteur_gauche_reculer() { nbtimers_gauche_commande = 16; moteur_gauche_actif = 1; }
void moteur_gauche_arreter() { nbtimers_gauche_commande = 11; moteur_gauche_actif = 0; }

void moteurs_pause()
	{
		moteur_droit_arreter();
		moteur_gauche_arreter();
		delay_ms(200);
	}

int1 ir_droit()
	{
		int nb, nb2, i;
		nb = 0;
		for (i = 0; i < 250; i++)
			{
				output_high(LED_DROITE);
				delay_us(250);
				if (input(CAPTEUR_IR))
					{
						output_low(LED_DROITE);
						nb++;
						for(nb2 = 0; (input(CAPTEUR_IR) && nb2 < 10); nb2++) delay_us(100);
					}
				output_low(LED_DROITE);
				delay_us(250);
			}
		printf("droite : %u\n\r", nb);
		if (nb > etalon) return 1;
		else return 0;
	}

int1 ir_gauche()
	{
		int nb, nb2, i;
		nb = 0;
		for (i = 0; i < 250; i++)
			{
				output_high(LED_GAUCHE);
				delay_us(250);
				if (input(CAPTEUR_IR))
					{
						output_low(LED_GAUCHE);
						nb++;
						for(nb2 = 0; (input(CAPTEUR_IR) && nb2 < 10); nb2++) delay_us(100);
					}
				output_low(LED_GAUCHE);
				delay_us(250);
			}
		printf("gauche : %u\n\r", nb);
		if (nb > etalon) return 1;
		else return 0;
	}

//**********************
// Timers & Interruptions
//**********************
#INT_TIMER0
void main_pwm() // toutes les 50µs
	{
		if (nbtimers_effectue >= 400) // 400*50µs = 20ms
			{
				nbtimers_effectue = 0;
				if (moteur_droit_actif  == 1) output_high(SERVO_D);
				if (moteur_gauche_actif == 1) output_high(SERVO_G);
			}
		else
			{
				if (moteur_droit_actif  == 1 && nbtimers_effectue >= nbtimers_droite_commande) output_low(SERVO_D);
				if (moteur_gauche_actif == 1 && nbtimers_effectue >= nbtimers_gauche_commande) output_low(SERVO_G);
			}
		nbtimers_effectue++;
		set_timer0(DEBUT_TIMER0);
	}

//**********************
// Programme principal
//**********************

void main ()
	{
		int nb, nb2, i;

		set_timer0(DEBUT_TIMER0);
		setup_timer_0(RTCC_INTERNAL);
		enable_interrupts(INT_TIMER0);
		enable_interrupts(GLOBAL);

		printf("Demarrage\n\r");
		delay_ms(200);

		printf("Etalonnage\n\r");
		delay_ms(5000);

		nb = 0;
		for (i = 0; i < 250; i++)
			{
				output_high(LED_DROITE);
				delay_us(250);
				if (input(CAPTEUR_IR))
					{
						output_low(LED_DROITE);
						nb++;
						for(nb2 = 0; (input(CAPTEUR_IR) && nb2 < 10); nb2++) delay_us(100);
					}
				output_low(LED_DROITE);
				delay_us(250);
			}
		etalon = nb;
printf("etalon 1 : %u\n\r", etalon);
		delay_ms(5000);

		nb = 0;
		for (i = 0; i < 250; i++)
			{
				output_high(LED_DROITE);
				delay_us(250);
				if (input(CAPTEUR_IR))
					{
						output_low(LED_DROITE);
						nb++;
						for(nb2 = 0; (input(CAPTEUR_IR) && nb2 < 10); nb2++) delay_us(100);
					}
				output_low(LED_DROITE);
				delay_us(250);
			}
printf("etalon 2 : %u\n\r", nb);

		etalon = etalon/2 + nb/2;
printf("etalon : %u\n\r", etalon);


		while(TRUE)
			{
				if(!input(SWITCH_G)) // obstacle détecté à gauche
					{
						moteurs_pause();
						moteur_droit_reculer();
						moteur_gauche_reculer();
						delay_ms(1000);
						moteurs_pause();
						moteur_droit_reculer();
						moteur_gauche_avancer();
						delay_ms(500);
						moteurs_pause();
						moteur_droit_avancer();
						moteur_gauche_avancer();
					}
				else if (!input(SWITCH_D)) // idem à droite
					{
						moteurs_pause();
						moteur_droit_reculer();
						moteur_gauche_reculer();
						delay_ms(1000);
						moteurs_pause();
						moteur_droit_avancer();
						moteur_gauche_reculer();
						delay_ms(500);
						moteurs_pause();
						moteur_droit_avancer();
						moteur_gauche_avancer();
					}
				else if (ir_droit())
					{
						moteur_droit_avancer();
						moteur_gauche_arreter();
						delay_ms(50);
					}
				else if (ir_gauche())
					{
						moteur_gauche_avancer();
						moteur_droit_arreter();
						delay_ms(50);
					}
				else
					{
						moteur_droit_avancer();
						moteur_gauche_avancer();
					}
				delay_ms(50);
				/*moteur_droit_actif = 1;
				for (nbtimers_droite_commande = 5; nbtimers_droite_commande <= 80; nbtimers_droite_commande++)
					{
						printf("%u\n\r", nbtimers_droite_commande);
						delay_ms(200);
					}*/
			}
	}