#include "global.h"
/*******************************************************************
Programme du robot aspirateur avec un 30F4012
Oscillateur: FRC (7.5Mhz)*16PLL= 30 Mips
Les trois sortie PWM sont utilisées (2 pour les moteurs de déplacement
 et 1 pour le moteur de l'aspirateur)
Le convertisseur analogique numérique sert pour la gestion des contactes
grâce à la mesure de m'intensité qui circule dans les moteurs et à la gestion de l'alimentation.
Le timer 23 sert pour faire cligonter une LED et pour la rampe de vitesse des moteurs. nb On pourrait tout aussi bien utiliser un seul timer 16 bits
Le timer 45 sert pour temporiser le actions
*********************************************************************/


//******************//
//fonction principale
//******************//
int main (void)
{
//Initialisation
ADPCFG=0xFFFF; // on désactive le CAN: ALL inputs digitals

//timer 23 pour la led et la rampe d'accélération!!!
OpenTimer23(T2_ON & T2_GATE_OFF & T2_IDLE_STOP & T2_PS_1_1 & T2_32BIT_MODE_ON & T2_SOURCE_INT, 10000 ); //0xE4E1C0/2
EnableIntT3;
ConfigIntTimer3(T3_INT_PRIOR_5 & T3_INT_ON);

//pour la gestion des tempo (une action à une durée limitée qu'il faut contrôler)
OpenTimer45(T4_ON & T4_GATE_OFF & T4_IDLE_STOP & T4_PS_1_256 & T4_32BIT_MODE_ON & T4_SOURCE_INT, 0xFFFFFFFF); 
EnableIntT5;
ConfigIntTimer5(T5_INT_PRIOR_5 & T5_INT_ON);

//Le CAN pour la gestion de la batterie !
//Et pour conaitre la l'intensité qui circule dans les moteurs

ConfigIntADC10(ADC_INT_DISABLE);

TRISCbits.TRISC14=in; //in Rx
TRISCbits.TRISC14=out;
TRISCbits.TRISC13=out; //out TX
TRISEbits.TRISE2=out;
TRISEbits.TRISE4=out;


LATEbits.LATE2=0; //Pour limiter la consommation
LATEbits.LATE4=0; //Pour limiter la consommation
LATCbits.LATC13=0; //Pour limiter la consommation
LATCbits.LATC14=0; //Pour limiter la consommation

//gestion des moteurs
cntct_drt_TRIS=in;
cntct_gch_TRIS=in;
cntct_centre_TRIS=in;

TRISBbits.TRISB0=in;//Pour la conversion analogique numérique
TRISBbits.TRISB1=in;
TRISBbits.TRISB2=in;

EN_A_TRIS=0;
IN_A1_TRIS=0;
IN_A2_TRIS=0;
EN_B_TRIS=0;
IN_B1_TRIS=0;
IN_B2_TRIS=0;
puls_aspi_TRIS=0;
alim_low=off;
/*
OpenUART1(
UART_EN & UART_IDLE_CON & UART_DIS_WAKE & UART_DIS_LOOPBACK  & UART_NO_PAR_8BIT & UART_1STOPBIT & UART_ALTRX_ALTTX &UART_DIS_ABAUD,
UART_INT_TX_BUF_EMPTY & UART_TX_PIN_NORMAL & UART_TX_ENABLE & UART_INT_RX_CHAR & UART_ADR_DETECT_DIS & UART_RX_OVERRUN_CLEAR,
BAUDS_9600_FRC_8PLL);
ConfigIntUART1(UART_RX_INT_EN & UART_RX_INT_PR2 & UART_TX_INT_DIS &UART_TX_INT_PR5);
IFS0bits.U1RXIF=0;
*/
led_count=0;
TRISEbits.TRISE0=0; //out pour la led
//Au reset on attend 4s avant de demarrer
LATEbits.LATE0=1;
demarrer_tempo(58594);
while(fin_tempo_var==off);
LATEbits.LATE0=0;




//configuration de la conversion analogique numérique
ADCON1bits.ADON = 0; /* turn off ADC */
SetChanADC10(ADC_CH0_POS_SAMPLEA_AN0 & ADC_CH0_NEG_SAMPLEA_NVREF);
ConfigIntADC10(ADC_INT_ENABLE & ADC_INT_PRI_1);
EnableIntADC;
 unsigned int PinConfig = ENABLE_AN0_ANA & ENABLE_AN1_ANA & ENABLE_AN2_ANA ;

 unsigned int Scanselect = SKIP_SCAN_AN3 & SKIP_SCAN_AN4 &
								SKIP_SCAN_AN5 & SKIP_SCAN_AN6 &
								SKIP_SCAN_AN7 & SKIP_SCAN_AN8;
								
 unsigned int Adcon3_reg = ADC_SAMPLE_TIME_31 &
								ADC_CONV_CLK_SYSTEM &
								ADC_CONV_CLK_32Tcy;
								
 unsigned int Adcon2_reg = ADC_VREF_AVDD_AVSS &
								ADC_SCAN_ON &
								ADC_ALT_BUF_OFF &
								ADC_ALT_INPUT_OFF &
								ADC_CONVERT_CH0 &
								ADC_SAMPLES_PER_INT_15;

 unsigned int Adcon1_reg = ADC_MODULE_ON &
								ADC_IDLE_CONTINUE &
								ADC_FORMAT_INTG &
								ADC_CLK_AUTO &
								ADC_SAMPLE_INDIVIDUAL &
								ADC_SAMP_ON &
								ADC_AUTO_SAMPLING_ON;

OpenADC10(Adcon1_reg, Adcon2_reg,Adcon3_reg,PinConfig, Scanselect);


ConfigIntADC10(ADC_INT_ENABLE & ADC_INT_PRI_1);
EnableIntADC;
ADCON1bits.SAMP = 1;
AN_alim=0;
AN_md=0;
AN_mg=0;

//configuration du module PWM
const unsigned int period = 1499;//20khz à 30Mips
const unsigned int sptime = 0x0;
const unsigned int config1 = PWM_EN & PWM_OP_SCALE1 & PWM_IPCLK_SCALE1 & PWM_MOD_FREE;
const unsigned int config2 = PWM_MOD1_IND & PWM_MOD2_IND & PWM_MOD3_IND  & PWM_PEN3H & PWM_PEN2H &
PWM_PEN1H & PWM_PDIS3L & PWM_PDIS2L & PWM_PDIS1L;
const unsigned int config3 = PWM_SEVOPS1 & PWM_OSYNC_PWM & PWM_UEN;

OpenMCPWM(period, sptime, config1,config2, config3);


SetDCMCPWM(1,2900,0);//aspi ! il faut l'activer !!!
pwm=pwm_min;
Etat=av;
fin_tempo_var=on;



//*************************************
//Bouce sans fin
//*************************************
while(1) // do forever
{
			
	//gestion de la batterie: trigger
	if(AN_alim < AN_alim_min_haut)
		{
		alim_low=on;
		}
	else if(AN_alim > AN_alim_min_bas)
	{
		alim_low=off;	
	}
	
	Etat_old=Etat;
	//on commence par mettre à jour la variable Evenement
	if(cntct_centre==on||(AN_md>AN_md_max&&AN_mg>AN_mg_max))
			Evenment=contacte_front;
	else if(cntct_drt==on||AN_md>AN_md_max)
			Evenment=contacte_drt;
	else if(cntct_gch==on||AN_mg>AN_mg_max)
			Evenment=contacte_gch;
	else if(fin_tempo_var==on)
			Evenment=fin_tempo;
	else
			Evenment=nothing;
						
	//On en déduit le nouvel etat à appliquer au robot

	switch (Evenment)
		{
		case nothing:
			//On ne fait rien
		break;
		case fin_tempo:
	    	switch (Etat)
			{
				case av:
				demarrer_tempo(av_tempo); Etat=av; break;
				case rec:
				demarrer_tempo(drte_tempo);pwm= pwm_min; Etat=drte; break;
				case drte:
				demarrer_tempo(av_tempo);pwm= pwm_min;Etat=av; break;
				case gche:
				demarrer_tempo(av_tempo);pwm= pwm_min;Etat=av; break;
				case ar:
				demarrer_tempo(drte_tempo);pwm= pwm_min;Etat=drte; break;
				case red:
				demarrer_tempo(av_tempo);pwm= pwm_min; Etat=av; break;
				case frn:
				Etat=red; break;
		   }
		break;
		case contacte_drt:
			switch (Etat)
			{
				case av:
				demarrer_tempo(gch_tempo);pwm=pwm_min;Etat=gche; break;
				case rec:
				 break;
				case drte:
				demarrer_tempo(gch_tempo);pwm= pwm_min;Etat=gche; break;
				case gche:
				 break;
				case ar:
				 break;
				case red:
				 break;
				case frn:	
				 break;
		   }		
		break;
		case contacte_gch:
			switch (Etat)
			{
				case av:
				 demarrer_tempo(drte_tempo);pwm= pwm_min;Etat=drte; break;
				case rec:
				 break;
				case drte:
				 break;
				case gche:
				 demarrer_tempo(drte_tempo);pwm= pwm_min;Etat=drte; break;
				case ar:
				 break;
				case red:
				 break;
				case frn:	
				 break;
		   }		
		break;
		case contacte_front:
			switch (Etat)
			{
				case av:
				 demarrer_tempo(rec_tempo);pwm= pwm_min;Etat=rec; break;
				case rec:
				 break;
				case drte:
					demarrer_tempo(gch_tempo);pwm= pwm_min;Etat=gche; break;
				case gche:
				demarrer_tempo(drte_tempo);pwm= pwm_min;Etat=drte; break;
				case ar:
				 break;
				case red:
				 break;
				case frn:	
				 break;
		   }		
		break;
		}
	
	//On applique le nouvel etat qui vient d'etre décidé 	
	switch (Etat)
		{
			case av:
			avance(); break;
			case rec:
			recule(); break;
			case drte:
			tourn_drt(); break;
			case gche:
			tourn_gch();break;
			case ar:
			arret(); break;
			case red:
			redemarrage(); break;
			case frn:	
			frein(); break;
		}
	}
}


//Fonction de déplacement du robot
void avance(void)
{
	if(Etat_old!=Etat)
		roue_libre();
			
	IN_A1=forward;
	IN_A2=!IN_A1;
	
	IN_B1=forward;
	IN_B2=!IN_B1;
	incr_pwm();	
	}
void recule(void)
{
	if(Etat_old!=Etat)
		roue_libre();
		
	IN_A1=reverse;
	IN_A2=!IN_A1;
	
	IN_B1=reverse;
	IN_B2=!IN_B1;
	incr_pwm();
	}
void tourn_drt(void)
{
	if(Etat_old!=Etat)
			roue_libre();
		
	IN_A1=reverse;
	IN_A2=!IN_A1;
	
	IN_B1=forward;
	IN_B2=!IN_B1;
	incr_pwm();
	}
void tourn_gch(void)
{
	if(Etat_old!=Etat)
		roue_libre();
		
	IN_A1=forward;
	IN_A2=!IN_A1;
	
	IN_B1=reverse;
	IN_B2=!IN_B1;
	incr_pwm();
	}

void arret(void)
{
	PTCONbits.PTEN=0;
	 EN_A_LAT=0;
	 EN_B_LAT=0;
	 puls_aspi=0;
	 }
void redemarrage(void)
{
	PTCONbits.PTEN=1;
}	

	
void frein(void)
{
	IN_A1=reverse;
	IN_A2=IN_A1;
	
	IN_B1=reverse;
	IN_B2=IN_B1;
	}

void roue_libre(void)
{
		SetDCMCPWM(3,0,0);
		SetDCMCPWM(2,0,0);
		
		int i,j;
		for(j=0;j<8;j++)
			for(i=0;i<20000;i++);
	}
void incr_pwm(void)
{
	if(Etat==av||Etat==rec)
		if( pwm < pwm_max_ar && pwm_count==on)
		{
			pwm_count=off;
			pwm++;
			SetDCMCPWM(3,pwm,0);
			SetDCMCPWM(2,pwm,0);
       	}		
     if(Etat==drte||Etat==gche)
		if( pwm < pwm_max_dg && pwm_count==on)
		{
			pwm_count=off;
			pwm++;
			SetDCMCPWM(3,pwm,0);
			SetDCMCPWM(2,pwm,0);
       	}		
}

	
//***************************************************************************//
//***************************************************************************//
//INTERRUPTIONS
//***************************************************************************//
//***************************************************************************//

//*********************//
//**interruption sur TIMER23 ***//
//the ISR name is chosen from the device linker script
void __attribute__((__interrupt__)) _T3Interrupt(void)
{
// Interrupt Service Routine code goes here 
if(led_count==1200)
{
	led_count=0;
	if(alim_low==on)
		LATEbits.LATE0=!LATEbits.LATE0;
	else
			LATEbits.LATE0=0;
}
else
	led_count++;
				
//Le timer23 se remet à 0 tt seul
IFS0bits.T3IF=0;
pwm_count=on;
}

//*********************//
//**interruption sur TIMER5 ***//
//the ISR name is chosen from the device linker script
void __attribute__((__interrupt__)) _T5Interrupt(void)
{
// Interrupt Service Routine code goes here */
IFS1bits.T5IF=0;
fin_tempo_var=on;
}

//*********************//
//**interruption sur U1RX ***//
//the ISR name is chosen from the device linker script
 void __attribute__((__interrupt__)) _U1RXInterrupt(void)
{
unsigned int recu;
	
// Interrupt Service Routine code goes here */
IFS0bits.U1RXIF=0;
}


//*********************//
//**interruption sur U1TX ***//
//the ISR name is chosen from the device linker script
void __attribute__((__interrupt__)) _U1TXInterrupt(void)
{
// Interrupt Service Routine code goes here
IFS0bits.U1TXIF=0;
}


//*********************//
//**interruption sur TIMER4 ***//
//the ISR name is chosen from the device linker script
void __attribute__((__interrupt__)) _T4Interrupt(void)
{
// Interrupt Service Routine code goes here */
IFS1bits.T4IF=0;
		
}
//*********************//
//**interruption sur ADC ***//
//the ISR name is chosen from the device linker script
void __attribute__((__interrupt__)) _ADCInterrupt(void)
{
// Interrupt Service Routine code goes here */
IFS0bits.ADIF=0;
	//Attention, la tension de reference est 4,69 V et pas 5. Cela depend du régulateur !!
	AN_alim=AN_alim/4+(3*(ADCBUF0+ADCBUF3+ADCBUF6+ADCBUF9+ADCBUFC))/10;
	AN_md =AN_md/4   +(3*(ADCBUF1+ADCBUF4+ADCBUF7+ADCBUFA+ADCBUFD))/10;
	AN_mg =AN_mg/4   +(3*(ADCBUF2+ADCBUF5+ADCBUF8+ADCBUFB+ADCBUFE))/10;
					
	ADCON1bits.SAMP = 1;//nouvelle conversion
		
}

//pour la gestion des tempos du robot
//Ainsi, on peut compter jusqu'a 256*2^32 cycles=
void demarrer_tempo(unsigned long tempo)
{
fin_tempo_var=off;
WriteTimer45(0xFFFFFFFF-tempo);//on met un unsigned long	
}