Aller au contenu


Photo
- - - - -

Robot 2WD homologable pour la coupe de france de robotique réalisé au dernier moment

Odométrie Codeur Encodeur Quadrature asservissement Moteur CC PID

45 réponses à ce sujet

#21 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 29 janvier 2019 - 12:25

D'accord voilà le programme

// Manipulation des ports
// PIND :  RX TX 2 3 4 5 6 7
// https://www.arduino.cc/en/Reference/PortManipulation


#include <PID_v1.h>   // Pour le PID
#include <TimerOne.h> // Pour le timer

// commande du moteur droit
#define ENCODEURDROITA 2
#define ENCODEURDROITB 4
#define MOTEURDROITA 6       // contrôle vitesse moteur 1; PWM
#define MOTEURDROITB 10       // controle direction moteur 1
#define MOTEURDROITC 11

// commande du moteur 2
#define ENCODEURGAUCHEA 3
#define ENCODEURGAUCHEB 7
#define MOTEURGAUCHEA 5       // contrôle vitesse moteur 2; PWM
#define MOTEURGAUCHEB 8     // controle direction moteur 2
#define MOTEURGAUCHEC 9      // controle direction moteur 2


double SetpointDroit, InputDroit, OutputDroit;
double SetpointGauche, InputGauche, OutputGauche;

double Kp = 0.8, Ki = 5, Kd = 0;

PID PIDDROIT(&InputDroit, &OutputDroit, &SetpointDroit, Kp, Ki, Kd, DIRECT);
PID PIDGAUCHE(&InputGauche, &OutputGauche, &SetpointGauche, Kp, Ki, Kd, DIRECT);

// volatile => pour toute variable qui seront utilisées dans les interruptions
volatile int8_t countDroit = 0; /* comptage de tick d'encoder qui sera incrémenté sur interruption " On change " sur l'interruption 0  max 127 attention à l'overflow*/
volatile int8_t countGauche = 0; /* comptage de tick d'encoder qui sera incrémenté sur interruption " On change " sur l'interruption 1 max 127 attention à l'overflow */
volatile double speedDroit = 0; // vitesse du moteur
volatile double speedGauche = 0; // vitesse du moteur

unsigned long time0 = 0; // stock un temps à un instant donné
unsigned long timer = 0; // variable qui va contenir le dernier temps enregistré via millis

int8_t valeur = 0;  //lecture de la consigne via le moniteur serie pour test, initialisé à 40 pour test rapide du PID

void setup()
{
  //initialisation moniteur serie
  Serial.begin(9600);       // facultatif uniquement pour feedback
  Serial.println("cool");
  // on initialise les entrées et sorties
  pinMode(ENCODEURDROITA, INPUT_PULLUP);
  pinMode(ENCODEURDROITB, INPUT_PULLUP);
  pinMode(ENCODEURGAUCHEA, INPUT_PULLUP);
  pinMode(ENCODEURGAUCHEB, INPUT_PULLUP);
  
  

  pinMode(MOTEURDROITA, OUTPUT);
  pinMode(MOTEURDROITB, OUTPUT);
  pinMode(MOTEURDROITC, OUTPUT);
  pinMode(MOTEURGAUCHEA, OUTPUT);
  pinMode(MOTEURGAUCHEB, OUTPUT);
  pinMode(MOTEURGAUCHEC, OUTPUT);

  // moteurs à l'arret
  stopMotors();

  // Encoder quadrature counter
  attachInterrupt(0, counterDroit, CHANGE); // lorsqu'il y une modification sur le pin 2, on interrompte le programme pour enclencher le comptage
  attachInterrupt(1, counterGauche, CHANGE); // lorsqu'il y une modification sur le pin 3, on interrompte le programme pour enclencher le comptage

  Timer1.initialize(100);//80000            // set a timer of length  0.08 sec => permet d'avoir speed le plus proche de 127 sans dépasser 127 en vitesse max ! 
  Timer1.attachInterrupt( timerIsr );

  // PID
  InputDroit = 0;
  SetpointDroit = 0;
  InputGauche = 0;
  SetpointGauche = 0;
  //active le PID
  PIDDROIT.SetMode(AUTOMATIC);
  PIDDROIT.SetOutputLimits(-255, 255);
  PIDGAUCHE.SetMode(AUTOMATIC);
  PIDGAUCHE.SetOutputLimits(-255, 255);
}

void loop() {
  //while(1){test();}
  if (Serial.available()) {
    time0 = timer;
    valeur = Serial.parseInt(); //récupération des caractères sur le port série
    Serial.println("---------------------------valeur");Serial.println(valeur);
    
  }
  if (valeur != 0) {
    runMotorSpeed(valeur,valeur);
  }
  else {
    stopMotors();
  }
  timer = millis();
  if ( (timer - time0) > 90000) { //Timeout !
    valeur = 0 ;
    stopMotors();
    time0 = timer;
    
  }

  Serial.println("**************************speed1");
  Serial.println(speedDroit*100);                 // ne voit pas la speed evolué
  Serial.println("**************************speed2");
  Serial.println(speedGauche*100);                 // just 100 ou -100
  Serial.println("'''''''''''''''''");    // à commenter, utilisé pour débug

}



void counterDroit()
{
  byte state = PIND;
  (((state >> ENCODEURDROITA) & 1) ^ ((state >> ENCODEURDROITB) & 1)) ? countDroit-- : countDroit++; // régler ++ et -- pour que faire avancer => +
}

// Encoder counter 2

void counterGauche()
{
  byte state = PIND;
  (((state >> ENCODEURGAUCHEA) & 1 ) ^ ((state >> ENCODEURGAUCHEB) & 1)) ? countGauche++ : countGauche--; // régler ++ et -- pour que faire avancer => +
}


// Timer pour calculer la vitesse grace aux encodeurs

void timerIsr()
{
  speedDroit = countDroit;
  countDroit = 0;
  speedGauche = countGauche;
  countGauche = 0;
}

void stopMotors() {
  digitalWrite(MOTEURDROITA, HIGH);
  digitalWrite(MOTEURDROITB, HIGH);
  digitalWrite(MOTEURGAUCHEA, HIGH);
  digitalWrite(MOTEURGAUCHEB, HIGH);
}


void test() {

  digitalWrite(MOTEURDROITA, LOW);
  digitalWrite(MOTEURDROITB, LOW); // low fait avancer
  digitalWrite(MOTEURGAUCHEA, HIGH);
  digitalWrite(MOTEURGAUCHEB, LOW); // Low fait reculer

  Serial.println("speed1");
  Serial.println(speedDroit*100);                 // à commenter, utilisé pour débug
  Serial.println("speed2");
  Serial.println(speedGauche*100);                 // à commenter, utilisé pour débug
  Serial.println("'''''''''''''''''");    // à commenter, utilisé pour débug
}

void avancerMoteurDroit(uint8_t a) // En avant
{
  analogWrite (MOTEURDROITA, a); // Contrôle de vitesse en PWM, moteur 1
  digitalWrite(MOTEURDROITB, LOW); // sens de marche avec HIGH
   digitalWrite(MOTEURDROITC, HIGH); // sens de marche avec HIGH
}
void avancerMoteurGauche(uint8_t a) // En avant
{
  analogWrite (MOTEURGAUCHEA, 255 - a); // Contrôle de vitesse en PWM, moteur 2
  digitalWrite(MOTEURGAUCHEB, HIGH);
  digitalWrite(MOTEURGAUCHEC, LOW);
}

void reculerMoteurDroit (uint8_t a) // En arrière
{
  analogWrite (MOTEURDROITA, 255 - a);
  digitalWrite(MOTEURDROITB, HIGH);
  digitalWrite(MOTEURDROITC, LOW);
}
void reculerMoteurGauche (uint8_t a) // En arrière
{
  analogWrite (MOTEURGAUCHEA, a);
  digitalWrite(MOTEURGAUCHEB, LOW);
  digitalWrite(MOTEURGAUCHEC, HIGH);
}


void runMotorSpeed( int8_t commandeMoteurDroit, int8_t commandeMoteurGauche) {

  SetpointDroit = commandeMoteurDroit;
  SetpointGauche = commandeMoteurGauche;

  InputDroit = speedDroit;
  InputGauche = speedGauche;

  PIDDROIT.Compute();
  PIDGAUCHE.Compute();

  int outputDroit = (int)OutputDroit;
  int outputGauche = (int)OutputGauche;

  //utilisation de la sortie du PID pour asservir les moteurs
  if (outputDroit >= 0) {
    avancerMoteurDroit(outputDroit);
  }
  if (outputDroit < 0) {
    reculerMoteurDroit(-outputDroit);
  }
  if (outputGauche >= 0) {
    avancerMoteurGauche(outputGauche);
  }
  if (outputGauche < 0) {
    reculerMoteurGauche(-outputGauche);
  }
  // à commenter, utilisé pour débug
  Serial.println("output1");
  Serial.println(outputDroit);                
  Serial.println("output2");
  Serial.println(outputGauche);                
  Serial.println("'''''''''''''''''"); 
  Serial.println("-------------------speed1");
  Serial.println(InputDroit);                 // à commenter, utilisé pour débug
  Serial.println("-------------------speed2");
  Serial.println(speedGauche);                 // à commenter, utilisé pour débug
  Serial.println("'''''''''''''''''");       
}

Je n'arrive pas à voir la speed1 ou speed2 évolue, je vois seulement 100 ou -100 sur l'un ou l'autre des moteurs quand je tape -10 ou 10.

 

Merci de cette réponse rapide.

 

 



#22 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 29 janvier 2019 - 04:17

quand tu fais tes essais tu utilises bien une arduino nano ou uno ? ou bien tu utilises ce code sur une autre arduino ?


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#23 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 30 janvier 2019 - 10:16

J'utilise une Uno.

 

Il se peut que pour le sens des moteurs, j'ai inversé les pin mais ce qui compte c'est que je puisse assigner à chaque moteur une vitesse, dans un sens ou dans l'autre et pouvoir visualiser les 2 vitesses (positive dans un sens , negative dans l'autre) 



#24 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 30 janvier 2019 - 10:46

Oui je pense que tu as inverser le sens des moteurs. 

 

essaye d'inverser  les ++ et -- dans cette partie : 

 

void counterDroit()
{
  byte state = PIND;
  (((state >> ENCODEURDROITA) & 1) ^ ((state >> ENCODEURDROITB) & 1)) ? countDroit-- : countDroit++; // régler ++ et -- pour que faire avancer => +
}

// Encoder counter 2

void counterGauche()
{
  byte state = PIND;
  (((state >> ENCODEURGAUCHEA) & 1 ) ^ ((state >> ENCODEURGAUCHEB) & 1)) ? countGauche++ : countGauche--; // régler ++ et -- pour que faire avancer => +
}


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#25 gerardosamara

gerardosamara

    Membre passionné

  • Membres
  • PipPipPip
  • 373 messages
  • Gender:Male
  • Location:Costa Rica & Bretagne
  • Interests:La vie sous les tropiques

Posté 30 janvier 2019 - 04:18

Petite qtestion : quand le PID est complètement désactivé dans le code , le problème existe t'il toujours ?

Parce que à mon avis quand on met un moteur à gauche et un moteur à droite sur un chassis , il faut inverser le sens de rotation pour un moteur soit au niveau electrique soit au niveau digitalWwerite.

Pura vida

 

Ma chaine youtube  https://www.youtube....EQ5MTR3A/videos

Tutoriel MIT Inventor2  https://www.robot-ma...e-robot-mobile/


#26 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 30 janvier 2019 - 04:44

Il est important de faire en sorte que ces fonctions marchent : 

 

void avancerMoteurDroit(uint8_t a) // En avant
{
  analogWrite (MOTEURDROITA, a); // Contrôle de vitesse en PWM, moteur 1
  digitalWrite(MOTEURDROITB, LOW); // sens de marche avec HIGH
   digitalWrite(MOTEURDROITC, HIGH); // sens de marche avec HIGH
}
void avancerMoteurGauche(uint8_t a) // En avant
{
  analogWrite (MOTEURGAUCHEA, 255 - a); // Contrôle de vitesse en PWM, moteur 2
  digitalWrite(MOTEURGAUCHEB, HIGH);
  digitalWrite(MOTEURGAUCHEC, LOW);
}

void reculerMoteurDroit (uint8_t a) // En arrière
{
  analogWrite (MOTEURDROITA, 255 - a);
  digitalWrite(MOTEURDROITB, HIGH);
  digitalWrite(MOTEURDROITC, LOW);
}
void reculerMoteurGauche (uint8_t a) // En arrière
{
  analogWrite (MOTEURGAUCHEA, a);
  digitalWrite(MOTEURGAUCHEB, LOW);
  digitalWrite(MOTEURGAUCHEC, HIGH);
}

 

La fonction avancerMoteurDroit doit faire avancer le robot avec le moteur droit  etc...
En fonction du câblage sur le moteur et des cartes utiliser il faudra adapter ces fonctions ... 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#27 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 30 janvier 2019 - 05:14

J'ai inversé les ++ et -- et réciproquement. 

 

Au lieu de 80000, je mets 800 pour l'échantillonnage, sinon les moteurs saccadent.

 

Ce qui m'importe c'est que les deux moteurs tournent en même temps et à la même vitesse. 

 

J'ai filmé les reactions du moteurs.

 

Merci

 

Vous avez la video en pj?

 

Fichier joint  essaie pid 2.m4v.zip   9,3 Mo   24 téléchargement(s)



#28 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 30 janvier 2019 - 05:49

tu peux mettre la vidéo dans un dossier .zip et mettre le dossier .zip en pj


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#29 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 01 février 2019 - 03:19

Bonjour,

 

J'ai un programme que j'avais deja pour contrôler deux moteurs, et ils tournent parfaitement à la même vitesse et reste synchrones. Mes deux moteurs sont drivés avec un L289N et je relis sa masse à celle de l'Arduino.

 

J'ai inclus  void runMotorSpeed( int8_t commandeMoteurDroit, int8_t commandeMoteurGauche

 

Maintenant, j'arrive bien à detecter les vitesses des deux moteurs, alors que je n'y arrivai pas avec votre programme.

 

Mais par contre les deux moteurs ne vont pas à la même vitesse. J'ai l'impression que l'encodage de la vitesse de l'un entre en confusion avec l'autre.

 

Peut être faut'il mettre un mask comme dans votre tout premier programme?

 

Je vous mets mon programme, si vous pouvez m'éclairer là c'est trop compliqué pour moi.

 

Merci

Fichier(s) joint(s)



#30 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 02 février 2019 - 04:05

Je suis désolé mais je suis un peu perdu dans les problèmes rencontrés ... 


1) Il est dit que vous avez un programme où les deux moteurs tournent à la même vitesse ? Quel était le problème pourquoi changer de programme ? 

2) Maintenant vous arrivez à détecter les vitesses alors qu'avant vous n'y arriviez pas ? Qu'avez vous changer ? 

 

3) Le programme joint c'est celui qui correspond à la vidéo? 

 

Oublions le PID 2 minutes et vérifions les bases. 

A ) définir le sens positif de chaque moteur. 
B ) Faire pour chaque moteur la fonction qui permet d'avancer et de reculer. 
C ) Tester pour chaque moteur ces deux fontions, faire les tests un par un, puis avec toutes les combinaisons d'avancer et de reculer :  AA AR RR RA
D ) Faire les fonctions pour les codeurs 
E ) Tester les fonctions pour les codeurs indépendamment un moteur par un moteur et vérifier que le + correspond à avancer et le moins à reculer. 

 

Quand tous ces points seront validés viendra ensuite le moment de :

D) Implanter le PID
F) Tuner le PID.


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#31 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 04 février 2019 - 04:07

Merci Mike pour tous les conseils. 

 

C bon, je controle mes moteurs dans les mêmes sens et à la même vitesse. 

 

J'ai simplifié comme ça la fonction de commande

 

void commandMotor (int8_t commandeMoteurDroit, int8_t commandeMoteurGauche) {

 
if (valeur >0){/
  
  SetpointB = commandeMoteurDroit;
  SetpointA = commandeMoteurGauche;
 
  }
 
  else  {
 
   SetpointB =  -commandeMoteurDroit;
   SetpointA =  -commandeMoteurGauche;
 
   } 
 
  //utilisation de la sortie du PID pour asservir les moteurs
  if (valeur >= 0) {
    advanceB();
  }
  if (valeur < 0) {
    backB();
  }
  if (valeur >= 0) {
    advanceA();
  }
  if (valeur < 0) {
    backA();
  }
 
J'ai retiré myPIDB.SetOutputLimits(-255, 255), pour lire la tension de sortie toujours positivement et que ça n'embrouille pas le programme (ou moi même).
 
Par contre petit truc, j'aurai aimé avoir la vitesse de mes moteurs hyper constante pour me rapprocher de la théorie du PID.,  
De sorte que, lorsque les moteurs sont censés être arrivés à leurs vitesses qui est stable dans le temps (celle de leur Setpoint), il y ait toujours le même décalage de phase entre eux.
 
Mes deux moteurs au bout de 2 minutes, ils se décalent de 90°. Ce qui est pas beaucoup sachant qu'ils tournent à 70BPM. Mais je sais pas s'il est peut être possible d'améliorer ce résultat en remplaçant des constantes byte par volatile byte ou unsigned char ou.....
 
 
Aussi quand, je bloque mes deux moteurs la tension augmente et quand je les lache, ils retournent à leurs vitesses asservies.
Ce qui est normal.
 
Mais quand je les bloque plus longtemps jusqu'à ce que  leur val_output atteint les 255, le moteur de gauche ne se "force plus" à revenir à son Setpoint il 'est à l'arrêt alors que celui de droite se comporte normalement, il fait des à coup.
.
C'est seulement quand je lache le moteur de droite que ça "débloque" le moteur de gauche qui retourne à son Setpoint. 
 
Là je pense que la vitesse détectée du moteur droit influence le calcule ou les attachinterrupt du moteur gauche.
 
D'où peut être l'utilité du mask comme dans votre premier programme avec un seul moteur, mais je ne sais pas comment l'intégrer au mien.
state|=B11101011;  // mask pour ne regarder que les changements sur 2 et 4 
  // Modifier le MASK  B01111011 par BXXXXXXXX mettre des 0 là où sont les pins utilisés par l'encodeur
  if( state!=laststate)
Je mets mon programme 
#include <PID_v1.h>

// PORT POUR KIT L298N 2nd (Bleu clair) supllied with 6 V
//  MOTEUR A  GAUCHE  avec myPIDA 8,9,5,3,7,

//Ports de commande du moteur A supplied with 6 volt= 90 round/min.

int motorPinA1 = 9; //13 pin to set H BRIDGE of L298N  
int motorPinA2 = 8; //12 pin to set H BRIDGE of L298N  
const int enablePinA= 5; //11 //  Commande de vitesse moteur, to Output ENA pour L298 the second

//  MOTEUR B Droit  avec myPIDB 10,11,6,2,4
  
//Ports de commande du moteur B supplied with 6 volt.

int motorPinB1 = 10; // pin to set H BRIDGE of L298N
int motorPinB2 = 11; //12 pin to set H BRIDGE of L298N
const int enablePinB= 6; //11 //  Commande de vitesse moteur, to Output ENA pour L298 the second



//MOTOR GAUCHE pin D 3,7,9,8,5
 
volatile byte encoderApinA = 3;//A pin -> the interrupt pin 3
byte encoderApinB = 7;//B pin -> the digital pin 7 /// What is better? byte or const byte ?

volatile byte encoderAPinALast;  // What is better? volatile byte encoderAPinALast or just byte encoderAPinALast  ?
double  latestDurationCountA,durationA,abs_durationA;//the number of the pulses
boolean DirectionA;//the rotation direction 
boolean resultA;

double val_outputA;//Power supplied to the motor PWM value.
double SetpointA;
double Kp=1, Ki=10, Kd=0.01; // 1 ; 10 ; 0.01
PID myPIDA(&abs_durationA, &val_outputA, &SetpointA, Kp, Ki, Kd, DIRECT); 


//**************************************************
//MOTOR DROIT myPIDB motor pinB+encoderB 2,4,11,10,6
volatile byte encoderBpinA = 2;//A pin -> the interrupt pin 1/*volatile byte
byte encoderBpinB = 4;//B pin -> the digital pin 4

volatile byte encoderBPinALast;
double  latestDurationCountB, durationB, abs_durationB; //the number of the pulses
boolean DirectionB;//the rotation direction 
boolean resultaB;

double val_outputB;//Power supplied to the motor PWM value.
double SetpointB;
//double Kp=0.6, Ki=5, Kd=0; // 0.6 ; 5 ; 0
PID myPIDB(&abs_durationB, &val_outputB, &SetpointB, Kp, Ki, Kd, DIRECT); 

unsigned long time0 = 0; // stock un temps à un instant donné
unsigned long timer = 0; // variable qui va contenir le dernier temps enregistré via millis

int8_t valeur = 20;  //lecture de la consigne via le moniteur serie pour test, initialisé à 40 pour test rapide du PID




void setup()

{  
  Serial.begin(19200);//Initialize the serial port
  
   // Configuration des ports en mode "sortie" A   moteur GAUCHE
  pinMode(motorPinA1, OUTPUT);
  pinMode(motorPinA2, OUTPUT);
  pinMode(enablePinA, OUTPUT);

   // Configuration des ports en mode "sortie" B moteur DROIT
   
   pinMode(motorPinB1, OUTPUT);   //L298N Control port settings direction of motor B (originaly L298N)
   pinMode(motorPinB2, OUTPUT);  //L298N Control port settings direction of motor B
   pinMode(enablePinB, OUTPUT);  // powerRate to control speed of motor B
   
   // REGLAGE PID MOTOR A (gauche)
   SetpointA; 
   
   myPIDA.SetMode(AUTOMATIC);//PID is set to automatic mode
   myPIDA.SetSampleTime(5);//Set PID sampling frequency is 100ms
  // myPIDA.SetOutputLimits(-255, 255);
   EncoderInitA();
  

   //REGLAGE PID MOTEUR DROIT (droit)
   
   SetpointB; 
   
   myPIDB.SetMode(AUTOMATIC);//PID is set to automatic mode
   myPIDB.SetSampleTime(5);//Set PID sampling frequency is 100ms
   //myPIDB.SetOutputLimits(-255, 255);
   EncoderInitB();
   

 
 
  
}
 
void loop()
{ if (Serial.available()) {
    time0 = timer;
    valeur = Serial.parseInt(); //récupération des caractères sur le port série
    Serial.println("-----------------valeur");Serial.println(valeur);
    
  }
  if (valeur != 0) {
    commandMotor(valeur,valeur);
  }
  else {
    SetpointA=0; SetpointB=0;
    stopMotors();
  }
  timer = millis();
  
  if ( (timer - time0) >120000) { // Timeout !
    valeur = 0 ;
    stopMotors();
    time0 = timer; 

 }  

//MOTOR GAUCHE 

      noInterrupts();
      latestDurationCountA = durationA;
      interrupts();
      abs_durationA = abs(latestDurationCountA);
        
      resultA=myPIDA.Compute();//PID conversion is complete and returns 1
      
    //   advanceA();
      if(resultA)
      {
        Serial.print("LD"); Serial.print(latestDurationCountA); 
        Serial.print("D  : ");  Serial.print(durationA); Serial.print(durationA); Serial.print(durationA);
        durationA = 0; //Count clear, wait for the next count
        //Serial.print("SetpointA"); Serial.println(SetpointA);
        Serial.print("V"); Serial.println(val_outputA);// volt to MOTOR= real speed
      }

        // *******************************************


  // SETTING  MOTEUR DROIT PIDB avec PIN B plus rapide?
   
      noInterrupts();
      latestDurationCountB = durationB;
      interrupts();
      abs_durationB = abs(latestDurationCountB);

     //  SetpointB=20; //
  
      resultaB=myPIDB.Compute();//PID conversion is complete and returns 1
      
    //  advanceB();
      
      if(resultaB){
  
        Serial.print("LDb"); Serial.print(latestDurationCountB);  
        Serial.print("Db: ");  Serial.print(durationB);Serial.print(durationB);Serial.print(durationB); 
        durationB = 0; //Count clear, wait for the next count
        //Serial.print("SetpointB"); Serial.println(SetpointB);
        Serial.print("Va"); Serial.println(val_outputB);// volt to MOTOR= real speed
      }
    
}


 
void EncoderInitA()
{
  DirectionA = true;//default -> Forward  
  pinMode(encoderApinB,INPUT);  
  attachInterrupt(1, wheelSpeedA, CHANGE);
}
 
void wheelSpeedA()
{
  byte Lstate = digitalRead(encoderApinA);
  if((encoderAPinALast == LOW) && Lstate==HIGH)
  {
    byte val = digitalRead(encoderApinB);
    if(val == LOW && DirectionA)
    {
      DirectionA = false; //Reverse
    }
    else if(val == HIGH && !DirectionA)
    {
      DirectionA = true;  //Forward
    }
  }
  encoderAPinALast = Lstate; // volatile byte encoderAPinALast ??
  
 
  if(!DirectionA)  durationA++;
  else  durationA--;

}
void advanceA()//Motor Forward
{
     digitalWrite(motorPinA1,HIGH);
     digitalWrite(motorPinA2,LOW);
     
     analogWrite(enablePinA,val_outputA);
}
void backA()//Motor reverse
{
      digitalWrite(motorPinA1,LOW);
     digitalWrite(motorPinA2,HIGH);
     
     analogWrite(enablePinA,val_outputA);
}

void StopA()//Motor stops
{
     digitalWrite(enablePinA, LOW); 
}

void EncoderInitB()
{
  DirectionB = true;//default -> Forward  
  pinMode(encoderBpinB,INPUT);  
  attachInterrupt(0, wheelSpeedB, CHANGE);
}
 
void wheelSpeedB()
{
  byte LstateB = digitalRead(encoderBpinA);
  if((encoderBPinALast == LOW) && LstateB==HIGH)
  {
    byte valB = digitalRead(encoderBpinB);
    if(valB == LOW && DirectionB)
    {
      DirectionB = false; //Reverse
    }
    else if(valB == HIGH && !DirectionB)
    {
      DirectionB = true;  //Forward
    }
  }
  encoderBPinALast = LstateB;
 
  if(!DirectionB)  durationB++;
  else  durationB--;

}
void advanceB()//Motor Forward
{
     digitalWrite(motorPinB1,HIGH);
     digitalWrite(motorPinB2,LOW);
     
     analogWrite(enablePinB,val_outputB);
}
void backB()//Motor reverse
{
      digitalWrite(motorPinB1,LOW);
     digitalWrite(motorPinB2,HIGH);
     
     analogWrite(enablePinB,val_outputB);
}

void StopB()//Motor stops
{
     digitalWrite(enablePinB, LOW); 

     
}

void stopMotors() {

  StopB();
  StopA();

 // digitalWrite(enablePinA, 0);
  //digitalWrite(enablePinB, 0);

}
void commandMotor (int8_t commandeMoteurDroit, int8_t commandeMoteurGauche) {

if (valeur >0){// || commandeMoteurDroit
  
  SetpointB = commandeMoteurDroit;
  SetpointA = commandeMoteurGauche;

  }

  else  {

   SetpointB =  -commandeMoteurDroit;
   SetpointA =  -commandeMoteurGauche;

   } 

  //int outputDroit = (int)val_outputB;   // creer nouvelle donnée de val-output egale à (int)val-outputB
  //int outputGauche = (int)val_outputA; //gauche

  //utilisation de la sortie du PID pour asservir les moteurs
  if (valeur >= 0) {
    advanceB();
  }
  if (valeur < 0) {
    backB();
  }
  if (valeur >= 0) {
    advanceA();
  }
  if (valeur < 0) {
    backA();
  }



}

Merci pour votre aide, j'espère n'être pas trop confus. :crazy:


#32 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 04 février 2019 - 06:37

volatile => à mettre pour toute variable modifiée dans une interruption 

=> On ne modifie pas un numéro de pin ... 
=> Préférer mettre const ( constante ) ou le mettre en #define 

mask avec old => Quand on cherche à voir un changement uniquement sur certains Bits d'un Octet on peut masquer des bits avec un "mask"

Mais cela peut aussi être fait dans l'autre sens avec du décalage à droite comme dans le cas du code donné :

byte state = PIND;
(((state >> ENCODEURGAUCHEA) & 1 ) ^ ((state >> ENCODEURGAUCHEB) & 1)) ? countGauche++ : countGauche--;

 

 

 
Mais quand je les bloque plus longtemps jusqu'à ce que  leur val_output atteint les 255, le moteur de gauche ne se "force plus" à revenir à son Setpoint il 'est à l'arrêt alors que celui de droite se comporte normalement, il fait des à coup.
.
C'est seulement quand je lache le moteur de droite que ça "débloque" le moteur de gauche qui retourne à son Setpoint. 
 
Là je pense que la vitesse détectée du moteur droit influence le calcule ou les attachinterrupt du moteur gauche.
 
D'où peut être l'utilité du mask comme dans votre premier programme avec un seul moteur, mais je ne sais pas comment l'intégrer au mien.
Merci pour votre aide, j'espère n'être pas trop confus. :crazy:

 

 

 

Sur le principe, sans chercher dans le code une éventuelle erreur / explication, Il n'est pas normal que le comportement ne soit pas le même sur les deux moteurs ...

 

Je pense que tes fonctions wheelSpeed peuvent être améliorée ...
Je ne comprends pas non plus pourquoi tu utilise le " noInterrupts(); " ...Justement si tu veux un bon résultat il ne faut jamais désactiver les interruptions ... 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#33 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 06 février 2019 - 04:12

Salut,

 

Juste pour dire que j'ai mis le programme dans une vrai Arduino, la Due et là je n'ai plus aucun problème. 

 

En plus, mes deux moteurs sont synchronisés encore plus précisément.

 

Merci pour votre patience. :zsmoke:



#34 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 06 février 2019 - 04:16

Peux tu poster le code utilisé avec la due ? Je suis sûr que cela aidera d'autre personnes ! =)
Je pourrais faire référence à ton code quand j'aiderais d'autres personnes qui utilisent une due ! =)

Merci d'avance !


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#35 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 06 février 2019 - 04:34

OK

Fichier(s) joint(s)



#36 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 06 février 2019 - 07:29

Pourquoi utiliser le " noInterrupt " ? 

D'ailleurs puisque tu utilises une DUE tu n'es pas limité à 2 interruptions, tu pourrais faire une interruption sur chaque pin pour doubler la résolution de la mesure ... Du moins si cela peut être intéressant pout toi =)


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#37 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 10 février 2019 - 07:56

Pourquoi utiliser le " noInterrupt " ? 

 

​Je sais pas trop en fait, je crois que c'était au cas où l'une des deux pins deconnait.

 

Dans le programme de base j'avais seulement ça

 //  abs_durationA=durationA;/

 

mais un programmeur sur le forum Arduino m'avait conseillé

 

noInterrupts();

      latestDurationCountA = durationA;
      interrupts();
      abs_durationA = abs(latestDurationCountA);
 
 
J'ai retiré noInterrupts(); et en effet, il n'y a aucune difference! 
 
Depuis, j'ai intégré dans mon programme des fonctions de controle de vitesse et sens pour chacun de mes deux moteurs, et là, je n'ai plus du tout la même synchronicité qu'avec le programme attaché dernièrement.
 
Donc, j'aurai bien aimé utilisé les deux pin de l'encodeur pour doubler la resolution.

Si tu pouvais me donner un bon coup de main avec un exemple de programme, je suis preneur. Car je suis loin d'avoir fini mon projet qui au final fera tourner 5 moteurs où j'aimerai observer justement observer des légers décalages de manières précises afin de créer une illusion de mouvement optique dans un mouvement réel.... (et faut que je réfléchisse à cette dernière partie).
 
Merci  :mr47_05:
 
 

 



#38 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 173 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 10 février 2019 - 09:27

 

 
Si tu pouvais me donner un bon coup de main avec un exemple de programme, je suis preneur. Car je suis loin d'avoir fini mon projet qui au final fera tourner 5 moteurs où j'aimerai observer justement observer des légers décalages de manières précises afin de créer une illusion de mouvement optique dans un mouvement réel.... (et faut que je réfléchisse à cette dernière partie).
 
Merci  :mr47_05:
 
 

 

 

Oui je peux aider mais pour cela il faut que tu prennes l'habitude de poster ton code avec tes questions =) . 
Plutôt que de mettre un .zip avec le code il y a une balise pour y mettre le code : " < > " 

 

 

 

J'ai retiré noInterrupts(); et en effet, il n'y a aucune difference! 
 
Depuis, j'ai intégré dans mon programme des fonctions de controle de vitesse et sens pour chacun de mes deux moteurs, et là, je n'ai plus du tout la même synchronicité qu'avec le programme attaché dernièrement.
 

 

 

Je n'ai pas compris => est ce qu'il y a un problème de synchro avec ton code actuel ? 

 

Pour un exemple avec les deux pins , il suffit de "doubler le code déjà présent " en rajoutant un interrupt sur le pin du codeur qui n'en possède pas " et le faire luis aussi incrémenter la vitesse. 

=> 2 fonctions vont incrémenter la vitesse et la résolution sera doublée.  =)


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#39 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 11 février 2019 - 09:17

Avant en assignant les mêmes vitesses à mes deux moteurs, ils se décalaient de 45° au bout de deux minutes,

 

maintenant en compliquant mon programme (avec le calcul de la moyenne de la vitesse par moteur), j'ai un décalage de 720° donc je vais essayer de

 

"doubler le code déjà présent ".

 

Ok, mais en plus de doubler la resolution, il faudrait que je puisse garder la fonction qui  detecte la direction de la roue. 

Voila le code, je vais essayer donc de doubler

volatile byte LstateA = digitalRead(encoderApinA);

avec

volatile byte LstateB = digitalRead(encoderApinB);

....

void EncoderInitA()
{
  DirectionA = true;//default -> Forward  
  pinMode(encoderApinB,INPUT);  
  attachInterrupt(3, wheelSpeedA, CHANGE);
}
 
void wheelSpeedA()
{
volatile byte LstateA = digitalRead(encoderApinA);  
  if((encoderAPinALast == LOW) && LstateA==HIGH)
  {
 volatile byte valA = digitalRead(encoderApinB);
    if(valA == LOW && DirectionA)
    {
      DirectionA = false; //Reverse
    }
    else if(valA == HIGH && !DirectionA)
    {
      DirectionA = true;  //Forward
    }
  }
  encoderAPinALast = LstateA; 
  
 
  if(!DirectionA)  durationA++;
  else  durationA--;

}


#40 bvking

bvking

    Nouveau membre

  • Membres
  • 29 messages

Posté 11 février 2019 - 10:40

voilà , j'ai changé comme ceci

 

la roue A avec l'encodeur A

void EncoderInitA()

{
  DirectionA = true;//default -> Forward  
  pinMode(encoderApinB,INPUT);  
  pinMode(encoderApinA,INPUT);  
  attachInterrupt(3, wheelSpeedA, CHANGE);
   attachInterrupt(7, wheelSpeedA, CHANGE);
}


void wheelSpeedA()
{
volatile byte LstateA = digitalRead(encoderApinA);  
volatile byte LstateAB = digitalRead(encoderApinB);  

  if(encoderAPinALast == LOW && LstateA==HIGH && encoderAPinBLast == HIGH && LstateAB==LOW)
  
  
  {
 volatile byte valA = digitalRead(encoderApinB);

    if(valA == LOW && DirectionA)
    {
      DirectionA = false; //Reverse
    }
    else if(valA == HIGH && !DirectionA)
    {
      DirectionA = true;  //Forward
    }
  }
  encoderAPinALast = LstateA; 
  encoderAPinBLast = LstateAB; 
  

  if(!DirectionA)  durationA++;
  else  durationA--;

}

 

 

et pareil avec la roue B

oid EncoderInitB()
{
  DirectionB = true;//default -> Forward  
  pinMode(encoderBpinB,INPUT); 
  pinMode(encoderBpinA,INPUT); 
  attachInterrupt(2, wheelSpeedB, CHANGE);
  attachInterrupt(4, wheelSpeedB, CHANGE);
}
 
void wheelSpeedB()
{
  volatile byte LstateB = digitalRead(encoderBpinA); 
  volatile byte LstateBB = digitalRead(encoderBpinA); 

  
  if(encoderBPinALast == LOW && LstateB==HIGH && encoderBPinBLast == HIGH && LstateBB==LOW)
  {
   volatile byte valB = digitalRead(encoderBpinB);
    if(valB == LOW && DirectionB)
    {
      DirectionB = false; //Reverse
    }
    else if(valB == HIGH && !DirectionB)
    {
      DirectionB = true;  //Forward
    }
  }
  encoderBPinALast = LstateB;
  encoderBPinBLast = LstateBB;
 
  if(!DirectionB)  durationB++;
  else  durationB--;

}

en n'oubliant pas ceci dans le setup

volatile byte encoderAPinBLast;
volatile byte encoderBPinBLast;

et bizarrement j'ai la roue A qui va plus doucement que la roue B .

La vitesse détectée pour la roue A est instable alors qu'elle ne saccade pas( à l'oeil nu)

Et la roue A est détecté à une vitesse negative quand l'autre est positive.

 

Mais est-ce que dans le principe c'est bon?

 

Je mets les vitesses perçues pour les moteurs avec LD (dernière vitesse de A) et LDb (dernière vitesse de B)

LDB-50.00Db: -9.00-12.00-16.00Vb92.85
LD49.00D  : 8.0012.0016.00V85.25
LDB-50.00Db: -9.00-13.00-17.00Vb92.85
LD51.00D  : 8.0011.0016.00V73.20
LDB-50.00Db: -9.00-13.00-17.00Vb92.85
LD50.00D  : 8.0011.0014.00V80.20
LDB-51.00Db: -8.00-12.00-16.00Vb89.80
LD50.00D  : 8.0011.0013.00V78.20
LDB-49.00Db: -9.00-13.00-17.00Vb97.85
LD51.00D  : 11.0014.0018.00V75.15
LDB-51.00Db: -9.00-13.00-17.00Vb87.80
LD52.00D  : 9.0012.0014.00V74.05
LDB-50.00Db: -10.00-14.00-18.00Vb94.80
LD54.00D  : 8.0011.0014.00V69.85
LDB-51.00Db: -9.00-13.00-17.00Vb89.75





Répondre à ce sujet



  



Aussi étiqueté avec au moins un de ces mots-clés : Odométrie, Codeur, Encodeur, Quadrature, asservissement, Moteur CC, PID

0 utilisateur(s) li(sen)t ce sujet

0 members, 0 guests, 0 anonymous users