//***************************************************
// CCS Compiler
// SLAM 20 - Premiers pas de SLAM 
//         + detection collision AV
//         + ODO simplifiée pour les 1/2 tours et 1/4
//	   + ODO IR 15 pulses par tour de roue
//		26-11-04
//		Essais odométrie du Pig OK
//	OK, sans recule avant quart de tour
//
// Auteur : Alain CROSET
//***************************************************

#include <16f84a.h>
#fuses HS,NOWDT,NOPROTECT
#use delay(clock=20000000)

//******************
// DEFINE
//******************

#define TIME 10
#define AV 5
#define TG 2
#define TD 8
#define AR 10
#define ROTG 9
#define ROTD 6
#define STOP 0
#define GA 0
#define DR 1
#define ND 18			// Nbre de pulses pour 1 demi-tour et 15 pulses par tour de roue
#define NQ 6			// Nbre de pulses pour 1 quart-tour et  15 pulses par tour de roue

#define LED_G PIN_A4		// led signalisation marche	SLAM	

#define MICRO_S_G PIN_B6	// switch gauche détection de collision SLAM 290404
#define MICRO_S_D PIN_B7	// switch droite détection de collision SLAM 290404

#define ODO_G PIN_B2		// entrée impulsions par tout roue gauche SLAM 030604
				// Pin B4 remplacée par B0 pour éviter de génerer des int par Odométrie
#define ODO_D PIN_B3		// entrée impulsions par tout roue droite SLAM 260504

#define SIZE 7			// taille de la table tab_marche

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

void init () {			// Initialisation générale

	set_timer0(TIME);
 	setup_counters(RTCC_INTERNAL,WDT_18MS);
	enable_interrupts(INT_RTCC);		// autorise interruption sur timer0
	enable_interrupts(INT_RB);		// autorise interruption sur b4 à b7
	PORT_B_PULLUPS(0);		// désactive les pull-up de b
	enable_interrupts(GLOBAL);


}


void delay_S (int n) {			// délais de (n) secondes
  for (;n!=0; n--)
  delay_ms( 1000 );
}

//	----------ODOMETRIE-----------
//	If the input is high, test the flag for zero. 
//	If the flag is zero, then the counter hasn't been incremented. 
//	Increment the counter, and set the flag to 1. 
//	Only set the flag to zero once the encoder input is zero.



void demi_ar_d(int countd, flagd, ratecountd) {
  	countd = 0;
  	while (countd <= 18)
  	{
	if (input(ODO_D) && (!flagd))
		{
		flagd=1;
		countd++;	//distance travelled
//		ratecountd++;	//this is reset each interrupt
		}
	if (!input(ODO_D)) flagd=0;
    	}
}

void quart_ar_d(int countd, flagd, ratecountd) {
  	countd = 0;
  	while (countd <= 6)
  	{
	if (input(ODO_D) && (!flagd))
		{
		flagd=1;
		countd++;	//distance travelled
//		ratecountd++;	//this is reset each interrupt
		}
	if (!input(ODO_D)) flagd=0;
    	}
}
	
	
void quart_ar_g(int countg, flagg, ratecountg) {
  	countg = 0;
  	while (countg <= 6)
	{
	if (input(ODO_G) && (!flagg))
		{
		flagg=1;
		countg++;
//		ratecountg++;
		}
	if (!input(ODO_G)) flagg=0;
    	}
}

void recule_de(int pas, flagd, ratecountd) {
  	while (pas >=0)
  	{
	if (input(ODO_D) && (!flagd))
		{
		flagd=1;
		pas--;	//distance travelled
		ratecountd++;	//this is reset each interrupt
		}
	if (!input(ODO_D)) flagd=0;
    	}		
}

//**********************
// Interruption collision
//**********************

#int_rb			// interruption sur PORTB 4 à 7 (micro-switch de collision) 

void collision () {
//	if ((input(MICRO_S_G)==1) && (input(MICRO_S_D)==1)) {
//	recule_de(2);     Si détection trop proche de l'obstacle il faut d'abord reculer avant de tourner
//	output_a (TD); 
//	demi_ar_d (0,0,0); // 1/2 de tour arrière à droite pendant ND pulses
//	output_a (AV);
//	}
	if ((input(MICRO_S_D)==1) && (input(MICRO_S_G)==0)) {
//	recule_de(15);     Si détection trop proche de l'obstacle il faut d'abord reculer avant de tourner
	output_a (TD); 
	quart_ar_g (0,0,0); // 1/4 de tour arrière à gauche pendant NQ pulses
	output_a (AV);
	}
	if ((input(MICRO_S_G)==1) && (input(MICRO_S_D)==0)) {
//	recule_de(15);     Si détection trop proche de l'obstacle il faut d'abord reculer avant de tourner
	output_a (TG); 
	quart_ar_d (0,0,0); // 1/4 de tour arrière à droite pendant NQ pulses
	output_a (AV);
	}
}


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

main()  {
init();

while (TRUE) {

	output_a (AV);
	delay_S(1);	// Tout droit pendant 1 sec

	output_a (STOP);
	delay_S(1);	// Stop pendant  1 sec	
	
	}
}