//************************************************************************************//
// Auteur     : Christophe Le Lann                                                    //
// Mail       : totofwebcreation@gmail.com                                            //
// Site perso : www.totofweb.ma.cx                                                    //
//************************************************************************************//
// Description :                                                                      //
//   Programme en language C (relativement 8) simple pour tester un mini              //
// asservissement de déplacement utilisant des moteurs Pas à Pas unipolaire.          //
//************************************************************************************//
// Bon à savoir :                                                                     //
//   - 100 pas/tour (mais en demi-pas, donc équivalent 200 pas/tour)                  //
//   - roues 88mm diamètre                                                            //
//   - 178mm entre chaque roue                                                        //
//   - nb_pas_dist = DISTANCE / (DIAM_ROUES * PI) * MOT_NBPAS                         //
//                 = DISTANCE / 1.3823                                                //
//   - nb_pas_rot_angle  = (LARGEUR_ENTRE_ROUES * PI / 360 * ANGLE) /                 //
//                                                     (DIAM_ROUES * PI) * MOT_NBPAS  //
//   -                   = ANGLE * 1.1237                                             //
//************************************************************************************//

#include <16F628.h>             // Inclusion du Header correspondant au µC utilisé
#fuses HS,NOWDT,NOPROTECT,NOBROWNOUT,NOLVP  // Paramétrage des fusibles du microcontrôleur
#use delay(clock=4000000)       // Définition de l'horloge (pour les calculs de délais)

//******************************
// Defines
//******************************

#define DEMI_PAS TRUE           // Contrôle en demi-pas activé

#define MOT_D_BR1 PIN_B0        // Moteur droit,  Bobine 1
#define MOT_D_BR2 PIN_B1        // Moteur droit,  Bobine 2
#define MOT_D_BR3 PIN_B2        // Moteur droit,  Bobine 3
#define MOT_D_BR4 PIN_B3        // Moteur droit,  Bobine 4
#define MOT_G_BR1 PIN_B4        // Moteur gauche, Bobine 1
#define MOT_G_BR2 PIN_B5        // Moteur gauche, Bobine 2
#define MOT_G_BR3 PIN_B6        // Moteur gauche, Bobine 3
#define MOT_G_BR4 PIN_B7        // Moteur gauche, Bobine 4

#define SW_G_PIN PIN_A3         // Capteur de contact gauche
#define SW_D_PIN PIN_A2         // Capteur de contact droite

#define LED_ROUGE PIN_A0        // LED de débuggage rouge
#define LED_VERTE PIN_A4        // LED de débuggage verte

#define DELAI_DEBUT 48          // Delai entre chaque pas, au début de l'accélération (ms)
#define DELAI_FIN 12            // Delai entre chaque pas, en fin d'accélération (ms)
#define DELAI_PAS 2             // Pas d'accélération (ms)
#define DEBUT_TIMER 4           // (255-1-4)/(4000000/4) = 250 µs

//******************************
// Variables globales
//******************************

// dernière bobine utilisée (1, 2, 3, 4)
int dernierpas_droite;
int dernierpas_gauche;
int obstacle_evite = 0;

//******************************
// Fonctions
//******************************

// Fonctions de déplacement de niveau 0

void moteurs_stop() {
	output_low(MOT_D_BR1); output_low(MOT_G_BR1);
	output_low(MOT_D_BR2); output_low(MOT_G_BR2);
	output_low(MOT_D_BR3); output_low(MOT_G_BR3);
	output_low(MOT_D_BR4); output_low(MOT_G_BR4);
}

void faire_pas_droite(int sens) {
	if (sens == 1) { // Avant
		switch (dernierpas_droite) {
#if DEMI_PAS == TRUE
			case 2:  output_low(MOT_D_BR1);  output_high(MOT_D_BR2); output_low(MOT_D_BR3);  output_low(MOT_D_BR4);  dernierpas_droite = 3; break;
			case 3:  output_low(MOT_D_BR1);  output_high(MOT_D_BR2); output_high(MOT_D_BR3); output_low(MOT_D_BR4);  dernierpas_droite = 4; break;
			case 4:  output_low(MOT_D_BR1);  output_low(MOT_D_BR2);  output_high(MOT_D_BR3); output_low(MOT_D_BR4);  dernierpas_droite = 5; break;
			case 5:  output_low(MOT_D_BR1);  output_low(MOT_D_BR2);  output_high(MOT_D_BR3); output_high(MOT_D_BR4); dernierpas_droite = 6; break;
			case 6:  output_low(MOT_D_BR1);  output_low(MOT_D_BR2);  output_low(MOT_D_BR3);  output_high(MOT_D_BR4); dernierpas_droite = 7; break;
			case 7:  output_high(MOT_D_BR1); output_low(MOT_D_BR2);  output_low(MOT_D_BR3);  output_high(MOT_D_BR4); dernierpas_droite = 8; break;
			case 8:  output_high(MOT_D_BR1); output_low(MOT_D_BR2);  output_low(MOT_D_BR3);  output_low(MOT_D_BR4);  dernierpas_droite = 1; break;
			default: output_high(MOT_D_BR1); output_high(MOT_D_BR2); output_low(MOT_D_BR3);  output_low(MOT_D_BR4);  dernierpas_droite = 2; break;
#else
			case 2:  output_high(MOT_D_BR3); dernierpas_droite = 3; break;
			case 3:  output_high(MOT_D_BR4); dernierpas_droite = 4; break;
			case 4:  output_high(MOT_D_BR1); dernierpas_droite = 1; break;
			default: output_high(MOT_D_BR2); dernierpas_droite = 2; break;
#endif
		}
	} else { // Arrière
		switch (dernierpas_droite) {
#if DEMI_PAS == TRUE
			case 2:  output_high(MOT_D_BR1); output_low(MOT_D_BR2);  output_low(MOT_D_BR3);  output_low(MOT_D_BR4);  dernierpas_droite = 1; break;
			case 3:  output_high(MOT_D_BR1); output_high(MOT_D_BR2); output_low(MOT_D_BR3);  output_low(MOT_D_BR4);  dernierpas_droite = 2; break;
			case 4:  output_low(MOT_D_BR1);  output_high(MOT_D_BR2); output_low(MOT_D_BR3);  output_low(MOT_D_BR4);  dernierpas_droite = 3; break;
			case 5:  output_low(MOT_D_BR1);  output_high(MOT_D_BR2); output_high(MOT_D_BR3); output_low(MOT_D_BR4);  dernierpas_droite = 4; break;
			case 6:  output_low(MOT_D_BR1);  output_low(MOT_D_BR2);  output_high(MOT_D_BR3); output_low(MOT_D_BR4);  dernierpas_droite = 5; break;
			case 7:  output_low(MOT_D_BR1);  output_low(MOT_D_BR2);  output_high(MOT_D_BR3); output_high(MOT_D_BR4); dernierpas_droite = 6; break;
			case 8:  output_low(MOT_D_BR1);  output_low(MOT_D_BR2);  output_low(MOT_D_BR3);  output_high(MOT_D_BR4); dernierpas_droite = 7; break;
			default: output_high(MOT_D_BR1); output_low(MOT_D_BR2);  output_low(MOT_D_BR3);  output_high(MOT_D_BR4); dernierpas_droite = 8; break;
#else
			case 2:  output_high(MOT_D_BR1); dernierpas_droite = 1; break;
			case 3:  output_high(MOT_D_BR2); dernierpas_droite = 2; break;
			case 4:  output_high(MOT_D_BR3); dernierpas_droite = 3; break;
			default: output_high(MOT_D_BR4); dernierpas_droite = 4; break;
#endif
		}
	}
}

void faire_pas_gauche(int sens) {
	if (sens == 1) { // Avant
		switch (dernierpas_gauche) {
#if DEMI_PAS == TRUE
			case 2:  output_low(MOT_G_BR1);  output_high(MOT_G_BR2); output_low(MOT_G_BR3);  output_low(MOT_G_BR4);  dernierpas_gauche = 3; break;
			case 3:  output_low(MOT_G_BR1);  output_high(MOT_G_BR2); output_high(MOT_G_BR3); output_low(MOT_G_BR4);  dernierpas_gauche = 4; break;
			case 4:  output_low(MOT_G_BR1);  output_low(MOT_G_BR2);  output_high(MOT_G_BR3); output_low(MOT_G_BR4);  dernierpas_gauche = 5; break;
			case 5:  output_low(MOT_G_BR1);  output_low(MOT_G_BR2);  output_high(MOT_G_BR3); output_high(MOT_G_BR4); dernierpas_gauche = 6; break;
			case 6:  output_low(MOT_G_BR1);  output_low(MOT_G_BR2);  output_low(MOT_G_BR3);  output_high(MOT_G_BR4); dernierpas_gauche = 7; break;
			case 7:  output_high(MOT_G_BR1); output_low(MOT_G_BR2);  output_low(MOT_G_BR3);  output_high(MOT_G_BR4); dernierpas_gauche = 8; break;
			case 8:  output_high(MOT_G_BR1); output_low(MOT_G_BR2);  output_low(MOT_G_BR3);  output_low(MOT_G_BR4);  dernierpas_gauche = 1; break;
			default: output_high(MOT_G_BR1); output_high(MOT_G_BR2); output_low(MOT_G_BR3);  output_low(MOT_G_BR4);  dernierpas_gauche = 2; break;
#else
			case 2:  output_high(MOT_G_BR3); dernierpas_gauche = 3; break;
			case 3:  output_high(MOT_G_BR4); dernierpas_gauche = 4; break;
			case 4:  output_high(MOT_G_BR1); dernierpas_gauche = 1; break;
			default: output_high(MOT_G_BR2); dernierpas_gauche = 2; break;
#endif
		}
	} else { // Arrière
		switch (dernierpas_gauche) {
#if DEMI_PAS == TRUE
			case 2:  output_high(MOT_G_BR1); output_low(MOT_G_BR2);  output_low(MOT_G_BR3);  output_low(MOT_G_BR4);  dernierpas_gauche = 1; break;
			case 3:  output_high(MOT_G_BR1); output_high(MOT_G_BR2); output_low(MOT_G_BR3);  output_low(MOT_G_BR4);  dernierpas_gauche = 2; break;
			case 4:  output_low(MOT_G_BR1);  output_high(MOT_G_BR2); output_low(MOT_G_BR3);  output_low(MOT_G_BR4);  dernierpas_gauche = 3; break;
			case 5:  output_low(MOT_G_BR1);  output_high(MOT_G_BR2); output_high(MOT_G_BR3); output_low(MOT_G_BR4);  dernierpas_gauche = 4; break;
			case 6:  output_low(MOT_G_BR1);  output_low(MOT_G_BR2);  output_high(MOT_G_BR3); output_low(MOT_G_BR4);  dernierpas_gauche = 5; break;
			case 7:  output_low(MOT_G_BR1);  output_low(MOT_G_BR2);  output_high(MOT_G_BR3); output_high(MOT_G_BR4); dernierpas_gauche = 6; break;
			case 8:  output_low(MOT_G_BR1);  output_low(MOT_G_BR2);  output_low(MOT_G_BR3);  output_high(MOT_G_BR4); dernierpas_gauche = 7; break;
			default: output_high(MOT_G_BR1); output_low(MOT_G_BR2);  output_low(MOT_G_BR3);  output_high(MOT_G_BR4); dernierpas_gauche = 8; break;
#else
			case 2:  output_high(MOT_G_BR1); dernierpas_gauche = 1; break;
			case 3:  output_high(MOT_G_BR2); dernierpas_gauche = 2; break;
			case 4:  output_high(MOT_G_BR3); dernierpas_gauche = 3; break;
			default: output_high(MOT_G_BR4); dernierpas_gauche = 4; break;
#endif
		}
	}
}

// Fonction de déplacement de niveau 1

void avancer(int sens, int16 nbpas) {
	int16 i;
	int vitesse = DELAI_DEBUT;
	obstacle_evite = 0;

	for (i = 0; i < nbpas; i++) {
		if (obstacle_evite == 0) {
			faire_pas_droite(sens);
			faire_pas_gauche(sens);
			delay_ms(vitesse);
			if (vitesse > DELAI_FIN) // accélération progressive
				vitesse -= DELAI_PAS;
		}
	}
}

void rotation(int sens, int16 nbpas) {
	int sens_droite, sens_gauche;
	int16 i;
	int vitesse = DELAI_DEBUT;
	obstacle_evite = 0;

		if (sens == 1) { // vers la gauche
			sens_droite = 1;
			sens_gauche = 0;
		} else { // vers la droite
			sens_droite = 0;
			sens_gauche = 1;
		}

	for (i = 0; i < nbpas; i++) {
		if (obstacle_evite == 0) {
			faire_pas_droite(sens_droite);
			faire_pas_gauche(sens_gauche);
			delay_ms(vitesse);
			if (vitesse > DELAI_FIN) // accélération progressive
				vitesse -= DELAI_PAS;
		}
	}
}

// Fonctions de déplacement de niveau 2

void deambuler() {
	avancer(1, 5000);
	delay_ms(350);
}

void evitement(int cote) {
	avancer(0, 200); // on recule un peu
	delay_ms(350);
	if (cote == 1) rotation(0, 70); // cote gauche, évitement à droite
	else rotation(1, 70);           // l'inverse
	output_low(LED_ROUGE);
	delay_ms(350);
}

//******************************
// Timers & Interruptions
//******************************

#INT_RTCC //#INT_timer0
main_int () { // appelé à chaque fois que timer0 repasse à 0
	if (input(SW_G_PIN)) { //si obstacle à gauche
		output_high(LED_ROUGE);
		evitement(1);
		obstacle_evite = 1;
	}
	if (input(SW_D_PIN)) { //si obstacle à droite
		output_high(LED_ROUGE);
		evitement(0);
		obstacle_evite = 1;
	}
	set_timer0(DEBUT_TIMER); // on remet le timer 0 pour qu'il compte 250µs
}

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

void main () {
	set_timer0(DEBUT_TIMER);
	setup_timer_0(RTCC_INTERNAL);
	enable_interrupts(INT_RTCC);
	enable_interrupts(GLOBAL);

	delay_ms(1000);
	while (TRUE)
		deambuler();
}