Pages: [1]   Go Down
Author Topic: Utillisation de la bibliothèque MsTimer2  (Read 1024 times)
0 Members and 1 Guest are viewing this topic.
France
Offline Offline
Newbie
*
Karma: 0
Posts: 38
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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
« Last Edit: May 16, 2011, 02:17:43 am by amine » Logged

Pages: [1]   Go Up
Jump to: