asservissement + capteur ultrason

Salut a tous

j 'utilise deux moteurs que j 'aimerais asservir en vitesse

Comme Dispositif ou matériel utilise:

  • une voiture robot avec ces deux moteurs Dc
  • deux capteurs rotatifs incrémentaux (gauche & droite) de types optocoupleur
  • un Motorschield (L298N) pour l asservissement de mes moteurs
  • un capteur Ultrasons hc-sr04).

Mon but est de pouvoir faire rouler ma voiture a une vitesse donnée lorsque le capteur ultrason ne reconnaît pas d’obstacle d’où l’asservissement. Lorsqu’il reconnaît l’obstacle par le capteur la voiture doit pouvoir s’arrêter

Pour cela j ‘ai écrit un code. Mais le problème est qu’ il ne roule même pas il reste toujours sur place . Je ne sais pas la raison

#include <SimpleTimer.h>

SimpleTimer timer; 

// définition des pins de l'Arduino qui contrôlent le  moteur droit (jaune)
#define pinIN1 8
#define pinIN2 12
#define pinENA 6 // doit être une pin PWM     

// définition des pins de l'Arduino qui contrôlent le  moteur gauche(Bleue)
#define pinIN3 13
#define pinIN4 4
#define pinENB 5 // doit être une pin PWM   


#define trigger 9 // Arduino Pin an HC-SR04 Trig  doit etre un pwm (Couleur orange )
#define echo  7 // Arduino Pin an HC-SR04 Echo doit etre pwm (cable blanc)

volatile long distance = 0;

volatile int  comptageImpulsion_gauche=0; // variable accessible dans la routine interruption externe 0
volatile int comptageImpulsion_droite=0; // variable accessible dans la routine interruption externe 1

const int radius = 3.325;


int timerId;
         
volatile int vitMoteur = 0.;      
volatile int vitMoteur_gauche = 0.; 
volatile  double vitesse_droite ;
volatile  double vitesse_gauche ;
volatile  double vit ;
volatile  double erreur;

// nombre de ticks sur le disque codeur
const int pas= 20 ;

// consigne du moteur
double consigne_moteur= 30.0; // cm/s

// init calculs asservissement PID
double erreur_precedente= consigne_moteur;  // (en cm/s)
double somme_erreur = 0;
double erreur_precedente_gauche= consigne_moteur;  // (en cm/s)
double somme_erreur_gauche = 0;


//Definition des constantes du correcteur PID
double kp = 0.4;           // Coefficient proportionnel    
double ki = 2;              // Coefficient intégrateur
double kd = 1;           // Coefficient dérivateur



// duree d asservissement
double temps_asservissement = 1000; // millisseconde


double dt = temps_asservissement /1000 ;  // seconde

unsigned long duree ;


void setup() {

  Serial.begin(9600);


  // Moteur Gauche
  pinMode(pinIN1, OUTPUT);
  pinMode(pinIN2, OUTPUT);
  pinMode(pinENA, OUTPUT);
  analogWrite(pinENA, 0);  // Initialisation sortie moteur à 0 
  delay(300);                // Pause de 0,3 sec pour laisser le temps au moteur de s'arréter si celui-ci est en marche
// Moteur droit
  pinMode(pinIN3, OUTPUT);
  pinMode(pinIN4, OUTPUT);
  pinMode(pinENB, OUTPUT);
  analogWrite(pinENB, 0);  // Initialisation sortie moteur à 0 
  delay(300);                // Pause de 0,3 sec pour laisser le temps au moteur de s'arréter si celui-ci est en marche



attachInterrupt(1, vitessedroite, RISING );
attachInterrupt(0, vitessegauche, RISING);

timerId= timer.setInterval(temps_asservissement, asservissement);


}

void loop() {

timer.run(); 

/***** Pas d'obstacle il doit rouler ********/
while (distance>=20)
{
  distance=getdistance();
  Serial.println("avance");
  asservissement();
  //Serial.print("Distance: ");
  //Serial.println(distance);
}


/***** Reconnaissance d'obstacle  ********/
while (distance<20)
{
  distance=getdistance();
  Serial.println("arret");
  arret_vehicule();
  Serial.print("Distance: ");
  Serial.println(distance);
}


  


}

void asservissement(){

noInterrupts();
int gauche = comptageImpulsion_gauche;
int droite=comptageImpulsion_droite;

// reinitialisation de mon tick de la codeuse
 comptageImpulsion_gauche= 0; //  reset counter to zero
 comptageImpulsion_droite= 0;


/******* calcul des vitesses en cm/s ********/
double rotation_gauche = ( gauche/ pas)/dt ;      // vitesse du moteur gauche en tour/s
double vitesse_gauche = ( 2*3.14*radius)*rotation_gauche; // cm /s
double rotation_droite = (droite/ pas)/dt ;// vitesse du moteur droit en  1 tour/s
double vitesse_droite = (2*3.14*radius)*rotation_droite;
 
double vit_droite = vitesse_droite;
double vit_gauche = vitesse_gauche;

/******* fin de calu des vitesses en cm/s ********/



/******* Régulation PID moteur droit ********/
 double erreur = consigne_moteur-vit_droite;
somme_erreur=somme_erreur+ erreur ;
double delta = erreur -erreur_precedente;
erreur_precedente = erreur;
vitMoteur = (kp*erreur +ki*somme_erreur+ kd*delta) ;
 /******* FIN Régulation PID moteur Droit ********/

/******* Régulation PID moteur gauche ********/
 double erreur_gauche = consigne_moteur-vit_gauche;
somme_erreur_gauche=somme_erreur_gauche+ erreur ;
double delta_gauche = erreur -erreur_precedente_gauche;
erreur_precedente_gauche = erreur_gauche;
vitMoteur_gauche = (kp*erreur_gauche +ki*somme_erreur_gauche+ kd*delta_gauche) ;
 /******* Régulation PID moteur gauche********/


/******* En cas de saturation ********/
if (vitMoteur_gauche > 255) {
   vitMoteur_gauche= 255;  // sachant que l'on est branché sur un pont en H L298
 } 
 else if (vitMoteur_gauche <0) {
  vitMoteur_gauche= 0;
 }
 
if (vitMoteur > 255) {
   vitMoteur = 255;  // sachant que l'on est branché sur un pont en H L298
 } 
 else if (vitMoteur <0) {
  vitMoteur = 0;
 }
/******* En cas de saturation ********/

/******* Sortir PWM moteur gauche et droite ********/
avancer_droite(vitMoteur);
avancer_gauche(vitMoteur_gauche);
/******* Sortir PWM moteur gauche et droite ********/

interrupts();

/******* lire dans le moniteur ********/
 Serial.print("vitMoteur_gauche :");
 Serial.print(vitMoteur_gauche);
 Serial.println();

Serial.print("vitMoteur_droite:");
Serial.print(vitMoteur);
Serial.println();
   

  }


 void avancer_gauche(int Powerrate){
  analogWrite( pinENA, Powerrate );
  digitalWrite( pinIN1, false );
  digitalWrite( pinIN2, true );}


  void avancer_droite (int power){
  // Moteur droit 
   analogWrite( pinENB, power );
  digitalWrite( pinIN3, LOW );
  digitalWrite( pinIN4, HIGH );
  }

void vitessegauche() {// la fonction appelée par l'interruption externe n°0

comptageImpulsion_gauche=comptageImpulsion_gauche+1; // Incrémente la variable de comptage

}

void vitessedroite() {// la fonction appelée par l'interruption externe n°1

comptageImpulsion_droite=comptageImpulsion_droite+1; // Incrémente la variable de comptage

}

int getdistance()
{
  long temps=0;
  
 // noInterrupts();

  // Lance une mesure de distance en envoyant une impulsion HIGH de 10µs sur la broche TRIGGER *
  digitalWrite(trigger, HIGH); //Trigger Impuls 10 us
  delayMicroseconds(10);
  digitalWrite(trigger, LOW);

  
  temps = pulseIn(echo, HIGH); // Echo-temps messen
 // interrupts();
  temps = (temps/2); // temps halbieren
  distance = temps / 29.1; // temps in Zentimeter umrechnen
  return(distance);
}



void arret_vehicule(){
  
 analogWrite(pinENA, 0); 
  digitalWrite(pinIN1, true); 
 digitalWrite(pinIN2, false);


  analogWrite(pinENB, 0); 
  digitalWrite(pinIN3, true); 
  digitalWrite(pinIN4, false);
  
  }

Comme Dispositif ou matériel utilise:

  • une voiture robot avec ces deux moteurs Dc
  • deux capteurs rotatifs incrémentaux (gauche & droite) de types optocoupleur
  • un Motorschield (L298N) pour l asservissement de mes moteurs
  • un capteur Ultrasons hc-sr04).

Ps de batterie dans la liste du matériel, difficlie de faire autre chose que du sur place....

il y a une batterie de 12 v qui alimente le Motorshield et l 'Arduino

Au départ distance est initialisé à 0

Ensuite on essaye d'entrer dans la boucle

/***** Pas d'obstacle il doit rouler ********/
while (distance>=20)
{
  distance=getdistance();
  Serial.println("avance");

mais comme distance=0 la condition n'est pas remplie et ça ne roule pas.

Il faut placer l'instruction distance=getdistance();
avant de réaliser le test.

je l’ai fait mais la voiture reste toujours sur place , elle ne sort pas de ce While-loop (Voir fichier PNG attachee)

while (distance<20)
{
  distance=getdistance();
  Serial.println("arret");
  arret_vehicule();
  Serial.print("Distance: ");
  Serial.println(distance);
}