controle_moteur.c.svn-base

Basic PID to drive two motors.

/*
 * Controle_moteur.c
 *
 *  Created on: 28 janv. 2009
 *      Author: Adrien
 */

#include "Controle_moteur.h"

int envoyer_consigne(robot* r) {
	double distance;
	double distance_entre_les_roues = 270;

	// SIMULATION DES CONSIGNES ET DE L ODOMETRIE
	// ICI IL FAUDRA METTRE LES VRAIES COMMANDES QUI ENVOIENT DE VRAIES CONSIGNES AU MOTEUR !

	if(r->consigne_d > MOTEUR_CONSIGNE_MAX)
	{
		r->consigne_d = MOTEUR_CONSIGNE_MAX;
	}
	if(r->consigne_d < MOTEUR_CONSIGNE_MIN)
	{
		r->consigne_d = MOTEUR_CONSIGNE_MIN;
	}
	if(r->consigne_g > MOTEUR_CONSIGNE_MAX)
	{
		r->consigne_g = MOTEUR_CONSIGNE_MAX;
	}
	if(r->consigne_g < MOTEUR_CONSIGNE_MIN)
	{
		r->consigne_g = MOTEUR_CONSIGNE_MIN;
	}
	// calcul de la distance parcourue
	distance = (r->consigne_d + r->consigne_g) /2;

	// calcul de la nouvelle orientation
	r->orientation += (r->consigne_d - r->consigne_g) / (distance_entre_les_roues);

	while (r->orientation>PI) {
		r->orientation -= 2*PI;
	}
	while (r->orientation<-PI) {
		r->orientation += 2*PI;
	}


	// calcul de la nouvelle position
	r->position.x += distance * cos(r->orientation);
	r->position.y += distance * sin(r->orientation);



	// FIN SIMULATION CONSIGNE ET ODOMETRIE

	return 0;
}