MES PROJETS ET TOUT LE RESTE
  1. S.U.P.R.A (V1.0)
  2. I. Présentation
  3. II. Mécanique
  4. III. Electronique
  5. IV. Informatique embarquée
  6. V. Code source et schéma
  7. VI. Photos et vidéos
  8. VII. Mes sources
  1. Présentation
    1. Objectif
    2. Caractéristiques
  2. Mécanique
  3. Electronique
    1. Carte maîtresse
    2. Carte d'asservissement
    3. Carte d'alimentation
  4. Informatique embarquée
  5. Code source et schéma
  6. Photos et vidéos
    1. Photos
    2. Vidéos
  7. Mes sources

Présentation

La réalisation de SUPRA (Supra, Une Plate-forme Robotique Asservie) découle de l’envie de mettre en pratique les asservissements courants dans le domaine de la robotique et de m'initier aux AVR. Ce robot est un prototype destiné à valider les algorithmes d'asservissement. Si les essais sont satisfaisants alors une version plus propre et aboutie sera développée afin de servir de plateforme d'expérimentation pour tester d'autres choses.

Objectif

L’objectif primaire est de réaliser une fonction d’asservissement permettant de suivre une trajectoire rectiligne à une vitesse donnée.

L'objectif secondaire serait de réaliser cette même fonction en ajoutant un asservissement de la vitesse de déplacement.

Caractéristiques

Caractéristiques principales :

  • Dimension : 350mm(L) X 320mm(l) X 240mm(h)
  • poids : ?
  • Vitesse : ≈57cm/s
  • Batterie : Li-ion 12V 4000mAh
  • CPU : ATmega 328p
  • Fréquence d'exéc. : 20Mhz
  • Langage : C
  • Environnement : Intérieur

Caractéristiques mécaniques :

  • Encodeurs : 360 tick/tr moteur
  • moto-réducteurs : 950D501LN (moteur RE385LN)
  • Réducteur : 50:1.
  • Roues : 75mm Ø
  • Diamètre robot : 350mm

Données utiles :

  • tick/tr de roue : 360x50= 18000 tick/tr de roue
  • Périmètre roue : PIx75= 235,6mm
  • Vitesse roue : 145tr/min (à 12V)
  • Vitesse moteur : 145x50= 7250tr/min soit 120,84tr/s
  • 1 tr moteur : 1/120,84= 8,3ms

Mécanique

La structure mécanique est volontairement simple. Un châssis rectangulaire en styroglass supporte deux ensembles moto-réducteur ainsi qu'une roue folle arrière. Ces trois éléments s'inscrivent dans un cercle de 350mm de diamètre (photo 1 et 4).

Chaque ensemble est composé d’un moto-réducteur, un encodeur, un coupleur d’axe, un moyeu, un support, une roue et… un morceau de coton-tige (photo 2 et 3).

L’assemblage de l’encodeur et du moto-réducteur fut la partie la plus délicate. Je savais, bien avant d’avoir ces moto-réducteurs entre les mains, que je pourrais avoir des surprises quant au diamètre de l’axe arrière, cette donnée importante n’étant indiquée nulle part, que ce soit dans le datasheet du moto-réducteur, du moteur seul ou sur le site du fabricant. Cet axe arrière accuse en fait un diamètre de 2,30mm (mesuré au pied à coulisse). Un coupleur d’axe permet l’union de ces deux pièces. Le morceau de coton tige est insérée en force sur l'axe arrière du moto-réducteur afin de combler l'écart de diamètre entre celui-ci et le coupleur.

Electronique

Une carte fond de panier accueil une carte d'alimentation, une carte dédiée à l'asservissement et une carte maîtresse qui gère la partie "haut niveau". J'ai laissé la place pour un 4ème et dernier emplacement au cas où. Le tout est alimenté par une batterie de pc portable (photo 6).

Carte maîtresse

Cette carte est gérée par un ATmega328P cadencé à 20Mhz. Cette carte s'occupe seulement d'envoyer les instructions de haut niveau par I²C.

Carte d'asservissement

La carte asservissement est gérée par un ATmega328p cadencé à 20Mhz. Chaque codeur fournissant un signal en quadrature, ils disposent de 2 canaux notés A et B. Les canaux A sont sur interruption externe. Le sens est décodé en hardware via des bascules CD4013.

Les moteurs sont commandés par des TLE5206-2S.

Carte d'alimentation

La carte d'alimentation fourni du 5V régulé grâce à un LM2575T-5. Les moteurs sont alimentés directement par la batterie sans aucune régulation.

Informatique embarquée

Un premier essai a été fait par la mise en pratique d'un asservissement en vitesse seule. Cette boucle d'asservissement se présente comme suit :

 asservissement vitesse

De l'encodeur ressort une mesure que l'on soustrait à la consigne pour former une erreur de vitesse que le PID va s'atteler à corriger. L'encodeur incrémental ne fournit pas directement une vitesse. On déduit cette donnée par calcul en comptant les impulsions pendant un interval de temps (voir ligne n°365).


asserv_vitesse.c: Voir le code
//###########################################################################
// 				ROBOT PID - CARTE ASSERVISSEMENT 							#
// 																			#
// VERSION			:	1 													#
// DATE DEPART		:	04/08/2014 											#
// DATE ACTUELLE	:	07/03/2015 											#
// AUTEUR			:	Frédéric .C 										#
// 																			#
// NOTE: 		Version de base du programme 								#
//###########################################################################

//	Encodeurs 		: 360 tick/tr moteur
//	Réducteur 		: 50:1
//	Roues			: 75mm de diamètre
//	Diamètre robot	: 350mm

//	tick/tr de roue	: 18000 tick/tr de roue  (360 x 50)
//	Périmètre roue	: 235,6mm (PI X 75)


//---------------------------------------------------------------------------
// 						LIBRAIRIES											-
//--------------------------------------------------------------------------- 
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>		// http://www.nongnu.org/avr-libc/user-manual/group__avr__interrupts.html
#include <compat/deprecated.h> 	// pour les fonctions sbi et cbi
//#include <stdbool.h>

//---------------------------------------------------------------------------
// 						DEFINITION DES CONSTANTES 							-
//---------------------------------------------------------------------------

#define F_CPU 20000000UL

// MOTEURS TRACTION
#define	MotDSens_DDR		DDRD		// REGISTRE
#define	MotDSens_portS		PORTD		// PORT (en sortie)
#define	MotDSens_broche		PD5			// BROCHE

#define	MotDPwm_DDR			DDRB		
#define	MotDPwm_portS		PORTB		
#define	MotDPwm_broche		PB1

#define	MotGSens_DDR		DDRD		
#define	MotGSens_portS		PORTD		
#define	MotGSens_broche		PD6			

#define	MotGPwm_DDR			DDRB		
#define	MotGPwm_portS		PORTB		
#define	MotGPwm_broche		PB2
	

// ENCODEURS
#define	EncodD_DDR			DDRD
#define	EncodD_portE		PIND		// PORT (en entree)
#define	EncodD_broche		PD2			// INT0

#define	EncodDSens_DDR		DDRD
#define	EncodDSens_portE	PIND		
#define	EncodDSens_broche	PD7

#define	EncodG_DDR			DDRD
#define	EncodG_portE		PIND		
#define	EncodG_broche		PD3			// INT1

#define	EncodGSens_DDR		DDRB
#define	EncodGSens_portE	PINB		
#define	EncodGSens_broche	PB0


// LED DEBUG (bicolore vert/rouge)
#define	Led1R_DDR			DDRC		
#define	Led1R_portS			PORTC		
#define	Led1R_broche		PC4
#define	Led1V_DDR			DDRC		
#define	Led1V_portS			PORTC		
#define	Led1V_broche		PC5


// EQUIVALENCES DIVERSES
#define	Avancer			1
#define	Reculer			2
#define	Gauche			3
#define	Droite			4

#define PWMD			OCR1A		// Registre PWM droit
#define PWMG			OCR1B		// Registre PWM gauche

#define	Rouge			1
#define	Vert			2
#define	Orange			3

//---------------------------------------------------------------------------
// 						DEFINITION DES VARIABLES GLOBALES 					-
//---------------------------------------------------------------------------
float tickD, tickG;			// tick moteur droit, moteur gauche sur interruption

const float kp = 0.7; 	//70;	70 qd on mesure en tr/s; 0.7 qd on mesure en cm/s		
const float ki = 0.1;	//10;
const float kd = 0.02;	//2;

float consigne;				// contient la consigne qui pilote le pid
float erreur_prec_d;
float erreur_prec_g;
	
float erreur_d;				// erreurs lues (terme proportionnel)
float erreur_g;			
float somme_erreur_d;		// total des erreurs (terme intégrale)
float somme_erreur_g;
float delta_erreur_d;		// variation des erreurs (terme dérivé)
float delta_erreur_g;

const float perim_roue = 235.6; 
const unsigned char reduction = 50;
const unsigned short tick_trm = 360;	// nombre de tick par tour moteur
const unsigned char freq_ech = 50;		// frequence d'échantillonage

//---------------------------------------------------------------------------
// 						PROTOTYPES DE FONCTIONS 							-
//--------------------------------------------------------------------------- 
void Ports_Init(void);
void Init(void);
void OrdreMoteur(unsigned char Sens, unsigned short VitesseG, unsigned short VitesseD);
void Led(unsigned char couleur);

void pid(float ConsigneVit);

void EnvoiCaractere(char DataCar);
void EnvoiChaine(char* DataString);
void EnvoiInt(unsigned int DataInt);
void EnvoiFloat(const float DataFloat);
unsigned char ReceptCaractere(void);

unsigned char ReceptString(unsigned char *x, unsigned char size);
//###########################################################################
// 																			#
// 						DEBUT DU PROGRAMME 									#
// 																			#
//###########################################################################

int main (void) 
{

	Ports_Init();				// initialisations E/S
	Init();						// initialisation avant démarrage
	
	consigne = 0;				// consigne de vitesse à 0 au départ
	somme_erreur_d = 0;
	somme_erreur_g = 0;
	erreur_prec_d = consigne; 	// initialisation sur consigne de depart
	erreur_prec_g = consigne;
	
	EnvoiFloat(0.0);		
	EnvoiChaine("\r\n"); 		//à la ligne!
	
	_delay_ms(1000);

	//consigne = 12/(perim_roue/10); //12cm/s converti en tr/s
	consigne = 12; // 12cm/s
	while (1) 
	{
	
		pid(consigne);			// consigne = nombre de tr/sec
		_delay_ms(20);			// PID à 50Hz (F=1/t) -> (1/freq_ech)*1000;
	}
	
	return 1;
}



//---------------------------------------------------------------------------
// 						INITIALISATION DES E/S 								-
//--------------------------------------------------------------------------- 
void Ports_Init() 
{						
	DDRB = 0;			// tout en entrée
	DDRC = 0;
	DDRD = 0;
	
	// Configuration des broches en sorties
	sbi(MotDSens_DDR,  MotDSens_broche);
	sbi(MotDPwm_DDR,  MotDPwm_broche);
	sbi(MotGSens_DDR,  MotGSens_broche);
	sbi(MotGPwm_DDR,  MotGPwm_broche);
	sbi(Led1R_DDR,  Led1R_broche);
	sbi(Led1V_DDR,  Led1V_broche);
	
	// Configuration des broches en entrée
	cbi(EncodD_DDR, EncodD_broche);
	cbi(EncodG_DDR, EncodG_broche);
	cbi(EncodGSens_DDR, EncodGSens_broche);
	cbi(EncodDSens_DDR, EncodDSens_broche);
}



//---------------------------------------------------------------------------
// 						INITIALISATION AVANT DEMARRAGE 						-
//--------------------------------------------------------------------------- 
void Init() 
{
	cbi(MotDSens_portS, MotDSens_broche);	// Sens Moteur Droit à 0
	cbi(MotGSens_portS, MotGSens_broche);	// Sens Moteur Gauche à 0
	cbi(Led1R_portS, Led1R_broche);			// Led Rouge à 0
	cbi(Led1V_portS, Led1V_broche);			// Led Verte à 0

	
	// fast pwm 8bits sans prescaler -> apparament pas assez précis
	//TCCR1A = 0b10000001; 
	//TCCR1B = 0b00001001;	
    
	// fast pwm 10bits sans prescaler
	TCCR1A = 0b10100011;			
	TCCR1B = 0b00001001;
	PWMD = 0;					// mini =~240; maxi=1023
	PWMG = 0;					// mini =~240; maxi=1023
	
	EIMSK = 0b00000011;  		// Activer INT1 et INT0
    EICRA = 0b00001111; 		// Déclanchement sur front montant de INT1 et INT0
    sei();						// Activer interruptions
	
	
	//---------- Initialisation UART ----------
	UBRR0H = 0b00000000;	// transmettre à 9600bps
	UBRR0L = 0b10000001;
	
	//UBRR0H = 0b00000010;	// transmettre à 2400bps -> génère trop d'erreurs
	//UBRR0L = 0b00001000;
	
	//UBRR0H = 0b00000000;	// transmettre à 19200bps -> génère trop d'erreurs
	//UBRR0L = 0b01000000;
	
	UCSR0C = 0b00000110; 	// mode asynchrone, pas de paritée, 8bits de données, 1bit de stop 
	UCSR0B = 0b00011000;	// activer reception et transmition
}



//---------------------------------------------------------------------------
// 							INTERRUPTIONS EXTERNES 							-
//---------------------------------------------------------------------------
ISR(INT0_vect)		
{
	// si EncodDSens = 0 (on avance) alors incrémenter TickD sinon décrémenter
	if ( bit_is_clear(EncodDSens_portE, EncodDSens_broche))
	{
		tickD++;
	}
		else
		{
			tickD--;
		}	
}


ISR(INT1_vect)
{
	// si EncodGSens = 0 (on avance) alors décrémenter TickG sinon incrémenter (car cet encodeur est inversé)
	if ( bit_is_clear(EncodGSens_portE, EncodGSens_broche))
	{
		tickG--;
	}
		else
		{
			tickG++;
		}
}



//---------------------------------------------------------------------------
// 							FONCTION MOTEURS 								-
//--------------------------------------------------------------------------- 
void OrdreMoteur(unsigned char Sens, unsigned short VitesseG, unsigned short VitesseD)
{
	switch (Sens)
	{
		case 0 : 		// ne rien faire
			break;
			
		case 1 : 		// avancer
			cbi(MotGSens_portS, MotGSens_broche);
			cbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 2 : 		// reculer
			sbi(MotGSens_portS, MotGSens_broche);
			sbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 3 : 		// tourner à gauche
			sbi(MotGSens_portS, MotGSens_broche);
			cbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 4 : 		// tourner à droite
			cbi(MotGSens_portS, MotGSens_broche);
			sbi(MotDSens_portS, MotDSens_broche);
			break;
			
		default : 		// si Sens prend une valeur inconnue on ne fait rien
			break;
	}

	PWMG = VitesseG;
	PWMD = VitesseD;
}


//---------------------------------------------------------------------------
// 							FONCTION LED DEBUG 								-
//---------------------------------------------------------------------------
void Led(unsigned char Couleur)
{
	 switch (Couleur)
	{
		case 0 : 		// Rouge et Vert éteind
			cbi(Led1R_portS, Led1R_broche);
			cbi(Led1V_portS, Led1V_broche);
			break;
			
		case 1 : 		// Rouge
			cbi(Led1V_portS, Led1V_broche);
			sbi(Led1R_portS, Led1R_broche);
			break;
			
		case 2 : 		// Vert
			cbi(Led1R_portS, Led1R_broche);
			sbi(Led1V_portS, Led1V_broche);
			break;
			
		case 3 : 		// Rouge + Vert = Orange
			sbi(Led1R_portS, Led1R_broche);
			sbi(Led1V_portS, Led1V_broche);
			break;
			
		default : 		// si Couleur prend une valeur inconnue on ne fait rien
			break;
	}
}



//---------------------------------------------------------------------------
// 					FONCTION ASSERVISSEMENT PID VITESSE 					-
//---------------------------------------------------------------------------
void pid(float ConsigneVit)
{
	float compteurD;
	float compteurG;
	float mesure_vit_d;
	float mesure_vit_g;
	
	if (ConsigneVit != 0) 	
	{
		// affectation des compteurs de PID et reset des compteurs sur interruption
		compteurD = tickD;
		compteurG = tickG;
		tickD = 0;
		tickG = 0;
		
		
		mesure_vit_d = (freq_ech*compteurD)/tick_trm/reduction;	// vitesse mesurée en tr/s
		mesure_vit_d = mesure_vit_d * (perim_roue/10);
		mesure_vit_g = (freq_ech*compteurG)/tick_trm/reduction;
		mesure_vit_g = mesure_vit_g * (perim_roue/10);
		
		//calcul des erreurs
		erreur_d = ConsigneVit - mesure_vit_d;
		erreur_g = ConsigneVit - mesure_vit_g;
		
		somme_erreur_d += erreur_d;
		somme_erreur_g += erreur_g;
		
		delta_erreur_d = erreur_d - erreur_prec_d;
		delta_erreur_g = erreur_g - erreur_prec_g;
		
		// calcul de la commande
		PWMD += ((kp * erreur_d) + (ki * somme_erreur_d) + (kd * delta_erreur_d));
		PWMG += ((kp * erreur_g) + (ki * somme_erreur_g) + (kd * delta_erreur_g));
		
		// Normalisation des commandes PWM de sortie (le moteur ne bouge pas avec un pwm < 240)
		if (PWMD < 240) {PWMD = 240;}
			else if (PWMD > 1023) {PWMD = 1023;}
		if (PWMG < 240) {PWMG = 240;}
			else if (PWMG > 1023) {PWMG = 1023;} 
			
		// mise à jour de l'erreur précédente
		erreur_prec_d = erreur_d;
		erreur_prec_g = erreur_g;
		
		//EnvoiFloat((PWMD/100));		// envoie la valeur via uart	
		EnvoiFloat(mesure_vit_d);
		EnvoiChaine("\r\n"); 			// retour a la ligne
		Led(Vert);						// on indique que le pid est actif par la led verte
	}
		else 							// si la consigne est 0 on desactive le PID et on l'indique par la led rouge
		{ 
			OrdreMoteur(1, 0, 0); 
			Led(Rouge);
		}
}



//---------------------------------------------------------------------------
// 						FONCTION ENVOI CARACTERE							-
//---------------------------------------------------------------------------
void EnvoiCaractere(char DataCar)
{ 
   while(!(UCSR0A & (1<<UDRE0)))	// attendre jusqu'a ce que le transmetteur soit prêt
   {
      //ne rien faire
   }

   //UCSR0A |= (1<<TXC0); 			// effacer txc flag
   UDR0 = DataCar;						// écrire la donnée dans le buffer de l'USART
}


//---------------------------------------------------------------------------
// 						FONCTION ENVOI STRING								-
//---------------------------------------------------------------------------
void EnvoiChaine(char* DataString)
{
	while(*DataString != 0x00)
	{
		EnvoiCaractere(*DataString);
		DataString++;
	}
}



//---------------------------------------------------------------------------
// 						FONCTION ENVOI INTEGER								-
//---------------------------------------------------------------------------
void EnvoiInt(unsigned int Dataint)
{
	unsigned char cent, dix, unit;
	
	unit = (Dataint%10) | 0x30;	//unités
	Dataint/= 10;					// stock le quotient de la division dans Dataint
	dix = (Dataint%10) | 0x30;		//dixaines
	Dataint = Dataint%100;
	cent = (Dataint/10) | 0x30;	//centaines
	
	if (cent != '0') {EnvoiCaractere(cent);} // si la centaine vaut 0 pas besoin de l'envoyer
	if ((cent != '0') && (dix != '0')) {EnvoiCaractere(dix);} // si centaine et dixaine vallent 0 pas besoin de l'envoyer non plus
	EnvoiCaractere(unit); // l'unité est toujours envoyée même si vaut 0
}



//---------------------------------------------------------------------------
// 						FONCTION ENVOI FLOAT								-
//---------------------------------------------------------------------------
void EnvoiFloat(const float DataFloat)
{
	unsigned int v,p;
	long int num;
	
	num = DataFloat*1000;
	p = num%1000;
	num = num/1000;
	v = num;

	EnvoiInt(v);
	EnvoiCaractere('.');
	EnvoiInt(p);

}


//---------------------------------------------------------------------------
// 						FONCTION RECEPTION CHAR								-
//---------------------------------------------------------------------------
unsigned char ReceptCaractere(void)
{
 
 while(!(UCSR0A & (1<<RXC0)));
 return UDR0;
 
}

				

SUPRA doit se déplacer en maintenant un cap (rouler en ligne droite). il existe une méthode éprouvée du nom d'asservissement polaire, schématisée ci-dessous :

 asservissement polaire

Une consigne de distance et une consigne d'orientation sont données à l’entrée du régulateur. Le nombre d'incréments — dans mes programmes un incrément est désigné par le mot "tick" — produits par les encodeurs est scindé en deux mesures, la distance parcourue par le robot en son centre et l'angle. Ces mesures sont retournées à l’entrée de la boucle en tant qu'erreurs.

asserv_polaire.c: Voir le code
		
//###########################################################################
// 				ROBOT PID - CARTE ASSERVISSEMENT 							#
// 																			#
// VERSION			:	1 													#
// DATE DEPART		:	07/03/2015 											#
// DATE ACTUELLE	:	26/03/2015 											#
// AUTEUR			:	Frédéric .C 										#
// 																			#
// NOTE				: 	ASSERVISSEMENT POLAIRE								#
//						Une freq. d'ech. de 100Hz au lieu de 50Hz accroît	#
//						nettement la précision du système.					#
// 																			#
// A FAIRE			:	variables pid à déplacer en local si possible		#
//						rampe de vitesse ?	(ajout pid vitesse ?)			#
//						test sprintf() pour debug							#
//						deplacer led bicolore pour libérer le bus i²c		#
//																			#
//	-----------------------------------------------------------------------	#
//						X_position = distance * sin(theta);
//						Y_position = distance * cos(theta);
//						multiplier theta * (180.0/PI) pour avoir 
//						orientation en degré.	
//###########################################################################

//	Encodeurs 		: 360 tick/tr moteur
//	Réducteur 		: 50:1
//	Roues			: 75mm de diamètre
//	Diamètre robot	: 350mm

//	tick/tr de roue	: 18000 tick/tr de roue  (360 x 50)
//	Périmètre roue	: 235,6mm (PI X 75)


//---------------------------------------------------------------------------
// 						LIBRAIRIES											-
//--------------------------------------------------------------------------- 
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>		// http://www.nongnu.org/avr-libc/user-manual/group__avr__interrupts.html
#include <compat/deprecated.h> 	// pour les fonctions sbi et cbi
//#include <stdbool.h>
#include <math.h>				// pi, cos, sin, etc.
#include <stdlib.h>				// itoa, ltoa, utoa -> à remplacer par sprintf() ?

//---------------------------------------------------------------------------
// 						DEFINITION DES CONSTANTES 							-
//---------------------------------------------------------------------------

#define F_CPU 20000000UL

// MOTEURS TRACTION
#define	MotDSens_DDR		DDRD		// REGISTRE
#define	MotDSens_portS		PORTD		// PORT (en sortie)
#define	MotDSens_broche		PD5			// BROCHE

#define	MotDPwm_DDR			DDRB		
#define	MotDPwm_portS		PORTB		
#define	MotDPwm_broche		PB1

#define	MotGSens_DDR		DDRD		
#define	MotGSens_portS		PORTD		
#define	MotGSens_broche		PD6			

#define	MotGPwm_DDR			DDRB		
#define	MotGPwm_portS		PORTB		
#define	MotGPwm_broche		PB2
	

// ENCODEURS
#define	EncodD_DDR			DDRD
#define	EncodD_portE		PIND		// PORT (en entree)
#define	EncodD_broche		PD2			// INT0

#define	EncodDSens_DDR		DDRD
#define	EncodDSens_portE	PIND		
#define	EncodDSens_broche	PD7

#define	EncodG_DDR			DDRD
#define	EncodG_portE		PIND		
#define	EncodG_broche		PD3			// INT1

#define	EncodGSens_DDR		DDRB
#define	EncodGSens_portE	PINB		
#define	EncodGSens_broche	PB0


// LED DEBUG (bicolore vert/rouge)
#define	Led1R_DDR			DDRC		
#define	Led1R_portS			PORTC		
#define	Led1R_broche		PC4
#define	Led1V_DDR			DDRC		
#define	Led1V_portS			PORTC		
#define	Led1V_broche		PC5

/*#define	Led1R_DDR			DDRC	// après déplacement de la led bicolore	
#define	Led1R_portS			PORTC		
#define	Led1R_broche		PC2
#define	Led1V_DDR			DDRC		
#define	Led1V_portS			PORTC		
#define	Led1V_broche		PC3*/

// EQUIVALENCES DIVERSES
#define	Avancer			1
#define	Reculer			2
#define	Gauche			3
#define	Droite			4

#define PWMD			OCR1A			// Registre PWM droit
#define PWMG			OCR1B			// Registre PWM gauche

#define	Rouge			1
#define	Vert			2
#define	Orange			3

//---------------------------------------------------------------------------
// 						DEFINITION DES VARIABLES GLOBALES 					-
//---------------------------------------------------------------------------
float tickD, tickG;						// tick moteur droit, moteur gauche sur interruption
short angle_deg;
short angle_rad;
float dist_metre;
unsigned char Pos_atteinte ;			// mis à 1 quand position voulue est atteinte
unsigned char step;						// est incrémenté à chaque étapes (déplacement et/ou rotation) du trajet

//-------------------------------- asservissement en distance
const float kp_dist = 0.6;	//0.2	//0.3	
const float ki_dist = 0;	//0.0002//0
const float kd_dist = 0.1;	//0		//0.1

float Consigne_Dist; 
float erreur_prec_dist;
float erreur_dist;
float somme_erreur_dist;
float delta_erreur_dist;

//-------------------------------- asservissement en angle
const float kp_orient = 0.6;			
const float ki_orient = 0;
const float kd_orient = 0.1;

float Consigne_Orient; 			
float erreur_prec_orient;
float erreur_orient;			
float somme_erreur_orient;
float delta_erreur_orient;

//-------------------------------- caractéristiques du robot
const float perim_roue = 235.6; 		// perimetre des roues. valeur en mm
const unsigned char reduction = 50;
const unsigned short tick_trm = 360;	// nombre de tick par tour moteur
const unsigned char freq_ech = 125;		// frequence d'échantillonage. valeur en Hz (100Hz fonctionne bien, ne pas passer sous 50)
const unsigned short entraxe = 350;		// distance entre le milieu de chaque roue. valeur en mm


//---------------------------------------------------------------------------
// 						PROTOTYPES DE FONCTIONS 							-
//--------------------------------------------------------------------------- 
void Ports_Init(void);
void Init(void);
void OrdreMoteur(unsigned char Sens, unsigned short VitesseG, unsigned short VitesseD);
void Led(unsigned char couleur);

void Deplacement(float distance, float angle);
void pid(float ConsigneDist, float consigneOrient);
void CmdMD(long c);
void CmdMG(long c);

void EnvoiCaractere(char DataCar);
void EnvoiChaine(char* DataString);
void EnvoiInt(int DataInt);
void EnvoiUInt(unsigned int DataInt);
void EnvoiFloat(const float DataFloat);
unsigned char ReceptCaractere(void);
unsigned char ReceptString(unsigned char *x, unsigned char size);

//###########################################################################
// 																			#
// 						DEBUT DU PROGRAMME 									#
// 																			#
//###########################################################################

int main (void) 
{
	Ports_Init();								// initialisations E/S
	Init();										// initialisation avant démarrage
	
	Consigne_Dist = 0;
	Consigne_Orient = 0;
	somme_erreur_dist = 0;
	somme_erreur_orient = 0;
	erreur_prec_dist = Consigne_Dist; 			// initialisation sur consigne de depart
	erreur_prec_orient = Consigne_Orient;
	
	tickD = 0;
	tickG = 0;
	Pos_atteinte = 0;
	step = 0;
	
	//EnvoiChaine("nsm");							// "new set of mesures" -> cafouillage
	//EnvoiChaine("\r\n");
	//_delay_ms(1000);
	EnvoiFloat(0.0);		
	EnvoiChaine("\r\n");
	
	/*char buffer [10];
	EnvoiChaine(itoa(65534,buffer,10)); EnvoiChaine("\r\n");
	EnvoiChaine(utoa(65534,buffer,10)); EnvoiChaine("\r\n");
	EnvoiChaine(ltoa(123450000,buffer,10)); EnvoiChaine("\r\n");
	EnvoiFloat(10.0);		
	EnvoiChaine("\r\n");
	EnvoiFloat(10000.8);		
	EnvoiChaine("\r\n");
	EnvoiFloat(109.0);		
	EnvoiChaine("\r\n");*/
	
	_delay_ms(1000);
	
//	Deplacement(0.5,0); 		
//Deplacement(0, 360);

	while (1) 
	{ 	
		if (Pos_atteinte == 1) // quand la consigne est atteinte on remet tout à zéro
		{
			Consigne_Dist = 0; 
			Consigne_Orient = 0;
			tickD = 0;
			tickG = 0;
			Pos_atteinte = 0;
			step = step + 1;
		}
		
		/*switch (step)	// faire un carré
		{
			case 0 :
				Deplacement(0.5,0); 
				break;
			
			case 1 :
				Deplacement(0,90); 			
				break;
				
			case 2 :
				Deplacement(0.5,0); 			
				break;
				
			case 3:
				Deplacement(0,90); 			
				break;	
				
			case 4 :
				Deplacement(0.5,0); 
				break;
			
			case 5:
				Deplacement(0,90); 			
				break;
				
			case 6 :
				Deplacement(0.5,0); 
				break;
			
			case 7 :
				Deplacement(0,90); 			
				break;
				
			default : 		
				break;
		}*/
		
		switch (step)	// faire un aller-retour
		{
			case 0 :
				Deplacement(0.5,0); 
				break;
			
			case 1 :
				Deplacement(0,180); 			
				break;
				
			case 2 :
				Deplacement(0.5,0); 			
				break;
				
			case 3:
				Deplacement(0,180); 			
				break;	
				
			default : 		
				break;
		}
		
		pid(Consigne_Dist, Consigne_Orient);	
		_delay_ms(8);							// PID à 100Hz (F=1/t)
	}
	
	return 1;
}



//---------------------------------------------------------------------------
// 						INITIALISATION DES E/S 								-
//--------------------------------------------------------------------------- 
void Ports_Init() 
{						
	DDRB = 0;			// tout en entrée
	DDRC = 0;
	DDRD = 0;
	
	// Configuration des broches en sorties
	sbi(MotDSens_DDR,  MotDSens_broche);
	sbi(MotDPwm_DDR,  MotDPwm_broche);
	sbi(MotGSens_DDR,  MotGSens_broche);
	sbi(MotGPwm_DDR,  MotGPwm_broche);
	sbi(Led1R_DDR,  Led1R_broche);
	sbi(Led1V_DDR,  Led1V_broche);
	
	// Configuration des broches en entrée
	cbi(EncodD_DDR, EncodD_broche);
	cbi(EncodG_DDR, EncodG_broche);
	cbi(EncodGSens_DDR, EncodGSens_broche);
	cbi(EncodDSens_DDR, EncodDSens_broche);
}



//---------------------------------------------------------------------------
// 						INITIALISATION AVANT DEMARRAGE 						-
//--------------------------------------------------------------------------- 
void Init() 
{
	cbi(MotDSens_portS, MotDSens_broche);	// Sens Moteur Droit à 0
	cbi(MotGSens_portS, MotGSens_broche);	// Sens Moteur Gauche à 0
	cbi(Led1R_portS, Led1R_broche);			// Led Rouge à 0
	cbi(Led1V_portS, Led1V_broche);			// Led Verte à 0

	
	// fast pwm 8bits sans prescaler -> apparament pas assez précis
	//TCCR1A = 0b10000001; 
	//TCCR1B = 0b00001001;	
    
	// fast pwm 10bits sans prescaler
	TCCR1A = 0b10100011;			
	TCCR1B = 0b00001001;
	PWMD = 0;					// mini =~240; maxi=1023
	PWMG = 0;					// mini =~240; maxi=1023
	
	EIMSK = 0b00000011;  		// Activer INT1 et INT0
    EICRA = 0b00001111; 		// Déclanchement sur front montant de INT1 et INT0
    sei();						// Activer interruptions
	
	
	//---------- Initialisation UART ----------
	UBRR0H = 0b00000000;	// transmettre à 9600bps
	UBRR0L = 0b10000001;
	
	//UBRR0H = 0b00000010;	// transmettre à 2400bps -> génère trop d'erreurs
	//UBRR0L = 0b00001000;
	
	//UBRR0H = 0b00000000;	// transmettre à 19200bps -> génère trop d'erreurs
	//UBRR0L = 0b01000000;
	
	UCSR0C = 0b00000110; 	// mode asynchrone, pas de paritée, 8bits de données, 1bit de stop 
	UCSR0B = 0b00011000;	// activer reception et transmition
}



//---------------------------------------------------------------------------
// 							INTERRUPTIONS EXTERNES 							-
//---------------------------------------------------------------------------
ISR(INT0_vect)		
{
	// si EncodDSens = 0 (on avance) alors incrémenter TickD sinon décrémenter
	if ( bit_is_clear(EncodDSens_portE, EncodDSens_broche))
	{
		tickD++;
	}
		else
		{
			tickD--;
		}	
}


ISR(INT1_vect)
{
	// si EncodGSens = 0 (on avance) alors décrémenter TickG sinon incrémenter (car cet encodeur est inversé)
	if ( bit_is_clear(EncodGSens_portE, EncodGSens_broche))
	{
		tickG--;
	}
		else
		{
			tickG++;
		}
}

//---------------------------------------------------------------------------
// 							FONCTION MOTEURS 								-
//--------------------------------------------------------------------------- 
void OrdreMoteur(unsigned char Sens, unsigned short VitesseG, unsigned short VitesseD)
{
	switch (Sens)
	{
		case 0 : 		// ne rien faire
			break;
			
		case 1 : 		// avancer
			cbi(MotGSens_portS, MotGSens_broche);
			cbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 2 : 		// reculer
			sbi(MotGSens_portS, MotGSens_broche);
			sbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 3 : 		// tourner à gauche
			sbi(MotGSens_portS, MotGSens_broche);
			cbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 4 : 		// tourner à droite
			cbi(MotGSens_portS, MotGSens_broche);
			sbi(MotDSens_portS, MotDSens_broche);
			break;
			
		default : 		// si Sens prend une valeur inconnue on ne fait rien
			break;
	}

	PWMG = VitesseG;
	PWMD = VitesseD;
}


//---------------------------------------------------------------------------
// 							FONCTION LED DEBUG 								-
//---------------------------------------------------------------------------
void Led(unsigned char Couleur)
{
	 switch (Couleur)
	{
		case 0 : 		// Rouge et Vert éteind
			cbi(Led1R_portS, Led1R_broche);
			cbi(Led1V_portS, Led1V_broche);
			break;
			
		case 1 : 		// Rouge
			cbi(Led1V_portS, Led1V_broche);
			sbi(Led1R_portS, Led1R_broche);
			break;
			
		case 2 : 		// Vert
			cbi(Led1R_portS, Led1R_broche);
			sbi(Led1V_portS, Led1V_broche);
			break;
			
		case 3 : 		// Rouge + Vert = Orange
			sbi(Led1R_portS, Led1R_broche);
			sbi(Led1V_portS, Led1V_broche);
			break;
			
		default : 		// si Couleur prend une valeur inconnue on ne fait rien
			break;
	}
}





//---------------------------------------------------------------------------
// 					FONCTION CALCUL DES CONSIGNES EN TICK 					-
//---------------------------------------------------------------------------
void Deplacement(float distance, float angle)
{
	if (distance != 0)
	{
		Consigne_Dist = ((distance*1000)/(M_PI*75) ) * (tick_trm*reduction); // en mètre
	}
		else {Consigne_Dist = 0;}
	
	if (angle != 0)
	{
		Consigne_Orient = (((M_PI*entraxe) / (360/angle)) / (M_PI*75)) * ((tick_trm*reduction)*2); // en degré
	}
		else {Consigne_Orient = 0;}
		
	//angle_rad = R_PI/2;					// rotation 1/4 de tour sens anti-horaire
	//Consigne_Orient = (((M_PI*entraxe) / (2*M_PI/(angle_rad))) / (M_PI*75)) * ((tick_trm*reduction)*2); // orientation en radian
	
	//Consigne_Dist = 18000;				// 18000 ticks = 1 tour de roue
	//Consigne_Orient = -42000; 			// rotation 1/4 de tour sens horaire
}

//---------------------------------------------------------------------------
// 					FONCTION ASSERVISSEMENT PID POLAIRE 					-
//---------------------------------------------------------------------------
void pid(float ConsigneDist, float ConsigneOrient)
{
	float compteurD;
	float compteurG;	
	float mesure_dist;
	float mesure_orient;
	float cmd_dist;
	float cmd_orient;
	
	if (ConsigneDist != 0 || ConsigneOrient != 0)	// si consignes <> de zéro
	{
		// affectation des compteurs de PID et reset des compteurs sur interruption
		compteurD = tickD;
		compteurG = tickG;
		
		// mesure distance et orientation
		mesure_dist = (compteurD + compteurG)/2;
		mesure_orient = compteurD - compteurG;		// (compteurD - compteurG) / entraxe ?? 
	 
		// Calcul des erreurs de distance
		erreur_dist = ConsigneDist - mesure_dist;
		somme_erreur_dist += erreur_dist;
		delta_erreur_dist = erreur_dist - erreur_prec_dist;
	  
		// Calcul des erreurs d'orientation
		erreur_orient = ConsigneOrient - mesure_orient;
		somme_erreur_orient += erreur_orient;
		delta_erreur_orient = erreur_orient - erreur_prec_orient;
	 
		// calcul des commandes
		cmd_dist = ((kp_dist * erreur_dist) + (ki_dist * somme_erreur_dist) + (kd_dist * delta_erreur_dist));	// PID distance
		cmd_orient = ((kp_orient * erreur_orient) + (ki_orient * somme_erreur_orient) + (kd_orient * delta_erreur_orient)); // PID orientation
		
		// appliquer les commandes aux moteur
		CmdMG(cmd_dist - cmd_orient);
		CmdMD(cmd_dist + cmd_orient);

		// mise à jour de l'erreur précédente
		erreur_prec_dist = erreur_dist;	
		erreur_prec_orient = erreur_orient;
		
		//EnvoiChaine("cmd_orient: "); EnvoiFloat(cmd_orient);EnvoiChaine("      | "); EnvoiChaine("cmd_dist: "); EnvoiFloat(cmd_dist); EnvoiChaine("\r\n");	//debug	
			
		
		Led(Vert);						// on indique que le pid est actif par la led verte
						//-0.9				//0.9					//-1				//1
		if (((cmd_dist > -9) && (cmd_dist < 9)) && ((cmd_orient > -9) && (cmd_orient < 9)))	// si la position est dans une fourchette acceptable par rapport à la consigne 
		{
			Pos_atteinte = 1;
		} 
			else {Pos_atteinte = 0;}
	}
		else 							// si les consignes sont à 0 on desactive le PID et on l'indique par la led rouge
		{ 
			OrdreMoteur(1, 0, 0); 
			Led(Rouge);
		}
}



//---------------------------------------------------------------------------
// 			FONCTION COMMANDE MOTEUR DROIT PAR ASSERVISSEMENT POLAIRE		-
//---------------------------------------------------------------------------
void CmdMD(long c)
{	
	if (c>=0) {cbi(MotDSens_portS, MotDSens_broche);}				// avancer
		else {sbi(MotDSens_portS, MotDSens_broche); c = c * -1;} 	// reculer. multiplie par -1 pour avoir un pwm positif

	if (c < 240) {c = 260;}
	if (c > 1023) {c = 500;}

	PWMD = c;														// appliquer le pwm
}


//---------------------------------------------------------------------------
// 			FONCTION COMMANDE MOTEUR GAUCHE	PAR ASSERVISSEMENT POLAIRE		-
//---------------------------------------------------------------------------
void CmdMG(long c)
{
	if (c>=0) {cbi(MotGSens_portS, MotGSens_broche);}
		else {sbi(MotGSens_portS, MotGSens_broche); c = c * -1;}
			
	if (c < 240) {c = 260;}
	if (c > 1023) {c = 500;}
	
	PWMG = c;
}



//---------------------------------------------------------------------------
// 						FONCTION ENVOI CARACTERE							-
//---------------------------------------------------------------------------
void EnvoiCaractere(char DataCar)
{ 
   while(!(UCSR0A & (1<<UDRE0)))	// attendre jusqu'a ce que le transmetteur soit prêt
   {
      //ne rien faire
   }

   //UCSR0A |= (1<<TXC0); 			// effacer txc flag
   UDR0 = DataCar;						// écrire la donnée dans le buffer de l'USART
}


//---------------------------------------------------------------------------
// 						FONCTION ENVOI STRING								-
//---------------------------------------------------------------------------
void EnvoiChaine(char* DataString)
{
	while(*DataString != 0x00)
	{
		EnvoiCaractere(*DataString);
		DataString++;
	}
}



//---------------------------------------------------------------------------
// 						FONCTION ENVOI INTEGER	V2							-
//---------------------------------------------------------------------------
void EnvoiInt(int Dataint)
{
	unsigned char minus, centmil, mil, cent, dix, unit;
	int memory;
	
	memory = Dataint;
	if (Dataint<0)					// detecter signe du nombre
	{
		Dataint = Dataint*-1;
		minus = 1;
	} 
		else {minus = 0;}
		
	unit = (Dataint%10) | 0x30;	// unités
	Dataint/= 10;					// stock le quotient de la division dans Dataint
	dix = (Dataint%10) | 0x30;		// dixaines
	Dataint/= 10;
	cent = (Dataint%10) | 0x30;	// centaines
	Dataint/= 10;
	mil = (Dataint%10) | 0x30;		// milliers
	Dataint/= 10;
	centmil = (Dataint%10) | 0x30;	// centaines de milliers
	
	if (minus == 1) {EnvoiCaractere('-');}	// si c'est un chiffre negatif, l'indiquer
	
	/*if(centmil != '0'){EnvoiCaractere(centmil);}
		else if( (centmil != '0') && (mil != '0') ){EnvoiCaractere(mil);}
			else if((centmil != '0') && (mil != '0') && (cent != '0')){EnvoiCaractere(cent); }
				else if((centmil != '0') && (mil != '0') && (cent != '0') && (dix != '0')){EnvoiCaractere(dix); }*/
				
	if(memory > 9999){EnvoiCaractere(centmil); EnvoiCaractere(mil); EnvoiCaractere(cent); EnvoiCaractere(dix);}
		else if( memory > 999 ){EnvoiCaractere(mil); EnvoiCaractere(cent); EnvoiCaractere(dix);}
			else if(memory > 99){EnvoiCaractere(cent); EnvoiCaractere(dix);}
				else if( memory > 9){EnvoiCaractere(dix); }			
	EnvoiCaractere(unit); // l'unité est toujours envoyée même si vaut 0
}


//---------------------------------------------------------------------------
// 						FONCTION ENVOI UNSIGNED INTEGER						-
//---------------------------------------------------------------------------
void EnvoiUInt(unsigned int Dataint)
{
	unsigned char centmil, mil, cent, dix, unit;
	int memory;
	
	memory = Dataint;
	
	unit = (Dataint%10) | 0x30;	// unités
	Dataint/= 10;					// stock le quotient de la division dans Dataint
	dix = (Dataint%10) | 0x30;		// dixaines
	Dataint/= 10;
	cent = (Dataint%10) | 0x30;	// centaines
	Dataint/= 10;
	mil = (Dataint%10) | 0x30;		// milliers
	Dataint/= 10;
	centmil = (Dataint%10) | 0x30;	// centaines de milliers

	if(memory > 9999){EnvoiCaractere(centmil); EnvoiCaractere(mil); EnvoiCaractere(cent); EnvoiCaractere(dix);}
		else if( memory > 999 ){EnvoiCaractere(mil); EnvoiCaractere(cent); EnvoiCaractere(dix);}
			else if(memory > 99){EnvoiCaractere(cent); EnvoiCaractere(dix);}
				else if( memory > 9){EnvoiCaractere(dix); }			
	EnvoiCaractere(unit); // l'unité est toujours envoyée même si vaut 0
}

//---------------------------------------------------------------------------
// 						FONCTION ENVOI FLOAT								-
//---------------------------------------------------------------------------
void EnvoiFloat(const float DataFloat)
{
	unsigned int v,p;
	long int num;
	
	num = DataFloat*1000;
	p = num%1000;
	num = num/1000;
	v = num;

	EnvoiInt(v);
	EnvoiCaractere('.');
	EnvoiUInt(p);
}


//---------------------------------------------------------------------------
// 						FONCTION RECEPTION CHAR								-
//---------------------------------------------------------------------------
unsigned char ReceptCaractere(void)
{
 
 while(!(UCSR0A & (1<<RXC0)));
 return UDR0;
 
}

asserv_polaire_vitesse.c: Voir le code
//###########################################################################
// 				ROBOT PID - CARTE ASSERVISSEMENT 							#
// 																			#
// VERSION			:	1 													#
// DATE DEPART		:	07/03/2015 											#
// DATE ACTUELLE	:	17/04/2015 											#
// AUTEUR			:	Frédéric .C 										#
// 																			#
// NOTE				: 	ASSERVISSEMENT POLAIRE + VITESSE					#
//						Une freq. d'ech. de 100Hz au lieu de 50Hz accroît	#
//						nettement la précision du système.					#
// 																			#
// A FAIRE			:	variables pid à déplacer en local si possible		#
//																			#
//	-----------------------------------------------------------------------	#
//						X_position = distance * sin(theta);					#
//						Y_position = distance * cos(theta);					#
//						multiplier theta * (180.0/PI) pour avoir 			#
//						orientation en degré.								#
//###########################################################################

//	Encodeurs 		: 360 tick/tr moteur
//	Réducteur 		: 50:1
//	Roues			: 75mm de diamètre
//	Diamètre robot	: 350mm

//	tick/tr de roue	: 18000 tick/tr de roue  (360 x 50)
//	Périmètre roue	: 235,6mm (PI X 75)

//---------------------------------------------------------------------------
// 						LIBRAIRIES											-
//--------------------------------------------------------------------------- 
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>		// http://www.nongnu.org/avr-libc/user-manual/group__avr__interrupts.html
#include <compat/deprecated.h> 	// pour les fonctions sbi et cbi
#include <math.h>				// pi, cos, sin, etc.
#include <stdlib.h>				// itoa, ltoa, utoa -> à remplacer par sprintf() ?

//---------------------------------------------------------------------------
// 						DEFINITION DES CONSTANTES 							-
//---------------------------------------------------------------------------

#define F_CPU 20000000UL

// MOTEURS TRACTION
#define	MotDSens_DDR		DDRD		// REGISTRE
#define	MotDSens_portS		PORTD		// PORT (en sortie)
#define	MotDSens_broche		PD5			// BROCHE

#define	MotDPwm_DDR			DDRB		
#define	MotDPwm_portS		PORTB		
#define	MotDPwm_broche		PB1

#define	MotGSens_DDR		DDRD		
#define	MotGSens_portS		PORTD		
#define	MotGSens_broche		PD6			

#define	MotGPwm_DDR			DDRB		
#define	MotGPwm_portS		PORTB		
#define	MotGPwm_broche		PB2
	

// ENCODEURS
#define	EncodD_DDR			DDRD
#define	EncodD_portE		PIND		// PORT (en entree)
#define	EncodD_broche		PD2			// INT0

#define	EncodDSens_DDR		DDRD
#define	EncodDSens_portE	PIND		
#define	EncodDSens_broche	PD7

#define	EncodG_DDR			DDRD
#define	EncodG_portE		PIND		
#define	EncodG_broche		PD3			// INT1

#define	EncodGSens_DDR		DDRB
#define	EncodGSens_portE	PINB		
#define	EncodGSens_broche	PB0


// LED DEBUG (bicolore vert/rouge)
#define	Led1R_DDR			DDRC	
#define	Led1R_portS			PORTC		
#define	Led1R_broche		PC2
#define	Led1V_DDR			DDRC		
#define	Led1V_portS			PORTC		
#define	Led1V_broche		PC3


// EQUIVALENCES DIVERSES
#define	Avancer			1
#define	Reculer			2
#define	Gauche			3
#define	Droite			4

#define PWMD			OCR1A			// Registre PWM droit
#define PWMG			OCR1B			// Registre PWM gauche

#define	Rouge			1
#define	Vert			2
#define	Orange			3

//---------------------------------------------------------------------------
// 						DEFINITION DES VARIABLES GLOBALES 					-
//---------------------------------------------------------------------------
float tickD, tickG, tickD2, tickG2;						// tick moteur droit, moteur gauche sur interruption
short angle_deg;
short angle_rad;
float dist_metre;
unsigned char Pos_atteinte ;			// mis à 1 quand position voulue est atteinte
unsigned char step;						// est incrémenté à chaque étapes (déplacement et/ou rotation) du trajet

//-------------------------------- asservissement en distance
const float kp_dist = 0.6;	
const float ki_dist = 0;
const float kd_dist = 0.1;

float Consigne_Dist; 
float erreur_prec_dist;
float erreur_dist;
float somme_erreur_dist;
float delta_erreur_dist;

//-------------------------------- asservissement en angle
const float kp_orient = 0.6;			
const float ki_orient = 0;
const float kd_orient = 0.1;

float Consigne_Orient; 			
float erreur_prec_orient;
float erreur_orient;			
float somme_erreur_orient;
float delta_erreur_orient;

//-------------------------------- asservissement en vitesse
const float kp_vit = 0.7; 	//70;	70 qd on mesure en tr/s; 0.7 qd on mesure en cm/s		
const float ki_vit = 0.1;	//10;
const float kd_vit = 0.02;	//2;

float Consigne_Vit;		
float Consigne_Vit_Max;			
float erreur_prec_vd;
float erreur_prec_vg;
	
float erreur_vd;			// erreurs lues (terme proportionnel)
float erreur_vg;			
float somme_erreur_vd;		// total des erreurs (terme intégrale)
float somme_erreur_vg;
float delta_erreur_vd;		// variation des erreurs (terme dérivé)
float delta_erreur_vg;

//-------------------------------- caractéristiques du robot
const float perim_roue = 235.6; 		// perimetre des roues. valeur en mm
const unsigned char reduction = 50;
const unsigned short tick_trm = 360;	// nombre de tick par tour moteur
const unsigned char freq_ech = 125;		// frequence d'échantillonage. valeur en Hz (100Hz fonctionne bien, ne pas passer sous 50)
const unsigned short entraxe = 350;		// distance entre le milieu de chaque roue. valeur en mm

//--------------------------------- variables i2c
unsigned char DataReg;

//---------------------------------------------------------------------------
// 						PROTOTYPES DE FONCTIONS 							-
//--------------------------------------------------------------------------- 
void Ports_Init(void);
void Init(void);
void OrdreMoteur(unsigned char Sens, unsigned short VitesseG, unsigned short VitesseD);
void Led(unsigned char couleur);

void Deplacement(float distance, float angle);
void pid(float ConsigneDist, float consigneOrient);
void CmdMD(long c);
void CmdMG(long c);

void EnvoiCaractere(char DataCar);
void EnvoiChaine(char* DataString);
void EnvoiInt(int DataInt);
void EnvoiUInt(unsigned int DataInt);
void EnvoiFloat(const float DataFloat);
unsigned char ReceptCaractere(void);
unsigned char ReceptString(unsigned char *x, unsigned char size);

void TwiStart(void);
void TwiAdr(unsigned char Adresse);
void TwiWrite(unsigned char Data);
void TwiStop(void);

void TwiReadSlave(void);
void TWI_match_read_slave(void); 

void TWI_match_write_slave(void);
void TWI_write_slave(unsigned char Data);

//###########################################################################
// 																			#
// 						DEBUT DU PROGRAMME 									#
// 																			#
//###########################################################################

int main (void) 
{
	Ports_Init();								// initialisations E/S
	Init();										// initialisation avant démarrage
	
	Consigne_Dist = 0;
	Consigne_Orient = 0;
	somme_erreur_dist = 0;
	somme_erreur_orient = 0;
	erreur_prec_dist = Consigne_Dist; 			// initialisation sur consigne de depart
	erreur_prec_orient = Consigne_Orient;
	
	Consigne_Vit = 0;				// consigne de vitesse à 0 au départ
	somme_erreur_vd = 0;
	somme_erreur_vg = 0;
	erreur_prec_vd = Consigne_Vit; 	// initialisation sur consigne de depart
	erreur_prec_vg = Consigne_Vit;
	
	tickD = 0;
	tickG = 0;
	tickD2 = 0;
	tickG2 = 0;
	Pos_atteinte = 0;
	step = 0;
	
	//EnvoiChaine("nsm");							// "new set of mesures" -> cafouillage
	//EnvoiChaine("\r\n");
	//_delay_ms(1000);
	EnvoiFloat(0.0);		
	EnvoiChaine("\r\n");
	
	/*char buffer [10];
	EnvoiChaine(itoa(65534,buffer,10)); EnvoiChaine("\r\n");
	EnvoiChaine(utoa(65534,buffer,10)); EnvoiChaine("\r\n");
	EnvoiChaine(ltoa(123450000,buffer,10)); EnvoiChaine("\r\n");
	EnvoiFloat(10.0);		
	EnvoiChaine("\r\n");
	EnvoiFloat(10000.8);		
	EnvoiChaine("\r\n");
	EnvoiFloat(109.0);		
	EnvoiChaine("\r\n");*/
	
	_delay_ms(1000);
	
//	Deplacement(0.5,0); 		
//Deplacement(0, 360);
	Consigne_Vit = 0; 
	Consigne_Vit_Max = 50;// 50cm/s
	
	Led(Rouge);
	
	while (1) 
	{ 	
		TWI_match_read_slave(); 
		TwiReadSlave();
		if (DataReg == 12) 
		{
			Led(Vert); 
			_delay_ms(1000);
			TWI_match_write_slave(); 
			TWI_write_slave(16);
		} 
			else {Led(Rouge);}
			
			
		/*if (Pos_atteinte == 1) // quand la consigne est atteinte on remet tout à zéro
		{
			Consigne_Dist = 0; 
			Consigne_Orient = 0;
			Consigne_Vit = 0;
			tickD = 0;
			tickG = 0;
			Pos_atteinte = 0;
			step = step + 1;
		}
		
		
		switch (step)	// faire un aller-retour
		{
			case 0 :
				Deplacement(0.5,0); 
				break;
			case 1 :
				Deplacement(0,180); 			
				break;
			case 2 :
				Deplacement(0.5,0); 			
				break;
			case 3:
				Deplacement(0,180); 			
				break;	
			default : 		
				break;
		}
		
		pid(Consigne_Dist, Consigne_Orient);	
		_delay_ms(8);							// PID à 100Hz (F=1/t)*/
	}
	
	return 1;
}



//---------------------------------------------------------------------------
// 						INITIALISATION DES E/S 								-
//--------------------------------------------------------------------------- 
void Ports_Init() 
{						
	DDRB = 0;			// tout en entrée
	DDRC = 0;
	DDRD = 0;
	
	// Configuration des broches en sorties
	sbi(MotDSens_DDR,  MotDSens_broche);
	sbi(MotDPwm_DDR,  MotDPwm_broche);
	sbi(MotGSens_DDR,  MotGSens_broche);
	sbi(MotGPwm_DDR,  MotGPwm_broche);
	sbi(Led1R_DDR,  Led1R_broche);
	sbi(Led1V_DDR,  Led1V_broche);
	
	// Configuration des broches en entrée
	cbi(EncodD_DDR, EncodD_broche);
	cbi(EncodG_DDR, EncodG_broche);
	cbi(EncodGSens_DDR, EncodGSens_broche);
	cbi(EncodDSens_DDR, EncodDSens_broche);
}



//---------------------------------------------------------------------------
// 						INITIALISATION AVANT DEMARRAGE 						-
//--------------------------------------------------------------------------- 
void Init() 
{
	cbi(MotDSens_portS, MotDSens_broche);	// Sens Moteur Droit à 0
	cbi(MotGSens_portS, MotGSens_broche);	// Sens Moteur Gauche à 0
	cbi(Led1R_portS, Led1R_broche);			// Led Rouge à 0
	cbi(Led1V_portS, Led1V_broche);			// Led Verte à 0

	
	// fast pwm 8bits sans prescaler -> apparament pas assez précis
	//TCCR1A = 0b10000001; 
	//TCCR1B = 0b00001001;	
    
	// fast pwm 10bits sans prescaler
	TCCR1A = 0b10100011;			
	TCCR1B = 0b00001001;
	PWMD = 0;					// mini =~240; maxi=1023
	PWMG = 0;					// mini =~240; maxi=1023
	
	EIMSK = 0b00000011;  		// Activer INT1 et INT0
    EICRA = 0b00001111; 		// Déclanchement sur front montant de INT1 et INT0
    sei();						// Activer interruptions
	
	
	//---------- Initialisation UART ----------
	UBRR0H = 0b00000000;	// transmettre à 9600bps
	UBRR0L = 0b10000001;
	
	//UBRR0H = 0b00000010;	// transmettre à 2400bps -> génère trop d'erreurs
	//UBRR0L = 0b00001000;
	
	//UBRR0H = 0b00000000;	// transmettre à 19200bps -> génère trop d'erreurs
	//UBRR0L = 0b01000000;
	
	UCSR0C = 0b00000110; 	// mode asynchrone, pas de paritée, 8bits de données, 1bit de stop 
	UCSR0B = 0b00011000;	// activer reception et transmition

	//---------- Initialisation TWI (I2C) ------
	TWAR=0x20;	// adresse de cette carte esclave
}



//---------------------------------------------------------------------------
// 							INTERRUPTIONS EXTERNES 							-
//---------------------------------------------------------------------------
ISR(INT0_vect)		
{
	// si EncodDSens = 0 (on avance) alors incrémenter TickD sinon décrémenter
	if ( bit_is_clear(EncodDSens_portE, EncodDSens_broche))
	{
		tickD++;
		tickD2++; // pour asservissement vitesse
	}
		else
		{
			tickD--;
			tickD2++;
		}	
}


ISR(INT1_vect)
{
	// si EncodGSens = 0 (on avance) alors décrémenter TickG sinon incrémenter (car cet encodeur est inversé)
	if ( bit_is_clear(EncodGSens_portE, EncodGSens_broche))
	{
		tickG--;
		tickG2++;	// pour asservissement vitesse
	}
		else
		{
			tickG++;
			tickG2++;
		}
}

//---------------------------------------------------------------------------
// 							FONCTION MOTEURS 								-
//--------------------------------------------------------------------------- 
void OrdreMoteur(unsigned char Sens, unsigned short VitesseG, unsigned short VitesseD)
{
	switch (Sens)
	{
		case 0 : 		// ne rien faire
			break;
			
		case 1 : 		// avancer
			cbi(MotGSens_portS, MotGSens_broche);
			cbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 2 : 		// reculer
			sbi(MotGSens_portS, MotGSens_broche);
			sbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 3 : 		// tourner à gauche
			sbi(MotGSens_portS, MotGSens_broche);
			cbi(MotDSens_portS, MotDSens_broche);
			break;
			
		case 4 : 		// tourner à droite
			cbi(MotGSens_portS, MotGSens_broche);
			sbi(MotDSens_portS, MotDSens_broche);
			break;
			
		default : 		// si Sens prend une valeur inconnue on ne fait rien
			break;
	}

	PWMG = VitesseG;
	PWMD = VitesseD;
}


//---------------------------------------------------------------------------
// 							FONCTION LED DEBUG 								-
//---------------------------------------------------------------------------
void Led(unsigned char Couleur)
{
	 switch (Couleur)
	{
		case 0 : 		// Rouge et Vert éteind
			cbi(Led1R_portS, Led1R_broche);
			cbi(Led1V_portS, Led1V_broche);
			break;
			
		case 1 : 		// Rouge
			cbi(Led1V_portS, Led1V_broche);
			sbi(Led1R_portS, Led1R_broche);
			break;
			
		case 2 : 		// Vert
			cbi(Led1R_portS, Led1R_broche);
			sbi(Led1V_portS, Led1V_broche);
			break;
			
		case 3 : 		// Rouge + Vert = Orange
			sbi(Led1R_portS, Led1R_broche);
			sbi(Led1V_portS, Led1V_broche);
			break;
			
		default : 		// si Couleur prend une valeur inconnue on ne fait rien
			break;
	}
}





//---------------------------------------------------------------------------
// 					FONCTION CALCUL DES CONSIGNES EN TICK 					-
//---------------------------------------------------------------------------
void Deplacement(float distance, float angle)
{
	if (distance != 0)
	{
		Consigne_Dist = ((distance*1000)/(M_PI*75) ) * (tick_trm*reduction); // en mètre
	}
		else {Consigne_Dist = 0;}
	
	if (angle != 0)
	{
		Consigne_Orient = (((M_PI*entraxe) / (360/angle)) / (M_PI*75)) * ((tick_trm*reduction)*2); // en degré
	}
		else {Consigne_Orient = 0;}
		
	//angle_rad = R_PI/2;					// rotation 1/4 de tour sens anti-horaire
	//Consigne_Orient = (((M_PI*entraxe) / (2*M_PI/(angle_rad))) / (M_PI*75)) * ((tick_trm*reduction)*2); // orientation en radian
	
	//Consigne_Dist = 18000;				// 18000 ticks = 1 tour de roue
	//Consigne_Orient = -42000; 			// rotation 1/4 de tour sens horaire
}

//---------------------------------------------------------------------------
// 					FONCTION ASSERVISSEMENT PID POLAIRE 					-
//---------------------------------------------------------------------------
void pid(float ConsigneDist, float ConsigneOrient)
{
	float compteurD;
	float compteurG;	
	float mesure_dist;
	float mesure_orient;
	float cmd_dist;
	float cmd_orient;
	
	float dist_restante;	// pour trapèze de vitesse
	float orient_restante;
	
	if (ConsigneDist != 0 || ConsigneOrient != 0)	// si consignes <> de zéro
	{
		// affectation des compteurs de PID et reset des compteurs sur interruption
		compteurD = tickD;
		compteurG = tickG;
		
		// mesure distance et orientation
		mesure_dist = (compteurD + compteurG)/2;
		mesure_orient = compteurD - compteurG;		// (compteurD - compteurG) / entraxe ?? 
		
		// Calcul des erreurs de distance
		erreur_dist = ConsigneDist - mesure_dist;
		somme_erreur_dist += erreur_dist;
		delta_erreur_dist = erreur_dist - erreur_prec_dist;
	  
		// Calcul des erreurs d'orientation
		erreur_orient = ConsigneOrient - mesure_orient;
		somme_erreur_orient += erreur_orient;
		delta_erreur_orient = erreur_orient - erreur_prec_orient;
	 
		// calcul des commandes
		cmd_dist = ((kp_dist * erreur_dist) + (ki_dist * somme_erreur_dist) + (kd_dist * delta_erreur_dist));	// PID distance
		cmd_orient = ((kp_orient * erreur_orient) + (ki_orient * somme_erreur_orient) + (kd_orient * delta_erreur_orient)); // PID orientation
		
		// Calcul trapèze de vitesse
		if (ConsigneOrient == 0)
		{
			dist_restante = ConsigneDist - mesure_dist;		//mesure de la distance restant à parcourir
			if (dist_restante < (ConsigneDist/3))			//si dist < 1/3 distance totale...
				{
					if(Consigne_Vit > 0){Consigne_Vit -= 0.6;} else {Consigne_Vit = 0;}	//...alors, s'il est possible de décélérer, on décélére
				} 
				else if ((dist_restante > (ConsigneDist*(2/3))) && (Consigne_Vit < Consigne_Vit_Max) ){Consigne_Vit += 0.6;}
		}
			else 
			{
				orient_restante = ConsigneOrient - mesure_orient;
				if (orient_restante < (ConsigneOrient/3))//si orient < 1/3 distance totale on décelere
				{
					if(Consigne_Vit > 0){Consigne_Vit -= 0.6;} else {Consigne_Vit = 0;}
				} 
				else if ((orient_restante > (ConsigneOrient*(2/3))) && (Consigne_Vit < Consigne_Vit_Max) ){Consigne_Vit += 0.6;}
			}
		// fin calcul trapèze de vitesse
	
		// appliquer les commandes aux moteur
		CmdMG(cmd_dist - cmd_orient);
		CmdMD(cmd_dist + cmd_orient);
		
		// mise à jour de l'erreur précédente
		erreur_prec_dist = erreur_dist;	
		erreur_prec_orient = erreur_orient;
		
		//EnvoiChaine("cmd_orient: "); EnvoiFloat(cmd_orient);EnvoiChaine("      | "); EnvoiChaine("cmd_dist: "); EnvoiFloat(cmd_dist); EnvoiChaine("\r\n");	//debug	
			
		
		Led(Vert);						// on indique que le pid est actif par la led verte
						//-0.9				//0.9					//-1				//1
		if (((cmd_dist > -4) && (cmd_dist < 4)) && ((cmd_orient > -4) && (cmd_orient < 4)))	// si la position est dans une fourchette acceptable par rapport à la consigne 
		{
			Pos_atteinte = 1;
		} 
			else {Pos_atteinte = 0; }
	}
		else 							// si les consignes sont à 0 on desactive le PID et on l'indique par la led rouge
		{ 
			OrdreMoteur(1, 0, 0); 
			Led(Rouge);
		}
}



//---------------------------------------------------------------------------
// 			FONCTION COMMANDE MOTEUR DROIT PAR ASSERVISSEMENT POLAIRE		-
//---------------------------------------------------------------------------
void CmdMD(long c)
{	
	float compteurVD;
	float mesure_vd;
	
	if (c>=0) {cbi(MotDSens_portS, MotDSens_broche);}				// avancer
		else {sbi(MotDSens_portS, MotDSens_broche); c = c * -1;} 	// reculer. multiplie par -1 pour avoir un pwm positif
	
	// affectation des compteurs de PID et reset des compteurs sur interruption
	compteurVD = tickD2;
	tickD2 = 0;

	mesure_vd = (freq_ech*compteurVD)/tick_trm/reduction;	// vitesse mesurée en tr/s
	mesure_vd = mesure_vd * (perim_roue/10);				// vitesse mesurée en cm/s
	
	//calcul des erreurs
	erreur_vd = Consigne_Vit - mesure_vd;
	somme_erreur_vd += erreur_vd;
	delta_erreur_vd = erreur_vd - erreur_prec_vd;

	// calcul de la commande
	PWMD += ((kp_vit * erreur_vd) + (ki_vit * somme_erreur_vd) + (kd_vit * delta_erreur_vd));

	// Normalisation des commandes PWM de sortie (le moteur ne bouge pas avec un pwm < 240)
	if (PWMD < 250) {PWMD = 250;}
		else if (PWMD > 1023) {PWMD = 1023;}

	// mise à jour de l'erreur précédente
	erreur_prec_vd = erreur_vd;	
	
/*	if (c < 240) {c = 255;}
	if (c > 1023) {c = 500;}

	PWMD = c;	*/													// appliquer le pwm
}


//---------------------------------------------------------------------------
// 			FONCTION COMMANDE MOTEUR GAUCHE	PAR ASSERVISSEMENT POLAIRE		-
//---------------------------------------------------------------------------
void CmdMG(long c)
{
	float compteurVG;
	float mesure_vg;
	
	if (c>=0) {cbi(MotGSens_portS, MotGSens_broche);}
		else {sbi(MotGSens_portS, MotGSens_broche); c = c * -1;}
	
	// affectation des compteurs de PID et reset des compteurs sur interruption
	compteurVG = tickG2;
	tickG2 = 0;
	
	mesure_vg = (freq_ech*compteurVG)/tick_trm/reduction;
	mesure_vg = mesure_vg * (perim_roue/10);
	
	//calcul des erreurs
	erreur_vg = Consigne_Vit - mesure_vg;
	somme_erreur_vg += erreur_vg;
	delta_erreur_vg = erreur_vg - erreur_prec_vg;
		
	// calcul de la commande
	PWMG += ((kp_vit * erreur_vg) + (ki_vit * somme_erreur_vg) + (kd_vit * delta_erreur_vg));
		
	// Normalisation des commandes PWM de sortie (le moteur ne bouge pas avec un pwm < 240)
	if (PWMG < 250) {PWMG = 250;}
		else if (PWMG > 1023) {PWMG = 1023;} 
			
	// mise à jour de l'erreur précédente
	erreur_prec_vg = erreur_vg;
	
	/*if (c < 240) {c = 255;}
	if (c > 1023) {c = 500;}
	
	PWMG = c;*/
}



//---------------------------------------------------------------------------
// 						FONCTION ENVOI CARACTERE							-
//---------------------------------------------------------------------------
void EnvoiCaractere(char DataCar)
{ 
   while(!(UCSR0A & (1<<UDRE0)))	// attendre jusqu'a ce que le transmetteur soit prêt
   {
      //ne rien faire
   }

   //UCSR0A |= (1<<TXC0); 			// effacer txc flag
   UDR0 = DataCar;						// écrire la donnée dans le buffer de l'USART
}


//---------------------------------------------------------------------------
// 						FONCTION ENVOI STRING								-
//---------------------------------------------------------------------------
void EnvoiChaine(char* DataString)
{
	while(*DataString != 0x00)
	{
		EnvoiCaractere(*DataString);
		DataString++;
	}
}



//---------------------------------------------------------------------------
// 						FONCTION ENVOI INTEGER	V2							-
//---------------------------------------------------------------------------
void EnvoiInt(int Dataint)
{
	unsigned char minus, centmil, mil, cent, dix, unit;
	int memory;
	
	memory = Dataint;
	if (Dataint<0)					// detecter signe du nombre
	{
		Dataint = Dataint*-1;
		minus = 1;
	} 
		else {minus = 0;}
		
	unit = (Dataint%10) | 0x30;	// unités
	Dataint/= 10;					// stock le quotient de la division dans Dataint
	dix = (Dataint%10) | 0x30;		// dixaines
	Dataint/= 10;
	cent = (Dataint%10) | 0x30;	// centaines
	Dataint/= 10;
	mil = (Dataint%10) | 0x30;		// milliers
	Dataint/= 10;
	centmil = (Dataint%10) | 0x30;	// centaines de milliers
	
	if (minus == 1) {EnvoiCaractere('-');}	// si c'est un chiffre negatif, l'indiquer
	
	/*if(centmil != '0'){EnvoiCaractere(centmil);}
		else if( (centmil != '0') && (mil != '0') ){EnvoiCaractere(mil);}
			else if((centmil != '0') && (mil != '0') && (cent != '0')){EnvoiCaractere(cent); }
				else if((centmil != '0') && (mil != '0') && (cent != '0') && (dix != '0')){EnvoiCaractere(dix); }*/
				
	if(memory > 9999){EnvoiCaractere(centmil); EnvoiCaractere(mil); EnvoiCaractere(cent); EnvoiCaractere(dix);}
		else if( memory > 999 ){EnvoiCaractere(mil); EnvoiCaractere(cent); EnvoiCaractere(dix);}
			else if(memory > 99){EnvoiCaractere(cent); EnvoiCaractere(dix);}
				else if( memory > 9){EnvoiCaractere(dix); }			
	EnvoiCaractere(unit); // l'unité est toujours envoyée même si vaut 0
}


//---------------------------------------------------------------------------
// 						FONCTION ENVOI UNSIGNED INTEGER						-
//---------------------------------------------------------------------------
void EnvoiUInt(unsigned int Dataint)
{
	unsigned char centmil, mil, cent, dix, unit;
	int memory;
	
	memory = Dataint;
	
	unit = (Dataint%10) | 0x30;	// unités
	Dataint/= 10;					// stock le quotient de la division dans Dataint
	dix = (Dataint%10) | 0x30;		// dixaines
	Dataint/= 10;
	cent = (Dataint%10) | 0x30;	// centaines
	Dataint/= 10;
	mil = (Dataint%10) | 0x30;		// milliers
	Dataint/= 10;
	centmil = (Dataint%10) | 0x30;	// centaines de milliers

	if(memory > 9999){EnvoiCaractere(centmil); EnvoiCaractere(mil); EnvoiCaractere(cent); EnvoiCaractere(dix);}
		else if( memory > 999 ){EnvoiCaractere(mil); EnvoiCaractere(cent); EnvoiCaractere(dix);}
			else if(memory > 99){EnvoiCaractere(cent); EnvoiCaractere(dix);}
				else if( memory > 9){EnvoiCaractere(dix); }			
	EnvoiCaractere(unit); // l'unité est toujours envoyée même si vaut 0
}

//---------------------------------------------------------------------------
// 						FONCTION ENVOI FLOAT								-
//---------------------------------------------------------------------------
void EnvoiFloat(const float DataFloat)
{
	unsigned int v,p;
	long int num;
	
	num = DataFloat*1000;
	p = num%1000;
	num = num/1000;
	v = num;

	EnvoiInt(v);
	EnvoiCaractere('.');
	EnvoiUInt(p);
}


//---------------------------------------------------------------------------
// 						FONCTION RECEPTION CHAR								-
//---------------------------------------------------------------------------
unsigned char ReceptCaractere(void)
{
 
 while(!(UCSR0A & (1<<RXC0)));
 return UDR0;
 
}



//---------------------------------------------------------------------------
// 						FONCTION START I2C									-
//---------------------------------------------------------------------------
void TwiStart(void)
{
	TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
		while ((TWCR & (1<<TWINT)) == 0);
}

//---------------------------------------------------------------------------
// 						FONCTION STOP I2C									-
//---------------------------------------------------------------------------
void TwiStop(void)
{
	TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN);
}

//---------------------------------------------------------------------------
// 						FONCTION ENVOI ADRESSE I2C							-
//---------------------------------------------------------------------------
void TwiAdr(unsigned char Adresse)
{
    TWDR=Adresse;
    TWCR=(1<<TWINT)|(1<<TWEN);    	// Clear TWI interrupt flag,Enable TWI
    while (!(TWCR & (1<<TWINT))); 	// Wait till complete TWDR byte transmitted
    while((TWSR & 0xF8)!= 0x18);  	// Check for the acknowledgement
}


//---------------------------------------------------------------------------
// 						FONCTION ECRITURE I2C								-
//---------------------------------------------------------------------------
void TwiWrite(unsigned char Data)
{
    TWDR = Data;  
    TWCR = (1<<TWINT)|(1<<TWEN);    	// Clear TWI interrupt flag,Enable TWI
    while (!(TWCR & (1<<TWINT))); 		// Wait till complete TWDR byte transmitted
    while ((TWSR & 0xF8) != 0x28); 	// Check for the acknowledgement
}


//---------------------------------------------------------------------------
// 				FONCTION LECTURE DE SON PROPRE REGISTRE I2C					-
//---------------------------------------------------------------------------
void TwiReadSlave(void) 
{
    // Clear TWI interrupt flag,Get acknowledgement, Enable TWI
    TWCR= (1<<TWINT)|(1<<TWEA)|(1<<TWEN);   
    // Wait for TWINT flag
    while (!(TWCR & (1<<TWINT)));
    // Wait for acknowledgement
    while((TWSR & 0xF8)!=0x80);
    // Get value from TWDR
    DataReg=TWDR;

}


void TWI_match_read_slave(void)
{
    // Loop till correct acknoledgement have been received
    while((TWSR & 0xF8)!= 0x60)
    {
        // Get acknowlegement, Enable TWI, Clear TWI interrupt flag
        TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);    
        // Wait for TWINT flag
        while (!(TWCR & (1<<TWINT))); 
    }
}
//-----------------------------------------------------------------------------

void TWI_match_write_slave(void)
{
    // Loop till correct acknowledgement have been received
    while((TWSR & 0xF8)!= 0xA8)
    {
        // Get acknowledgment, Enable TWI, Clear TWI interrupt flag
        TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);    
        while (!(TWCR & (1<<TWINT)));  // Wait for TWINT flag
    }
}


void TWI_write_slave(unsigned char Data) // Function to write data
{
    // Fill TWDR register with the data to be sent 
    TWDR= Data;
    // Enable TWI, Clear TWI interrupt flag 
    TWCR= (1<<TWEN)|(1<<TWINT);
    // Wait for the acknowledgement
    while((TWSR & 0xF8) != 0xC0);
}
				

Ces programmes ont montrés la faisabilité d'un tel système. aussi je n'ai pas pris le temps de les fignoler ou de nettoyer le code. Je préfère me consacrer à la version 2.

Code source et schéma

Photos et vidéos

Photos du robot

Vidéos du robot

Mes sources

Je me suis aidé des sites suivants :