Correcteur PID avec un MPU6050 et un moteur.

**Jay M** · 12/03/2020, 16h40

Envoyé par seb201

De la même manière je ne vois pas ou est ce que cette ligne apparait dans le code pour pouvoir la corriger: " - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);".

Bonsoir

c'est dit dans le warning: cette ligne est dans le fichier Users/seb/Documents/Arduino/libraries/MPU6050/MPU6050_6Axis_MotionApps20.h (ligne 689)

Pour le PID, je vous propose déjà de voir si le code fourni fonctionne chez vous et affiche sur la console série un "angle" qui sera utilisable.

**seb201** · 12/03/2020, 18h50

Envoyé par Jay M

Bonsoir
Pour le PID, je vous propose déjà de voir si le code fourni fonctionne chez vous et affiche sur la console série un "angle" qui sera utilisable.

Bonsoir,
J'ai tester le code et je n'ai absolument rien qui s'affiche .Et j'aimerai bien que vous m'expliquiez cette partie du code car je ne comprend vraiment rien😅🙁.

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
void loop()
{
  if (mpuInterrupt || fifoCount >= packetSize) { // on a des données
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {     // test si overflow (ne devrait pas arriver si on dépile vite la FIFO)
      mpu.resetFIFO();       // reset
    } else if (mpuIntStatus & 0x02) {       // verifie si on a le "DMP data ready interrupt" (this should happen frequently)
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // on s'assure que le buffer est suffisant, ça devrait être le cas
      mpu.getFIFOBytes(fifoBuffer, packetSize); // lire un paquet de FIFO fifoBuffer
      fifoCount -= packetSize;
 
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      uint32_t currentChrono = micros();

et

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
   if ((abs(currentYaw - lastYaw) > 1) || (abs(currentPitch - lastPitch) > 1) || (abs(currentRoll - lastRoll) > 1)) {
        Serial.print(F("YPR\t"));
        Serial.print(currentYaw, 0);
        Serial.write("\t");
        Serial.print(currentPitch, 0);
        Serial.write("\t");
        Serial.println(currentRoll, 0);
        lastYaw = currentYaw;
        lastPitch = currentPitch;
        lastRoll = currentRoll;
      }
#endif

Le problème c'est que je n'arrive meme pas a comprendre à quoi sert ce code .Et que dois je modifier de ce code:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
#include <AFMotor.h>
#define moteurDirectionPin 8
#define moteurVitessePin 11
#define moteurDirectionCW HIGH                    // Sens des aiguilles d'une montre
#define moteurDirectionCCW LOW  
int16_t Acc_rawY,Gyr_rawY;
float Acceleration[1];
float Gyro_angle[1];
float Total_angle[1];
float Acceleration_angle[1];
float elapsedTime,time,timePrev;
int i;
float rad_to_deg = 180/3.141592654;
float PID,error;
float previous_error =0;
float pid_d =0;
float pid_i =0;
float pid_p =0;
double kp=3.55;
double kd=0.005;
double ki=2.05;
float desired_angle =0;
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
int inputPin =A0;
int inputPin1 =A1;
uint8_t Accel_range;
uint8_t Gyro_range;
float angle=0;
int16_t ax, ay, az;  //mesures brutes
int16_t gx, gy, gz;       
                  // Sens contraire (faudra peut etre inverser)
byte moteurVitesse = 250;
void setup() {
 
  pinMode (moteurVitessePin, OUTPUT);
  pinMode(moteurDirectionPin,OUTPUT);
  Wire.begin();  //I2C bus
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB (LEONARDO)
  }
   // initialize device
  Serial.println("Initialisation I2C...");
  accelgyro.initialize();
 
  // verify connection
  Serial.println("Test de la conection du dispositif ...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection reussie" : "MPU6050 connection echec");
  delay(1000);
 
  Serial.begin(9600);           // set up Serial library at 9600 bps
 
  pinMode(inputPin,INPUT_PULLUP);
  pinMode(inputPin1,INPUT_PULLUP);
  time = millis();        
}
 
void loop() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  angle=0.0*(angle+float(gy)*0.01/131) + 1*atan2((double)ax,(double)az)*180/PI;
  Serial.println(angle); 
  controlemotV2(angle);
  delay(10);
}
 
void controlemotV2(int alpha) {
 
int Val1=digitalRead(inputPin);
 Serial.println(Val1);
 
 int Val2=digitalRead(inputPin1);
 Serial.println(Val2);
 
if ( Val1==0){
   moteurVitesse = 255;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCCW);
 
 
}
else if ( Val2==0){
   moteurVitesse = 255;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCW);
}
else if (int (alpha < -10)){
   moteurVitesse = 255;
   analogWrite(moteurVitessePin,moteurVitesse);
   digitalWrite(moteurDirectionPin, moteurDirectionCCW); //fait avancer le chariot vers la droite
 
}
else if (int (alpha > 10)){
  moteurVitesse = 255;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCW); //fait avancer le chariot vers la gauche
 
}
else{
   moteurVitesse = 0;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCCW);
 
 }
}
 
 
 
void correcteur() {
 
  timePrev = time;
  time = millis();
  elapsedTime = (time - timePrev) / 1000;
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68,6,true);
  Acc_rawY = Wire.read()<<8| Wire.read();
  Gyro_angle[1] = Gyr_rawY/131;
  Total_angle[1] = 0.98*(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
  error = Total_angle[1] - desired_angle;
  pid_p = kp*error;
 
 if ((-3<error) and (error<3)){
  pid_i = pid_i +(ki*error);
}
 
  pid_d = kd*((error - previous_error)/elapsedTime);
  PID = pid_d + pid_i + pid_p;
 
 
 // je ne suis pas du tout sur si cela peut etre mis dans le correcteur PID
for(i=0; i<255; i=i+5) {
if ((abs (int (angle(timePrev))) - abs(int( angle(time)))) < 2 ){
  moteur.setSpeed(i)
  moteur.run(BACKWARD);
}
else if ((abs (int alpha(t)) - abs(int alpha(t+1))) > 2 ){
   moteur.setSpeed(i)
   moterur.run(FORWARD);
}
}
previous-error=error
}

**Jay M** · 13/03/2020, 14h07

Envoyé par seb201

Bonsoir,
J'ai testé le code et je n'ai absolument rien qui s'affiche

vous avez bien ouvert la console à 115200 bauds ?

on parle bien de ne mettre QUE CE CODE après avoir correctement installé les répertoires I2Cdev et MPU6050de la librairie dans votre dossier des librairies ainsi que fait le correction dans vos librairies du fichier MPU6050_6Axis_MotionApps20.h ?

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
// remerciement à Jeff Rowberg pour sa bibliothèque i2cdevlib
#include "Wire.h"
#include "I2Cdev.h" //copier https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev  dans le répertoire des librairies
#include "MPU6050_6Axis_MotionApps20.h" //copier https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 dans le répertoire des librairies
 
MPU6050 mpu;
 
#define EULER_ANGLES // à commenter pour ne pas afficher les angles d'Euler
//#define YPR_ANGLES // à commenter pour ne pas afficher les angles de roulis, tangage, lacet
 
 
const float RADIANS_TO_DEGREES = 180.0 / M_PI;
 
// MPU control/status vars
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer, doit être plus grand que packetSize
 
// variables d'orientation/mouvement
Quaternion q;
VectorFloat gravity;
float ypr[3];
float euler[3];
 
uint32_t lastChrono;
 
#ifdef EULER_ANGLES
float lastPsi ;
float lastTheta ;
float lastPhi;
#endif
 
 
 
#ifdef YPR_ANGLES
float lastYaw;
float lastPitch;
float lastRoll;
#endif
 
 
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}
 
 
void setup() {
  uint8_t dmpStatus;
 
  Wire.begin();
  Serial.begin(115200);
 
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println(F("pas de MPU"));
    while (true);
  }
 
  if (dmpStatus = mpu.dmpInitialize()) {
    Serial.print(F("Erreur DMP #"));
    Serial.println(dmpStatus);
    while (true);
  }
 
  mpu.setDMPEnabled(true);
  attachInterrupt(0, dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus();
  packetSize = mpu.dmpGetFIFOPacketSize(); // 42
}
 
void loop()
{
  if (mpuInterrupt || fifoCount >= packetSize) { // on a des données
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {     // test si overflow (ne devrait pas arriver si on dépile vite la FIFO)
      mpu.resetFIFO();       // reset
    } else if (mpuIntStatus & 0x02) {       // verifie si on a le "DMP data ready interrupt" (this should happen frequently)
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // on s'assure que le buffer est suffisant, ça devrait être le cas
      mpu.getFIFOBytes(fifoBuffer, packetSize); // read a packet from FIFO fifoBuffer
      fifoCount -= packetSize;
 
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      uint32_t currentChrono = micros();
 
#ifdef EULER_ANGLES
      // Obtain Euler angles from buffer
      mpu.dmpGetEuler(euler, &q);
 
      float currentPsi = -euler[0] * RADIANS_TO_DEGREES;
      float currentTheta = euler[1] * RADIANS_TO_DEGREES;
      float currentPhi = -euler[2] * RADIANS_TO_DEGREES;
 
      if ((abs(currentPsi - lastPsi) > 1) || (abs(currentTheta - lastTheta) > 1) || (abs(currentPhi - lastPhi) > 1)) {
        Serial.print(F("Euler\t"));
        Serial.print(currentPsi, 0);
        Serial.write("\t");
        Serial.print(currentTheta, 0);
        Serial.write("\t");
        Serial.println(currentPhi, 0);
        lastPsi = currentPsi;
        lastTheta = currentTheta;
        lastPhi = currentPhi;
      }
#endif
 
#ifdef YPR_ANGLES
      // Obtain YPR angles from buffer
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 
      float currentYaw = ypr[0] * RADIANS_TO_DEGREES;       // yaw: (about Z axis)
      float currentPitch = -ypr[1] * RADIANS_TO_DEGREES;    // pitch: (nose up/down, about Y axis)
      float currentRoll = ypr[2] * RADIANS_TO_DEGREES;      // roll: (tilt left/right, about X axis)
 
      if ((abs(currentYaw - lastYaw) > 1) || (abs(currentPitch - lastPitch) > 1) || (abs(currentRoll - lastRoll) > 1)) {
        Serial.print(F("YPR\t"));
        Serial.print(currentYaw, 0);
        Serial.write("\t");
        Serial.print(currentPitch, 0);
        Serial.write("\t");
        Serial.println(currentRoll, 0);
        lastYaw = currentYaw;
        lastPitch = currentPitch;
        lastRoll = currentRoll;
      }
#endif
 
 
 
      lastChrono = currentChrono;
    }
  }
}

**seb201** · 14/03/2020, 18h09

Envoyé par Jay M

fait le correction dans vos librairies du fichier MPU6050_6Axis_MotionApps20.h ?

Bonsoir Jay ,
Tout avait été fait à part ceci ,mais je ne comprend ce que je suis censé faire. voici ce que j'ai lorsque je lance le code. Je suis disponible toute la soiree si vous pouvez m'aider😂😂😅.
Car en plus je ne vois pas vraiment comment insérer mon correcteur PID pour qu'il fasse en sorte de faire varier la vitesse de mon chariot en fonction de l'angle.
Pièce jointe 545942

**seb201** · 14/03/2020, 18h16

Autant pour moi je viens de tout revérifier et la sortie int était mal branchée.
Voici donc ce que j'obtiens:Pièce jointe 545954
A
Mais a quoi servent exactement ces donnes et comment dois-je utiliser ces données pour mon correcteur PID?

**Jay M** · 15/03/2020, 12h57

Ah c'est bon signe alors

je suppose que vous avez modifié le code pour enlever le commentaire sur la ligne YPR_ANGLES et mis en commentaire celle des EULER_ANGLES pour ne pas afficher les angles d'Euler mais YPR (le code à l'origine était comme cela):

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
#define EULER_ANGLES // à commenter
//#define YPR_ANGLES // à commenter pour ne pas afficher les angles de roulis, tangage, lacet

==> le code affiche donc les angles YPR (Yaw, Pitch Roll).
Nom : 800px-Flight_dynamics_with_text_ortho.svg.png
Affichages : 506
Taille : 50,5 Ko

En français on parle de roulis, tangage, lacet.

dans votre copie d'écran (c'est du texte, vaut mieux le copier en tant que tel) on voit que seul l'affichage de Yaw varie, les deux autres restent quasiment constants. si votre montage a déjà l'axe de contrainte physique installée ça veut dire que c'est votre axe de rotation.

il vous suffira de suivre dans votre PID l'évolution du Yaw. il y a pour cela des variables qui répondent à votre interrogation passée:

currentYaw = le Yaw au moment présent. Ce moment présent est noté dans currentChrono
lastYaw = le Yaw lors de la dernière mesure qui a été effectuée au moment lastChrono

Si vous regardez le code PID dans la vidéo ci dessous, dans sa loop il fait:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
 if (millis() - time  > period) // correction Jay pour gérer correctement le rollover de millis() dans ~50 jours 
  {
    time = millis();    
    distance = get_dist(100);   
    distance_error = distance_setpoint - distance;   
    PID_p = kp * distance_error;
    PID_d = kd*((distance_error - distance_previous_error)/period);
 
    if(-3 < distance_error && distance_error < 3)
    {
      PID_i = PID_i + (ki * distance_error);
    }
    else
    {
      PID_i = 0;
    }
 
    PID_total = PID_p + PID_i + PID_d;  
    PID_total = map(PID_total, -150, 150, 0, 150);
 
    if(PID_total < 20){PID_total = 20;}
    if(PID_total > 160) {PID_total = 160; } 
 
    myservo.write(PID_total+30);  
    distance_previous_error = distance_error;
  }

il a défini une position fixe à atteindre pour sa balle qui est dans distance_setpoint (c'est la distance entre le centre et le capteur)

il commence par tester si suffisamment de temps s'est écoulé par rapport à la dernière mesure car il veut modifier sa commande toutes ∆t = period
si c'est le moment de faire une nouvelle mesure:
il commence par mesurer la distance de la balle par rapport au capteur avec get_dist()
il calcule l'erreur de position en faisant la soustraction distance_error = distance_setpoint - distance

et ensuite il calcule les 3 éléments du régulateur PID (proportionnel, intégral, dérivé) comme il l'explique dans sa vidéo;

la partie proportionnelle à l'erreur:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

PID_p = kp * distance_error;

la partie dérivée à l'erreur:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

PID_d = kd*((distance_error - distance_previous_error)/period);

la partie intégrale (un peu bidouillé):

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

PID_i = PID_i + (ki * distance_error);

(il a un peu bidouillée la partie intégrale si l'erreur absolue est de moins de 3 alors il met cette partie intégrale à 0)

Ensuite le PID est la somme des trois éléments

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

PID_total = PID_p + PID_i + PID_d;

qu'il transforme en bidouillant un peu en un angle pour son servo

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

    PID_total = map(PID_total, -150, 150, 0, 150);

. c'est un peu bidouillé ensuite pour contraindre le PID entre 20 et 160

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
if(PID_total < 20){PID_total = 20;}
if(PID_total > 160) {PID_total = 160; }

(ce qu'il aurait pu faire avec la fonction constrain())

Enfin c'est l'angle (en rajoutant 30, j'imagine dû à l'angle de départ lié à son montage physique) qu'il balance à son servo:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

myservo.write(PID_total+30);

vous pouvez essayer d'appliquer cela à votre cas. Pour vous le chariot ne doit pas être à une position fixe, ce qui doit être fixe c'est le Yaw du bateau qui doit revenir à 0 (ou si le capteur n'est pas bien monté, au Yaw quand le bateau est bien à l'équilibre). Le moteur déplace la charge pour influencer ce Yaw.

**seb201** · 15/03/2020, 18h02

Envoyé par Jay M

Ah c'est bon signe alors

je suppose que vous avez modifié le code pour enlever le commentaire sur la ligne YPR_ANGLES et mis en commentaire celle des EULER_ANGLES pour ne pas afficher les angles d'Euler mais YPR (le code à l'origine était comme cela):

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
#define EULER_ANGLES // à commenter
//#define YPR_ANGLES // à commenter pour ne pas afficher les angles de roulis, tangage, lacet

C'est exactement ce que j'ai fait ,et je viens de modifier le code pour n'afficher que le pitch (car c'est uniquement ce qui m'intéresse ):

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
// remerciement à Jeff Rowberg pour sa bibliothèque i2cdevlib
#include "Wire.h"
#include "I2Cdev.h" //copier https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev  dans le répertoire des librairies
#include "MPU6050_6Axis_MotionApps20.h" //copier https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 dans le répertoire des librairies
 
MPU6050 mpu;
 
//#define EULER_ANGLES // à commenter pour ne pas afficher les angles d'Euler
#define YPR_ANGLES // à commenter pour ne pas afficher les angles de roulis, tangage, lacet
 
 
const float RADIANS_TO_DEGREES = 180.0 / M_PI;
 
// MPU control/status vars
uint8_t mpuIntStatus;   // contient l'octet d'état d'interruption réel du MPU
uint16_t packetSize;    // taille de paquet DMP attendue (la valeur par défaut est de 42 octets)
uint16_t fifoCount;     // nombre de tous les octets actuellement dans FIFO 
uint8_t fifoBuffer[64]; // Tampon de stockage FIFO, doit être plus grand que packet
 
// variables d'orientation/mouvement
Quaternion q;
VectorFloat gravity;
float ypr[3];
float euler[3];
 
uint32_t lastChrono;
 
#ifdef EULER_ANGLES
float lastTheta ;
#endif
 
#ifdef YPR_ANGLES
float lastPitch;
#endif
 
 
volatile bool mpuInterrupt = false;     // indique si la broche d'interruption MPU est devenue haute
void dmpDataReady() {
  mpuInterrupt = true;
}
 
 
void setup() {
  uint8_t dmpStatus;
 
  Wire.begin();
  Serial.begin(115200);
 
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println(F("pas de MPU"));
    while (true);
  }
 
  if (dmpStatus = mpu.dmpInitialize()) {
    Serial.print(F("Erreur DMP #"));
    Serial.println(dmpStatus);
    while (true);
  }
 
  mpu.setDMPEnabled(true);
  attachInterrupt(0, dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus();
  packetSize = mpu.dmpGetFIFOPacketSize(); // 42
}
 
void loop()
{
  if (mpuInterrupt || fifoCount >= packetSize) { // on a des données
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {     // test si overflow (ne devrait pas arriver si on dépile vite la FIFO)
      mpu.resetFIFO();       // reset
    } else if (mpuIntStatus & 0x02) {       // verifie si on a le "DMP data ready interrupt" (this should happen frequently)
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // on s'assure que le buffer est suffisant, ça devrait être le cas
      mpu.getFIFOBytes(fifoBuffer, packetSize); // lire un paquet de FIFO fifoBuffer
      fifoCount -= packetSize;
 
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      uint32_t currentChrono = micros();
 
#ifdef EULER_ANGLES
      // Obtain Euler angles from buffer
      mpu.dmpGetEuler(euler, &q);
 
      float currentTheta = euler[1] * RADIANS_TO_DEGREES;
 
      if (abs(currentTheta - lastTheta) > 1){
        Serial.println(F("Euler\t"));
        Serial.print(currentTheta, 0);
        Serial.write("\t");
        lastTheta = currentTheta;
 
      }
#endif
 
#ifdef YPR_ANGLES
      // Obtenir des angles YPR à partir du tampon
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      float currentPitch = -ypr[1] * RADIANS_TO_DEGREES;    // pas: (nez haut / bas, autour de l'axe Y)
 
      if (abs(currentPitch - lastPitch) > 1){
        Serial.println(F("P\t"));
        Serial.print(currentPitch, 0);
        Serial.write("\t");       
        lastPitch = currentPitch;
      }
#endif
 
    }
  }
}

Mais je ne sais pas à quoi me servira l'angle d'Euler ?
Et j'ai 2 petites questions:
-J'aimerai modifier mon code pour faire en sorte que si mon chariot arrive en bout de course il n'aille pas plus loin mais puisse revenir dans l'autre sens si nécessaire .Car le code que j'ai fait fait seulement déplacer le chariot pour que l'interrupteur ne soit plus appuyer , ce qui donne lieu à des oscillations lorsque le chariot arrive en bout de course .

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
 
int Val1=digitalRead(inputPin);
 Serial.println(Val1);
 
 int Val2=digitalRead(inputPin1);
 Serial.println(Val2);
 
if ( Val1==0){
   moteurVitesse = 255;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCCW);
 
 
}
else if ( Val2==0){
   moteurVitesse = 255;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCW);
}
else if (int (alpha < -10)){
   moteurVitesse = 255;
   analogWrite(moteurVitessePin,moteurVitesse);
   digitalWrite(moteurDirectionPin, moteurDirectionCCW); //fait avancer le chariot vers la droite
 
}

-Et deuxièmement je ne vois pas trop quoi rajouter à la suite de ce code pour pouvoir faire varier la vitesse de mon moteurs à l'aide de mon correcteur PID.

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
#include <Wire.h>
#include <Servo.h>
Servo right_prop;
Servo left_prop;
 
int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180/3.141592654;
float PID, pwmLeft, pwmRight, error, previous_error;
float pid_p=0;
float pid_i=0;
float pid_d=0;
/////////////////PID CONSTANTS/////////////////
double kp=3.55;//3.55
double ki=0.005;//0.003
double kd=2.05;//2.05
///////////////////////////////////////////////
 
double throttle=1300; //initial value of throttle to the motors
float desired_angle = 0; //This is the angle in which we whant the
                         //balance to stay steady
 
 
void setup() {
  Wire.begin(); //begin the wire comunication
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(250000);
  time = millis(); //Start counting time in milliseconds
 
}
 
void loop() {
/////////////////////////////I M U/////////////////////////////////////
    timePrev = time;  // the previous time is stored before the actual time read
    time = millis();  // actual time read
    elapsedTime = (time - timePrev) / 1000;
    Wire.beginTransmission(0x68);
    Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
    Wire.endTransmission(false);
    Wire.requestFrom(0x68,6,true); 
    Acc_rawY=Wire.read()<<8|Wire.read();
    Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
    Wire.beginTransmission(0x68);
    Wire.write(0x43); //Gyro data first adress
    Wire.endTransmission(false);
    Wire.requestFrom(0x68,4,true); //Just 4 registers
    Gyr_rawY=Wire.read()<<8|Wire.read();
    Gyro_angle[1] = Gyr_rawY/131.0;
    Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
   /*Now we have our angles in degree and values from -10º0 to 100º aprox*/
    Serial.println(Total_angle[1]);
 
/*///////////////////////////P I D///////////////////////////////////*/
error = Total_angle[1] - desired_angle;
pid_p = kp*error;
if(-3 <error <3)
{
  pid_i = pid_i+(ki*error);  
}
pid_d = kd*((error - previous_error)/elapsedTime);
PID = pid_p + pid_i + pid_d;
previous_error = error; //Remember to store the previous error.
 
}//end of loop void

Et pour finir dois je enlever ceci de mon code principale et le remplacer par le 1er code ci-dessus.

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  angle=0.0*(angle+float(gy)*0.01/131) + 1*atan2((double)ax,(double)az)*180/PI;
  Serial.println(angle); 
  controlemotV2(angle);
  delay(10);[ATTACH=CONFIG]546161[/ATTACH]

**seb201** · 15/03/2020, 18h29

Je rencontre de même des difficultés pour regrouper les codes car certains utilisent des Serial.begin(115200); et d'autres des Serial.begin(9600);. Je ne sais pas comment faire pour faire en sorte que les 2 codes puissent être réunis. J'ai essayé de ne mettre que des Serial.begin(115200); ou que des Serial.begin(9600); sans succès .

**Jay M** · 15/03/2020, 18h34

Mais je ne sais pas à quoi me servira l'angle d'Euler ?

à rien

si vous partez sur YPR et vous pouvez virer tout ce qui est entre #ifdef EULER_ANGLES et le #endif associé pour rendre votre code plus lisible.

En pratique c'est un plus rapide de calculer les angles d'Euler. Si vous regardez le code on va d'abord chercher la représentation en quaternions (celle qui est la plus précise) par

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

mpu.dmpGetQuaternion(&q, fifoBuffer);

et ensuite si on veut les angles d'Euler on fait simplement appel à

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

mpu.dmpGetEuler(euler, &q);

alors que pour obtenir le YPR on est obligé de faire ça en deux étapes avec un peu plus de calcul matriciel

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
 mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

OK pour la modif, oui pas la peine de tout calculer ou afficher puisque vous n'avez qu'un seul degrés de liberté qui vous intéresse

le code que j'ai fait fait seulement déplacer le chariot pour que l'interrupteur ne soit plus appuyer , ce qui donne lieu à des oscillations lorsque le chariot arrive en bout de course

Pour les détecteurs de fin de course il faut en tenir compte juste avant d'envoyer un ordre de mouvement au moteur après calcul du PID.
-> vous faites le calcul de comment le moteur devrait tourner en théorie et juste avant d'envoyer l'ordre au moteur vous regardez si le contacteur de cette direction est appuyé. ==> si l'ordre moteur essaye d'aller plus loin dans cette même direction (de forcer donc), alors vous n'envoyez pas l'ordre et le chariot ne bougera pas, il sera coincé sur le contacteur jusqu'à ce que le PID lui demande d'aller dans l'autre sens car le bateau aura bougé et se sera (on espère) restabilisé.

Le PID va se mélanger un peu les pinceaux en ne voyant aucune conséquence de la commande sur la lecture et va essayer d'insister encore plus fort mais c'est pas grave vu que vous n'envoyez aucune commande. Et si vous en arrivez là de toutes façon c'est que votre bateau est en situation de grand danger...

je ne vois pas trop quoi rajouter à la suite de ce code pour pouvoir faire varier la vitesse de mon moteurs à l'aide de mon correcteur PID

tout votre ancien code avec Wire et vos Gyro_angle est à ranger dans un coin... le nouveau code représente votre architecture de base.

La vitesse du moteur pourrait être un nombre positif ou négatif par exemple (le signe donnant la direction et la valeur absolue la vitesse souhaitée) un peu comme dans le code d'exemple il faisait

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

 map(PID_total, -150, 150, 0, 150);

On prend le résultat du calcul PID et on transforme cela en vitesse (au lieu d'un angle) en se disant que plus on est loin de l'objectif, plus vite on voudra y aller.

**seb201** · 16/03/2020, 09h34

Envoyé par Jay M

La vitesse du moteur pourrait être un nombre positif ou négatif par exemple (le signe donnant la direction et la valeur absolue la vitesse souhaitée) un peu comme dans le code d'exemple il faisait

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

 map(PID_total, -150, 150, 0, 150);

On prend le résultat du calcul PID et on transforme cela en vitesse (au lieu d'un angle) en se disant que plus on est loin de l'objectif, plus vite on voudra y aller.

Bonjour Jay,
Je viens de modifier mon code pour régler le problème des interrupteurs , néanmoins de temps a autres le problèmes du rebondissement interviens et donne lieux a des oscillations . Savez vous comment régler ce problème ?Voici le code:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
#include <AFMotor.h>
#define moteurDirectionPin 8
#define moteurVitessePin 11
#define moteurDirectionCW HIGH                    // Sens des aiguilles d'une montre
#define moteurDirectionCCW LOW  
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
int inputPin =A0;
int inputPin1 =A1;
int16_t ax, ay, az;  //mesures brutes
int16_t gx, gy, gz;
uint8_t Accel_range;
uint8_t Gyro_range;
float angle=0;
                  // Sens contraire (faudra peut etre inverser)
byte moteurVitesse = 250;
 
 
void setup() {
  pinMode (moteurVitessePin, OUTPUT);
  pinMode(moteurDirectionPin,OUTPUT);
  Wire.begin();  //I2C bus
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB (LEONARDO)
  }
 
  // initialize device
  Serial.println("Initialisation I2C...");
  accelgyro.initialize();
 
  // verify connection
  Serial.println("Test de la conection du dispositif ...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection reussie" : "MPU6050 connection echec");
  delay(1000);
 
  Serial.begin(9600);           // set up Serial library at 9600 bps
  pinMode(inputPin,INPUT_PULLUP);
  pinMode(inputPin1,INPUT_PULLUP);
 
}
 
void loop() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  angle=0.0*(angle+float(gy)*0.01/131) + 1*atan2((double)ax,(double)az)*180/PI;
  Serial.println(angle); 
  controlemotV2(angle);
  delay(10);
}
 
void controlemotV2(int alpha) {
 
int Val1=digitalRead(inputPin);
 Serial.println(Val1);
 
 int Val2=digitalRead(inputPin1);
 Serial.println(Val2);
 
if (int (alpha < -10)){
   moteurVitesse = 120;
   analogWrite(moteurVitessePin,moteurVitesse);
   digitalWrite(moteurDirectionPin, moteurDirectionCCW); //fait avancer le chariot vers la droite
if ( Val2==0){
   moteurVitesse = 0;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCCW);
}
}
else if (int (alpha > 10)){
  moteurVitesse = 120;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCW); //fait avancer le chariot vers la gauche
if ( Val1==0){
   moteurVitesse = 0;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCW);
}
}
else{
   moteurVitesse = 0;
  analogWrite(moteurVitessePin,moteurVitesse);
  digitalWrite(moteurDirectionPin, moteurDirectionCCW);
 
 }
}

Voici de meme le code qui me donne uniquement l'angle selon le Pitch:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
// remerciement à Jeff Rowberg pour sa bibliothèque i2cdevlib
#include "Wire.h"
#include "I2Cdev.h" //copier https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev  dans le répertoire des librairies
#include "MPU6050_6Axis_MotionApps20.h" //copier https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 dans le répertoire des librairies
 
MPU6050 mpu;
 
#define YPR_ANGLES // à commenter pour ne pas afficher les angles de roulis, tangage, lacet
 
 
const float RADIANS_TO_DEGREES = 180.0 / M_PI;
 
// MPU control/status vars
uint8_t mpuIntStatus;   // contient l'octet d'état d'interruption réel du MPU
uint16_t packetSize;    // taille de paquet DMP attendue (la valeur par défaut est de 42 octets)
uint16_t fifoCount;     // nombre de tous les octets actuellement dans FIFO 
uint8_t fifoBuffer[64]; // Tampon de stockage FIFO, doit être plus grand que packet
 
// variables d'orientation/mouvement
Quaternion q;
VectorFloat gravity;
float ypr[3];
 
uint32_t lastChrono;
 
 
#ifdef YPR_ANGLES
float lastPitch;
#endif
 
 
volatile bool mpuInterrupt = false;     // indique si la broche d'interruption MPU est devenue haute
void dmpDataReady() {
  mpuInterrupt = true;
}
 
 
void setup() {
  uint8_t dmpStatus;
 
  Wire.begin();
  Serial.begin(115200);
 
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println(F("pas de MPU"));
    while (true);
  }
 
  if (dmpStatus = mpu.dmpInitialize()) {
    Serial.print(F("Erreur DMP #"));
    Serial.println(dmpStatus);
    while (true);
  }
 
  mpu.setDMPEnabled(true);
  attachInterrupt(0, dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus();
  packetSize = mpu.dmpGetFIFOPacketSize(); // 42
}
 
void loop()
{
  if (mpuInterrupt || fifoCount >= packetSize) { // on a des données
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {     // test si overflow (ne devrait pas arriver si on dépile vite la FIFO)
      mpu.resetFIFO();       // reset
    } else if (mpuIntStatus & 0x02) {       // verifie si on a le "DMP data ready interrupt" (this should happen frequently)
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // on s'assure que le buffer est suffisant, ça devrait être le cas
      mpu.getFIFOBytes(fifoBuffer, packetSize); // lire un paquet de FIFO fifoBuffer
      fifoCount -= packetSize;
 
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      uint32_t currentChrono = micros();
 
#ifdef YPR_ANGLES
      // Obtenir des angles YPR à partir du tampon
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      float currentPitch = -ypr[1] * RADIANS_TO_DEGREES;    // pas: (nez haut / bas, autour de l'axe Y)
 
      if (abs(currentPitch - lastPitch) > 1){
        Serial.println(F("P\t"));
        Serial.print(currentPitch, 0);
        Serial.write("\t");       
        lastPitch = currentPitch;
      }
#endif
 
    }
  }
}

J'ai cependant du mal a comprendre ceci:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
if(PID_total < 20){PID_total = 20;}
    if(PID_total > 160) {PID_total = 160; } 
 
    myservo.write(PID_total+30);

Pour le moment voici a quoi ressemble mon correcteur est il valide en l'état :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
#include <Wire.h>
#include <Servo.h>
Servo right_prop;
Servo left_prop;
 
int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180/3.141592654;
float PID, pwmLeft, pwmRight, error, previous_error;
float pid_p=0;
float pid_i=0;
float pid_d=0;
/////////////////PID CONSTANTS/////////////////
double kp=3.55;//3.55
double ki=0.005;//0.003
double kd=2.05;//2.05
///////////////////////////////////////////////
 
double throttle=1300; //initial value of throttle to the motors
float desired_angle = 0; //This is the angle in which we whant the
                         //balance to stay steady
 
 
void setup() {
  Wire.begin(); //begin the wire comunication
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(250000);
  time = millis(); //Start counting time in milliseconds
 
}
 
void loop() {
/////////////////////////////I M U/////////////////////////////////////
    timePrev = time;  // the previous time is stored before the actual time read
    time = millis();  // actual time read
    elapsedTime = (time - timePrev) / 1000;
    Wire.beginTransmission(0x68);
    Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
    Wire.endTransmission(false);
    Wire.requestFrom(0x68,6,true); 
    Acc_rawY=Wire.read()<<8|Wire.read();
    Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
    Wire.beginTransmission(0x68);
    Wire.write(0x43); //Gyro data first adress
    Wire.endTransmission(false);
    Wire.requestFrom(0x68,4,true); //Just 4 registers
    Gyr_rawY=Wire.read()<<8|Wire.read();
    Gyro_angle[1] = Gyr_rawY/131.0;
    Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
   /*Now we have our angles in degree and values from -10º0 to 100º aprox*/
    Serial.println(Total_angle[1]);
 
/*///////////////////////////P I D///////////////////////////////////*/
error = Total_angle[1] - desired_angle;
pid_p = kp*error;
if(-3 <error <3)
{
  pid_i = pid_i+(ki*error);  
}
pid_d = kd*((error - previous_error)/elapsedTime);
PID = pid_p + pid_i + pid_d;
PID=map(PID, -90, 90, -255, 255);// 
moteurVitesse =PID ;
previous_error = error; //Remember to store the previous error.
}//end of loop void

Je n'est de même pas trouvé de solution permettant de combiner tout mes codes car il n'ont pas tous le meme serial.begin() , déplus lorsque j'essaye de les combiner j'obtiens ceci:
Pièce jointe 546250

**Jay M** · 16/03/2020, 12h05

come je vous l'ai dit, le nouveau code remplace entièrement (à l’exception de la déclaration des pins de pilotage moteur et des contacts de fin de course bien sûr) l'ancienne structure de code. Soit vous décidez de conserver l'ancien (mais vous ne profitez pas du DMP qui permet d'éviter la dérive des mesures dans le temps) soit vous le mettez de côté et travaillez à partir du nouveau. Ma recommendation est de partir sur le nouveau code.

Dans le nouveau code (virez au passage le define et test associés pour YPR puisque que c'est ce que vous voulez), à la fin de la loop vous avez le nouvel angle et l'ancien angle, le ∆t entre les deux mesures et vous connaissez l'angle de référence que vous souhaitez obtenir. appelons le angleReference, c'est une constante globale par exemple 0° si votre capteur est posé de façon à ce que la lecture quand le bateau est "à plat" donne 0°

Pour calculer la partie proportionnelle pidProportionnelle, on a juste besoin de l'erreur courante c'est currentError = (currentPitch - angleReference)

pour la dérivée pidDerivee, il nous faut calculer
* l'erreur precedente c'est lastError = (lastPitch - angleReference)
* le temps ∆t écoulé entre les deux mesures qui est de deltaT = currentChrono - lastChrono
la dérivée est alors (currentError - lastError) / deltaT

Enfin pour l'intégrale pidIntegrale on cumule les erreurs courantes currentError pondérées par un coefficient constant K_i

On aura donc

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
pidProportionnelle = Kp * currentError;
pidDerivee = Kd * (currentError - lastError) / deltaT;
pidIntegrale = pidIntegrale + (Ki * currentError);
pidTotal = pidProportionnelle + pidDerivee + pidIntegrale;

il faudra affiner les 3 coefficients K_x bien sûr par la pratique. commencez par K_p en mettant les 2 autres à 0, puis affinez le comportement avec la dérivée K_d et enfin K_i comme il le montre dans sa vidéo.

Ce pidTotal va varier entre deux grandes bornes PID_MIN et PID_MAX, quand vous devez aller à fond CCW ou à fond CW. ces deux bornes dépendent des coefficients retenus, elles se mesurent dans la pratique en faisant marcher le système et en regardant l'évolution de pidTotal

Une fois que vous avez vu un peu sa zone d'amplitude, vous pouvez forcer le fait qu'il y reste pour éviter toute mauvaise surprise avec un

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

 pidTotal = constrain(pidTotal, PID_MIN, PID_MAX); // https://www.arduino.cc/reference/en/language/functions/math/constrain/

NOTE:
ce constrain() correspond au code que vous aviez du mal à comprendre ci dessous:

Code : Sélectionner tout - Visualiser dans une fenêtre à part

1
2
if(PID_total < 20){PID_total = 20;} if(PID_total > 160) {PID_total = 160; }

en gros il voulait toujours un PID_total entre 20 et 160.

Maintenant on est sûr que pidTotal est entre ces deux bornes et il faut transformer cela en vitesse moteur et direction.

Comme la commande de votre moteur se fait en PWM avec un

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

 analogWrite(moteurVitessePin, pwm);

avec pwm entre 0 et 255 on va associer au PID une valeur entre -255 et 255, le signe indiquant le sens de déplacement (à valider avec votre PID pour que négatif veuille dire CW et positif CCW)

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

int pilotageMoteur = map(pidTotal, PID_MIN, PID_MAX, -255, 255);

Comme il faut tenir compte de vos détecteurs de fin de course on va bidouiller cette "vitesse" en fonction de la position des capteurs avant d'essayer de piloter les moteurs.

imaginons qu'on ait

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
const byte capteurFinDeCourseCW = A0; // le capteur de fin de course qu'on touche en avançant en "CW"
const byte capteurFinDeCourseCCW = A1; // le capteur de fin de course qu'on touche en avançant en "CCW"
....
// dans le setup
  pinMode(capteurFinDeCourseCW,INPUT_PULLUP);
  pinMode(capteurFinDeCourseCCW,INPUT_PULLUP);

alors on peut modifier le PWM théorique

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
if ( (pilotageMoteur < 0) && (digitalRead(capteurFinDeCourseCW) == LOW)) { // si on essaye d'aller CW et qu'on touche déjà le capteur de ce côté
  pilotageMoteur = 0;
} else
if ( (pilotageMoteur > 0) && (digitalRead(capteurFinDeCourseCCW) == LOW)) { // si on essaye d'aller CCW et qu'on touche le capteur de ce côté
  pilotageMoteur = 0;
}

maintenant donc même si le PID nous dit de faire tourner le moteur en CW ou CCW mais qu'on est en butée, on n'avancera pas.

On peut donc passer à l'ordre moteur et on peut un peu affiner pour éviter les petits mouvements liés à des erreurs possibles de lecture, donc on peut décider de dire que si le PWM calculé est entre -2 et 2 par exemple, on ne fait rien.

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
if (pilotageMoteur > 2) { // on peut affiner un hystérésis en mettant par exemple +2 au lieu de 0
   digitalWrite(moteurDirectionPin, moteurDirectionCCW); //fait avancer le chariot vers la droite
   analogWrite(moteurVitessePin, pilotageMoteur); // on sait que pilotageMoteur est positif
} else if (pilotageMoteur < -2) { // on peut affiner un hystérésis en mettant par exemple -2 au lieu de 0
  digitalWrite(moteurDirectionPin, moteurDirectionCW);
  analogWrite(moteurVitessePin, -pilotageMoteur); // on met -pilotageMoteur car pilotageMoteur est négatif et un PWM par analogWrite doit être positif
} else {
  // le pwm était entre -2 et 2, on considère qu'on n'a pas à bouger
  analogWrite(moteurVitessePin, 0); // arrêt moteur
}

En gros c'est un peu comme cela qu'il faut aborder votre problème

**seb201** · 16/03/2020, 17h26

bonsoir,
Pouriez vous m'aider , je ne comprend pas l'erreur:
Pièce jointe 546389
Voici le code actuel:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
#include <AFMotor.h>
#define moteurDirectionPin 8
#define moteurVitessePin 11
#define moteurDirectionCW HIGH                   
#define moteurDirectionCCW LOW  
 
float angleReference=0;
float pidProportionnelle=0;
float pidIntegrale=0;
float pidDerivee=0;
/////////////////PID CONSTANTS/////////////////
double Kp=3.55;//3.55
double Ki=0.005;//0.003
double Kd=2.05;//2.05
///////////////////////////////////////////////
MPU6050 mpu;
const float RADIANS_TO_DEGREES = 180.0 / M_PI;
// MPU control/status vars
uint8_t mpuIntStatus;   // contient l'octet d'état d'interruption réel du MPU
uint16_t packetSize;    // taille de paquet DMP attendue (la valeur par défaut est de 42 octets)
uint16_t fifoCount;     // nombre de tous les octets actuellement dans FIFO 
uint8_t fifoBuffer[64]; // Tampon de stockage FIFO, doit être plus grand que packet
float ypr[3];
float euler[3];
uint32_t lastChrono;
// variables d'orientation/mouvement
Quaternion q;
VectorFloat gravity;
#ifdef EULER_ANGLES
float lastPsi ;
float lastTheta ;
float lastPhi;
#endif
 
#ifdef YPR_ANGLES
float lastYaw;
float lastPitch;
float lastRoll;
#endif
 
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}
 
void setup() {
  pinMode(capteurFinDeCourseCW,INPUT_PULLUP);
  pinMode(capteurFinDeCourseCCW,INPUT_PULLUP);
  Wire.begin();  //I2C bus
  Serial.begin(9600);
 
 
  // initialize device
  Serial.println("Initialisation I2C...");
  accelgyro.initialize();
 
  // verify connection
  Serial.println("Test de la conection du dispositif ...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection reussie" : "MPU6050 connection echec");
  delay(1000);
 
  Serial.begin(9600);          
 
}
 
void loop() {
  if (mpuInterrupt || fifoCount >= packetSize) { // on a des données
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {    
      mpu.resetFIFO();       // reset
    } else if (mpuIntStatus & 0x02) {     
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 
      mpu.getFIFOBytes(fifoBuffer, packetSize); 
      fifoCount -= packetSize;
 
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      uint32_t currentChrono = micros();
 
#ifdef YPR_ANGLES
      // Obtenir des angles YPR à partir du tampon
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      float currentPitch = -ypr[1] * RADIANS_TO_DEGREES;    // pas: (nez haut / bas, autour de l'axe Y)
 
      if (abs(currentPitch - lastPitch) > 1){
        Serial.println(F("P\t"));
        Serial.print(currentPitch, 0);
        Serial.write("\t");       
        lastPitch = currentPitch;
      }
#endif
 
    }
  }
/*///////////////////////////P I D///////////////////////////////////*/
  currentError = (currentPitch - angleReference);
  pidProportionnelle = Kp * currentError;
  lastError = (lastPitch - angleReference) ;
  deltaT = currentChrono - lastChrono;
  pidDerivee = Kd * (currentError - lastError) / deltaT;
if(-3 < currentError)and( currentError < 3){
      pidIntegrale = pidIntegrale + (ki * currentError);
    }
    else {
      pidIntegrale = 0;
    }
  pidTotal = pidProportionnelle + pidDerivee + pidIntegrale;
  pidTotal = map(PID_total, -150, 150, 0, 150);
  pidTotal = constrain(pidTotal, PID_MIN, PID_MAX);
  distance_previous_error = distance_error;
}
 
void controlemotV2(int alpha) {
 
int Val1=digitalRead(capteurFinDeCourseCW);
 Serial.println(Val1);
 
 int Val2=digitalRead(capteurFinDeCourseCCW);
 Serial.println(Val2);
 
if (pilotageMoteur > 2) { 
   digitalWrite(moteurDirectionPin, moteurDirectionCCW); 
   analogWrite(moteurVitessePin, pilotageMoteur); 
if ( (pilotageMoteur > 0) and (digitalRead(capteurFinDeCourseCCW) == LOW)) { 
  pilotageMoteur = 0;
}
} 
else if (pilotageMoteur < -2) { /
  digitalWrite(moteurDirectionPin, moteurDirectionCW);
  analogWrite(moteurVitessePin, -pilotageMoteur); 
if ( (pilotageMoteur < 0) && (digitalRead(capteurFinDeCourseCW) == LOW)) { 
  pilotageMoteur = 0;
}
} 
else {
  analogWrite(moteurVitessePin, 0);
}
}

**Jay M** · 16/03/2020, 18h06

Mais pourquoi je vois encore du accelGyro ??
Je vous ai dit deux fois que c’est soit le premier code soit le second pour gérer votre MPU... Si vous voulez utiliser le DMP et le second code, il faut oublier votre ancien code, le mettre dans un coin, ne plus le toucher... je ne sais pas comment le dire différemment...

Sinon pour les quaternions ça vient des includes du second code

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
 #include "Wire.h"
#include "I2Cdev.h" //copier https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev  dans le répertoire des librairies
#include "MPU6050_6Axis_MotionApps20.h" //copier https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 dans le répertoire des librairies
 
MPU6050 mpu;

**seb201** · 16/03/2020, 19h11

Excusez moi, c'est juste que je commence à m'emmêler les pinceaux avec tout ces programmes .😂😂😂😉`
Voici le dernier d'actualité :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
#include "Wire.h"
#include "I2Cdev.h" 
#include "MPU6050_6Axis_MotionApps20.h" 
#define moteurDirectionPin 8
#define moteurVitessePin 11
#define moteurDirectionCW HIGH                   
#define moteurDirectionCCW LOW  
MPU6050 mpu; 
const float RADIANS_TO_DEGREES = 180.0 / M_PI;
 uint8_t mpuIntStatus; 
uint16_t packetSize;    
uint16_t fifoCount;    
uint8_t fifoBuffer[64]; 
uint32_t lastChrono;
float ypr[3];
float euler[3];
float pidProportionnelle=0;
float pidIntegrale=0;
float pidDerivee=0;
const byte capteurFinDeCourseCW = A0; 
const byte capteurFinDeCourseCCW = A1;
/////////////////PID CONSTANTS/////////////////
double Kp=3.55;//3.55
double Ki=0.005;//0.003
double Kd=2.05;//2.05
///////////////////////////////////////////////
#ifdef EULER_ANGLES
float lastPsi ;
float lastTheta ;
float lastPhi;
#endif
 
#ifdef YPR_ANGLES
float lastYaw;
float lastPitch;
float lastRoll;
#endif
 
volatile bool mpuInterrupt = false;     
void dmpDataReady() {
  mpuInterrupt = true;
}
Quaternion q;
VectorFloat gravity;
 
void setup() {
  pinMode(capteurFinDeCourseCW,INPUT_PULLUP);
  pinMode(capteurFinDeCourseCCW,INPUT_PULLUP);
  uint8_t dmpStatus;
 
  Wire.begin();
  Serial.begin(115200);
 
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println(F("pas de MPU"));
    while (true);
  }
 
  if (dmpStatus = mpu.dmpInitialize()) {
    Serial.print(F("Erreur DMP #"));
    Serial.println(dmpStatus);
    while (true);
  }
 
  mpu.setDMPEnabled(true);
  attachInterrupt(0, dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus();
  packetSize = mpu.dmpGetFIFOPacketSize(); 
}
 
void loop(){
  if (mpuInterrupt || fifoCount >= packetSize) { 
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {    
      mpu.resetFIFO();       // reset
    } else if (mpuIntStatus & 0x02) {     
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
      mpu.getFIFOBytes(fifoBuffer, packetSize); 
      fifoCount -= packetSize;
 
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      uint32_t currentChrono = micros();
 
#ifdef YPR_ANGLES
      // Obtain YPR angles from buffer
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
       float currentPitch = -ypr[1] * RADIANS_TO_DEGREES;   
if (abs(currentPitch - lastPitch) > 1){
        Serial.print(F("P\t"));
        Serial.print(currentPitch, 0);
        Serial.write("\t");
        lastPitch = currentPitch;
        currentError = (currentPitch - angleReference);
        pidProportionnelle = Kp * currentError;
        lastError = (lastPitch - angleReference) ;
        deltaT = currentChrono - lastChrono;
        pidDerivee = Kd * (currentError - lastError) / deltaT;
if(-3 < currentError)and( currentError < 3){
      pidIntegrale = pidIntegrale + (ki * currentError);
    }
    else {
      pidIntegrale = 0;
    }
  pidTotal = pidProportionnelle + pidDerivee + pidIntegrale;
  pidTotal = map(PID_total, -150, 150, 0, 150);
  pidTotal = constrain(pidTotal, PID_MIN, PID_MAX);
  int pilotageMoteur = map(pidTotal, PID_MIN, PID_MAX, -255, 255);
}
 
      }
#endif
      lastChrono = currentChrono;
    }
  }
}
void controlemotV2(int currentPitch) {
 
  int Val1=digitalRead(capteurFinDeCourseCW);
 Serial.println(Val1);
 
 int Val2=digitalRead(capteurFinDeCourseCCW);
 Serial.println(Val2);
 
 
if (pilotageMoteur > 2) { 
   digitalWrite(moteurDirectionPin, moteurDirectionCCW); 
   analogWrite(moteurVitessePin, pilotageMoteur); 
if ( (pilotageMoteur > 0) and (digitalRead(capteurFinDeCourseCCW) == LOW)) { 
  pilotageMoteur = 0;
}
} 
else if (pilotageMoteur < -2) { /
  digitalWrite(moteurDirectionPin, moteurDirectionCW);
  analogWrite(moteurVitessePin, -pilotageMoteur); 
if ( (pilotageMoteur < 0) && (digitalRead(capteurFinDeCourseCW) == LOW)) { 
  pilotageMoteur = 0;
}
} 
else {
  analogWrite(moteurVitessePin, 0);
}
}

Pour résoudre ce problème dois-je tout mettre dans la meme boucle?Pièce jointe 546419

**Jay M** · 16/03/2020, 19h22

Ce bout de code

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
 if(-3 < currentError)and( currentError < 3){
      pidIntegrale = pidIntegrale + (ki * currentError);
    }
    else {
      pidIntegrale = 0;
    }

est un bidouillage du gars sur la vidéo, pour le moment vous ne savez pas s’il faut en faire ou pas donc on le vire

Ensuite réfléchissez à la structure logique du code:

On mesure l’angle courant
On calcule le PID
On affine le PID avec des contraintes min et Max
On transforme le PID en ordre PWM et direction
On prend en compte les contacteurs de fin de course si on est en butée pour forcer la vitesse (le pwm) a zéro
Enfin on donne l’ordre au moteur

Puis on recommence

C’est ce que doit faire votre loop(). Vous pouvez segmenter en petites fonctions ou tout mettre d’un coup, à vous de voir mais si vous séparez en fonctions, il faut prendre en compte la portée (lire des trucs sur variable scope) des variables. Seules les globales seront visibles entre toutes les fonctions. Ici si vous déclarez une variable dans la loop, elle est locale à la loop et inconnue ailleurs. Soit vous la passez en globale, soit en paramètre lors de l’appel à la fonction

**seb201** · 16/03/2020, 20h16

Voila le code après correction:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
#include "Wire.h"
#include "I2Cdev.h" 
#include "MPU6050_6Axis_MotionApps20.h" 
#define moteurDirectionPin 8
#define moteurVitessePin 11
#define moteurDirectionCW HIGH                   
#define moteurDirectionCCW LOW  
MPU6050 mpu; 
const float RADIANS_TO_DEGREES = 180.0 / M_PI;
 uint8_t mpuIntStatus; 
uint16_t packetSize;    
uint16_t fifoCount;    
uint8_t fifoBuffer[64]; 
uint32_t lastChrono;
float ypr[3];
float euler[3];
float pidProportionnelle=0;
float pidIntegrale=0;
float pidDerivee=0;
const byte capteurFinDeCourseCW = A0; 
const byte capteurFinDeCourseCCW = A1;
/////////////////PID CONSTANTS/////////////////
double Kp=3.55;//3.55
double Ki=0.005;//0.003
double Kd=2.05;//2.05
///////////////////////////////////////////////
#ifdef EULER_ANGLES
float lastPsi ;
float lastTheta ;
float lastPhi;
#endif
 
#ifdef YPR_ANGLES
float lastYaw;
float lastPitch;
float lastRoll;
#endif
 
volatile bool mpuInterrupt = false;     
void dmpDataReady() {
  mpuInterrupt = true;
}
Quaternion q;
VectorFloat gravity;
 
void setup() {
  pinMode(capteurFinDeCourseCW,INPUT_PULLUP);
  pinMode(capteurFinDeCourseCCW,INPUT_PULLUP);
  uint8_t dmpStatus;
 
  Wire.begin();
  Serial.begin(115200);
 
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println(F("pas de MPU"));
    while (true);
  }
 
  if (dmpStatus = mpu.dmpInitialize()) {
    Serial.print(F("Erreur DMP #"));
    Serial.println(dmpStatus);
    while (true);
  }
 
  mpu.setDMPEnabled(true);
  attachInterrupt(0, dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus();
  packetSize = mpu.dmpGetFIFOPacketSize(); 
}
 
void loop(){
  if (mpuInterrupt || fifoCount >= packetSize) { 
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {    
      mpu.resetFIFO();       // reset
    } else if (mpuIntStatus & 0x02) {     
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
      mpu.getFIFOBytes(fifoBuffer, packetSize); 
      fifoCount -= packetSize;
 
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      uint32_t currentChrono = micros();
 
#ifdef YPR_ANGLES
      // Obtain YPR angles from buffer
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
       float currentPitch = -ypr[1] * RADIANS_TO_DEGREES;   
if (abs(currentPitch - lastPitch) > 1){
        Serial.print(F("P\t"));
        Serial.print(currentPitch, 0);
        Serial.write("\t");
        lastPitch = currentPitch;
        currentError = (currentPitch - angleReference);
        pidProportionnelle = Kp * currentError;
        lastError = (lastPitch - angleReference) ;
        deltaT = currentChrono - lastChrono;
        pidDerivee = Kd * (currentError - lastError) / deltaT;
       pidIntegrale = pidIntegrale + (ki * currentError);
    }
  pidTotal = pidProportionnelle + pidDerivee + pidIntegrale;
  pidTotal = map(PID_total, -150, 150, 0, 150);
  pidTotal = constrain(pidTotal, PID_MIN, PID_MAX);
  int pilotageMoteur = map(pidTotal, PID_MIN, PID_MAX, -255, 255);
}
 
      }
#endif
      lastChrono = currentChrono;
 
    }
 
  }
}
void controlemotV2(int pilotageMoteur) {
 
  int Val1=digitalRead(capteurFinDeCourseCW);
 Serial.println(Val1);
 
 int Val2=digitalRead(capteurFinDeCourseCCW);
 Serial.println(Val2);
 
if (pilotageMoteur > 2) { 
   digitalWrite(moteurDirectionPin, moteurDirectionCCW); 
   analogWrite(moteurVitessePin, pilotageMoteur); 
if ( (pilotageMoteur > 0) and (digitalRead(capteurFinDeCourseCCW) == LOW)) { 
  pilotageMoteur = 0;
}
} 
else if (pilotageMoteur < -2) { 
  digitalWrite(moteurDirectionPin, moteurDirectionCW);
  analogWrite(moteurVitessePin, -pilotageMoteur); 
if ( (pilotageMoteur < 0) && (digitalRead(capteurFinDeCourseCW) == LOW)) { 
  pilotageMoteur = 0;
}
} 
else {
  analogWrite(moteurVitessePin, 0);
}
}

Néanmoins je n'ai rien qui s'affiche dans le moniteurs et je ne comprend pas pourquoi.

**Jay M** · 16/03/2020, 22h37

vous n'avez pas défini un des deux éléments, YPR_ANGLES donc la compilation conditionnelle ne met pas le code...
--> virez tout le reste du code associé à EULER_ANGLES, enlevez les ifdef etc...

ensuite appuyez sur ctrl-T (ou cmd-T si vous êtes sur Mac) pour indenter le code.. il y a des accolades en trop.. ça ne va pas compiler.

vous continuez à définir pilotageMoteur dans le bloc

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

if (mpuIntStatus & 0x02) {

en faisant

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

int pilotageMoteur = map(pidTotal, PID_MIN, PID_MAX, -255, 255);

si vous avez lu ce que je vous ai posté sur scope, vous comprendrez que pilotageMoteur ne sera pas visible en dehors de ce bloc...

ne faites pas

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

      if (abs(currentPitch - lastPitch) > 1) {

c'était pour imprimer les valeur que quand elle changent.
Laisser tourner le PID à chaque tour de boucle, pas la peine d'imprimer l'angle on a vu que le code de base fonctionnait

currentError n'est pas déclaré

vous n'appelez jamais controlemotV2()

lastChrono doit être mémorisé que quand vous avez effectivement pris en compte cette mesure, donc à l'intérieur et à la fin du bloc if

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

if (mpuIntStatus & 0x02)

**jpbbricole** · 17/03/2020, 20h03

Bonsoir Sébastien

Excuses cet encart personnel

. J'ai tâté du servo de modellisme pour équillibrer mon mobile, j'ai actuellement un gyroscope GY-85 que j'ai de la peine à stabiliser. J'ai en commande un GY-521 MPU-6050, après ça je me lance dans les PID.
Nom : ARDDEV_BasculeEtServo.jpg
Affichages : 490
Taille : 86,2 Ko

Nom : ARDDEV_BasculeEtServo.jpg
Affichages : 490
Taille : 86,2 Ko

Le problème, avec un servo, c'est sa "brusquerie", il faudrait que je trouve une autre bibliothèque où je peux régler la progressivité.

A+
Cordialement
jpbbricole