Go Down

Topic: Utillisation de la bibliothèque MsTimer2 (Read 1 time) previous topic - next topic

amine

May 16, 2011, 02:49 am Last Edit: May 16, 2011, 09:17 am by amine Reason: 1
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 :smiley-eek:
Voici mon code ...

Code: [Select]
#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

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy