Bonjour,
Mon projet est de faire de l'odométrie interne en temps réel à l'aide de codeurs rotatifs ...
Ce que j'ai réussi à maîtriser avec mon robot à ce jour:
- Reconnaître le sens de rotation des roues et incrémenter / décrémenter les pas en conséquence en utilisant les interrupts;
- Calculer l'absolu les coordonnées (x, y, theta) de mon robot à un instant t;
- Utilisation de la librairie MsTimer2, pour exécuter la fonction d'odométrie, avec une période de 5 ms;
Le problème, c'est que quand j'ai ajouté à tout ça la commande des moteurs pour faire mon robot aller à un point de coordonnées données... les moteurs ne fonctionnent pas
Voici mon code ...
#include <MsTimer2.h>
///////////////////////////////////////////////////////
//////////////////////VARIABLES////////////////////////
///////////////////////////////////////////////////////
// variables et pins des codeurs et interrupts
int encoderPinB[2] = {25,22}; // Encoder B pin
long encoderTicks[2];
int odoGPin = 4; // 4 pour la patte Digital 19
int odoDPin = 5; // 5 pour la patte Digital 18
// Définition des constantes
#define COEF_D 11.0184191 // 1 cm -> 11 pas incrémentaux
#define COEF_ROT 4.352083 // 1 degré -> 4,35 pas
#define PI 3.14159265
#define CONV_RD PI/(180*COEF_ROT) //conversion de teta en pasà teta en radian
// Définition des variables globales
long distance,distance_precedente ; // dist : distance parcourue à l'instant tn ; distance_precedente : distance parcourue à l'instant tn-1
long orient, orient_precedente, orient_init ; // orient : angle mesuré à l'instant tn , orient_precedente : angle mesuré à l'instant tn-1 ; orient_init : angle initial
float x, y; // coordonnées (x,y) en pas
int x_cm, y_cm ; // coordonnées (x,y) en cm
// Variables et constantes Commande de moteurs
#define TOPSPEED 200
#define FORWARD_SPEED 150
#define TURN_SPEED 60
// motor Gauche
int dir1PinA = 13;
int dir2PinA = 12;
int speedPinA = 10;
// motor Droit
int dir1PinB = 11;
int dir2PinB = 8;
int speedPinB = 9;
int oam_speed[2]={0}; //oam_speed[0] = gauche; oam_speed[1] = droite
// variables d'odometrie
int delta_d, delta_orient; // differentiel de distance , differentiel d'angle
long orient_moy ; // moyenne entre angle et angle précedent
float orient_moy_radian, delta_orient_radian ; // Les mêmes convertis en rad
float K=1, dx,dy; // K=1 : interpretation linéaire ; dx,dy : differentiels en x et y
///////////////////////////////////////////////////////
//////////////////////FUNCTIONS////////////////////////
///////////////////////////////////////////////////////
// Conditions initiales
void fonction_initialisation (float x0, float y0, long teta0)
{
orient_init = teta0 ;
orient_precedente = teta0 ;
x = x0 ;
y = y0 ;
distance = 0 ;
distance_precedente=0 ;
}
// Interruption executée toutes les 5ms
void update()
{
calcul_xy () ; // La fonction qui nous intéresse
// Garder l'angle dans un intervalle [0,2*pi]
while(orient_moy_radian<0){orient_moy_radian+=2*PI;}
while(orient_moy_radian>2*PI){orient_moy_radian-=2*PI;}
}
//Calcul de x et y
void calcul_xy (void)
{
distance = ((encoderTicks[1]+encoderTicks[0])/2); // distance en pas parcourue à tn
orient = orient_init +(encoderTicks[1]-encoderTicks[0]); //correspond à qn mais en pas
delta_d = distance - distance_precedente; // correspond à L mais en pas
delta_orient = orient - orient_precedente; // correspond à Dqn mais en pas
orient_moy = (orient +orient_precedente) / 2; // correspond à qmoy en pas
delta_orient_radian = CONV_RD*delta_orient; // correspond à Dqn en rd
orient_moy_radian = CONV_RD*orient_moy; // correspond à qmoy en rd
dx=K*delta_d*cos(orient_moy_radian);
dy=K*delta_d*sin(orient_moy_radian);
x= x + dx; // valeurs exprimées dans le système d’unité robot
y= y + dy;
x_cm=(int)(x/COEF_D); // Ce calcul peut être fait en dehors de la fonction
y_cm=(int)(y/COEF_D); // quand on en a besoin
orient_precedente = orient ; // actualisation de qn-1
distance_precedente = distance ; //actualisation de Dn-1
}
// Codeur incrémental gauche
void incL(){
if (digitalRead(encoderPinB[0]) == HIGH){
encoderTicks[0]++;
} else {
encoderTicks[0]--; // arrêt du moteur
}
}
// Codeur incrémental droit
void incR(){
if (digitalRead(encoderPinB[1]) == LOW){
encoderTicks[1]++;
} else {
encoderTicks[1]--; // arrêt du moteur
}
}
void init_encoders(){
pinMode(encoderPinB[0], INPUT); // Encoder pin B INPUT
pinMode(encoderPinB[1], INPUT); // Encoder pin B INPUT
}
void setup (){
fonction_initialisation(0,0,0);
// démarrage de la liaison série
Serial.begin(38400);
init_motors(); // Motor Pin intialization
init_encoders(); //Encoders Pin initialization
// attach the odometer interrupt
attachInterrupt(odoGPin, incL, RISING); //18
attachInterrupt(odoDPin, incR, RISING); //19
// utilisation du timer 2 pour gérer l'asservissement
MsTimer2::set(5, update); // 500ms period
MsTimer2::start();
}
void loop(){
while(orient_moy_radian*180/PI < 360){
analogWrite(speedPinA,TOPSPEED);
analogWrite(speedPinB,TOPSPEED);
digitalWrite(dir1PinA, LOW);
digitalWrite(dir2PinA, HIGH);
digitalWrite(dir1PinB, HIGH);
digitalWrite(dir2PinB, LOW);
}
/Serial.print(encoderTicks[0]);
Serial.print("|");
Serial.print(encoderTicks[1]);
Serial.print("\tx = ");
Serial.print(x_cm);
Serial.print("| y = ");
Serial.print(y_cm);
Serial.print("\tTheta = ");
Serial.print(orient_moy_radian*180/PI);
Serial.print("| Pas = ");
Serial.println(orient_moy);
}
En fait, ce qui marche quand j'uploade ce code, c'est l'incrémentation des codeurs et le calcul d'angle et de coordonnées ( tout marche nickel, et je peux voir les Serial).
Alors pourquoi les moteurs ne tournent pas? Ai-je mal placé les commandes de moteurs?
Merci