Discussion :

[seb201]

Bonsoir tout le monde ceci est la suite de mon projet de stabilisateur de bateau a masse mobile (Stabilisateur du porte avion Charle de Gaule ).Si vous souhaitez voir tout le debut de ce projet je vous invite a jeter un coup d'oeil sur ce lien :https://www.developpez.net/forums/d1...u-arduino-uno/. Si vous êtes toujours là ,mon projet actuel est de faire en sorte que la planche ce stabilise le plus rapidement possible à l'aide d'un correcteur PID. Je ne suis qu'au tout début de ce codage , j'ai pour le moment récupérer un code de correcteur PID d'un stabilisateur de drone avec 2 moteurs que j'ai déjà commencé à adapter. Néanmoins je fais face a quelque soucis, car je ne comprend pas absolument tout le code et a vrai dire je me sens un peu perdue . C'est pourquoi je sollicite votre aide . Merci à tous d'avance.

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
#include <Wire.h>
int16_t Acc_rawY,Gyr_rawY;
float Acceleration[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_d =0;
float pid_i =0;
float pid_p =0;
double kp=3.55;
double kd=0.005;
double ki=2.05;
double throttle =1300;
float desired_angle =0;
 
 
void setup() {
  Wire.begin();
  Wire.beginTransmission(0x68);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(250000);
  moteur.attach(...);
  time = millis();
  moteur.writeMicroseconds(1000);
  delay(7000);
}
 
void loop() {
 
  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_erreur)/elapsedTime);
  PID = pid_d + pid_i + pid_p;
if(PID<-1000){
  PID=-1000;
}
if (PID>1000){
  PID=1000
}
  pwmLeft = throttle +PID;
  pwmRight = throttle -PID;
 
//Right
if (pwmRight < 1000){
  pwmRight =1000;
 
}
if(pwmRight>2000){
  pwmRight =2000;
} 
//Left
if (pwmLeft < 1000){
  pwmLeft =1000;
 
}
if(pwmLeft>2000){
  pwmLeft =2000;
}
 
 
 
 
Ce que je pensais faire!!!
 
int i = 200
 
if (abs (int alpha(t) - int alpha(t+1)) < 2 ){
  i+=5
  moteur.setSpeed(i)
}
else if (abs (int alpha(t) - alpha(t+1)) > 2 ){
  i-=5
  moteur.setSpeed(i)
}

Je ne comprend pas ceci:

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
if(PID<-1000){
  PID=-1000;
}
if (PID>1000){
  PID=1000
}
  pwmLeft = throttle +PID;
  pwmRight = throttle -PID;
 
//Right
if (pwmRight < 1000){
  pwmRight =1000;
 
}
if(pwmRight>2000){
  pwmRight =2000;
} 
//Left
if (pwmLeft < 1000){
  pwmLeft =1000;
 
}
if(pwmLeft>2000){
  pwmLeft =2000;
}

et ceci: //car c'est à dire que si error n'est pas inclus dans cette intervalle alors il n'a pas de correcteur intégrateur ,pourquoi donc?

Code :

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

1
2
3
if (-3<error) and (error<3){
  pid_i = pid_i +(ki*error);
}

[Jay M]

j'ai vu

qui reprend le concept d'une position destination donnée par une entrée externe (ici ils ont mis un encodeur rotatif) qui est un peu similaire à ce que jpbricole avait effectué avec son potentiomètre dans sa simulation. ça ne devrait pas être top dur à changer pour prendre comme consigne donc celle du MPU.

le code se trouve ici

ils utilisent la librairie TimerOne

Code :

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

1
2
  Timer1.initialize(sampleTime * 1000);               // setup timer 1
  Timer1.attachInterrupt(Compute);

qui leur permet d'exécuter la fonction Compute() toutes les N µs, ici sampleTime vaut 10 donc ils appellent la fonction 100 fois par seconde. ça peut se faire sans cette librairie bien sûr, juste avec l'approche avec millis()

Le régulateur PID est codé "à la main" dans la fonction Compute() avec des bornes min et max pour le pilotage de la vitesse.


cette autre vidéo est pas mal pour comprendre le principe aussi

[seb201]

Merci beaucoup mais j'ai déjà regardé pas mal de vidéo du deuxièmes mecs car il a fait pas mal de video sur les pid .Mais bon pour le moment je n'en suis qu'au tout début et puis je n'est pas trop de temps pour cela non plus malheureusement . Mais merci pour le premier lien que je n'avais pas encore vue je vais jeter un coup d'deuil au code et je vous tiendrai au courant.😉😊

[seb201]

Je ne sais pas si c'est possible et si ça intéresserait quelqu'un mais ça serait cool je trouve si on pouvait se réunir une soirée ou se fixer une heure et avoir une conversation à plusieurs .Après avoir ,que ça soit sur un chat vocal ou sur le chat du site par exemple pour pouvoir avancer plus vite sur le projet et avoir comme une relation prof/éleves ou nous réunissons tous notre savoir en directe .J'aimerai connaitre un peu votre avis sur la question .Pourriez vous mettre un pouce vert si vous êtes d'accord et rouge sinon . Merci bien.😊😊

[seb201]

J'ai essayé de faire quelque essai et j'ai graisser mes axes de poulies et mon moteur pour leur donner une seconde vie et on voit vraiment la différence .Néanmoins au bout de 10 /15 seconde mon lmd182000t surchauffe et deviens intouchable malgré un dissipateur thermique ferreux, je cesse donc de faire tourner mon programme.Il est pourtant dit dans le datasheetqu'il supporte des piques de 6A de courant et 3A en continu . Je ne sais pas si il faut envisager le remplacer ,et par quoi dans ce cas?? Ou bien envisager un water cooling ?? J'ai fais des mesure en faisant tourner le moteur a vide car les valeurs non pas le temps de s'afficher si je rajoute les contraintes du système et je consomme 11,56V et 2,0 mA sur le calibre 600mA lorsque le moteur tourne dans le sens trigonométrique et 11,71V et 3,8mA sur le calibre 600mA lorsque le moteur tourne dans le sens des aiguilles d'une montre.A quoi cela peut il etre du et comment équilibrer ces valeurs ?De plus j'ai continuer le correcteur PID mais je bloque a cette étape je ne sais pas quoi écrire par la suite...

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
#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[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_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;
                  // 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();        
  @moteur.writeMicroseconds(1000);     
  delay(7000);
 
}
 
 
void loop() {
 
  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);
}
else{
  pid_i = 0
}
  pid_d = kd*((error - previous_erreur)/elapsedTime);
  PID = pid_d + pid_i + pid_p;
}
 
for i in range(0,255){
  if (abs (int alpha(t) - int alpha(t+1)) < 2 ){
  i+=PID
  moteur.setSpeed(i)
}
else if (abs (int alpha(t) - alpha(t+1)) > 2 ){
  i-=PID
  moteur.setSpeed(i)
}
}

Et je ne comprend pas les lignes qui ont un @ à leur début.

[seb201]

Ah oui j'avais oublié voici mon circuit en entier.
Pièce jointe 540392

[Jay M]

OK vous pouvez virer

Code :

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

#define YPR_ANGLES

et mettre en dur dans le code tout ce qui est associé puisque c'est ce dont vous avez besoin (y compris le tableau euler[])

attention dans votre fonction correcteur, le mieux est de conserver le float pas convertir en int

Code :

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

void correcteur(float currentPitch)

angleReference est une constante, autant le dire au compilateur

Code :

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

const float angleReference = 0;

(sauf si vous voulez le mesurer au lancement du sketch en vous disant que le bateau est au repos au branchement);
Il en va de même pour les constantes K_x du PID

Code :

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

1
2
3
const double Kp = 3.55; //3.55
const double Ki = 0.005; //0.003
const double Kd = 2.05; //2.05

pour deltaT et currentChrono il vaut mieux conserver unsigned long puisque ça vient de micros() (vous pouvez aussi choisir millis(), faut juste en tenir compte dans les unités)

Code :

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

1
2
float unsigned long currentChrono;
float unsigned long deltaT;

vous calculez bien un pidTotal mais le constrain et le map m'interpellent

Code :

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

1
2
  pidTotal = constrain(pidTotal, -90, 90);
  int pilotageMoteur = map(pidTotal, -90, 90, 0, 255);

D'une part qu'est-ce qui vous dit que le pidTotal va être entre -90 et 90 --> avez vous mesuré cela en le laissant tourner un peu ? autant définir les constantes PID_TOTAL_MIN et PID_TOTAL_MAX au début du code.

Code :

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

1
2
const int PID_TOTAL_MIN = -90; // à affiner !
const int PID_TOTAL_MAX = +90; // à affiner !

D'autre part, pilotageMoteur doit être entre -255 et +255, pas juste 0 et 255 sinon on n'aura pas la direction moteur.


évitez d'imprimer les capteurs de fins de course (allumez des LEDs éventuellement) plus vous chargez les interruptions moins la lecture du MPU va être stable.

Code :

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

1
2
3
4
5
  int Val1 = digitalRead(capteurFinDeCourseCW);
  Serial.println(Val1);

  int Val2 = digitalRead(capteurFinDeCourseCCW);
  Serial.println(Val2);

ces valeurs seront LOW ou HIGH, donc quand vous faites les tests pour la lisibilité utilsez ces constantes

Code :

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

    if ( (pilotageMoteur > 0) and (Val2 == LOW)) {

vous avez inversé l'ordre des choses en faisant

Code :

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

1
2
3
4
5
    digitalWrite(moteurDirectionPin, moteurDirectionCCW);
    analogWrite(moteurVitessePin, pilotageMoteur);
    if ( (pilotageMoteur > 0) and (Val2 == 0)) {
      pilotageMoteur = 0;
    }

Il faut mettre pilotageMoteur à 0 si vous êtes en butée AVANT d'envoyer l'ordre au moteur de tourner, sinon vous allez forcer!


voilà en tenant compte de ces éléments, pour le moment ça ressemble à cela

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
150
151
152
153
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
 
#define moteurDirectionPin 8
#define moteurVitessePin 11
#define moteurDirectionCW HIGH
#define moteurDirectionCCW LOW
 
////////////////ANGLE_PITCH////////////////////
uint8_t mpuIntStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
uint32_t lastChrono;
 
MPU6050 mpu;
Quaternion q;
VectorFloat gravity;
const float RADIANS_TO_DEGREES = 180.0 / M_PI;
float ypr[3];
float lastPitch;
 
volatile bool mpuInterrupt = false;
 
void dmpDataReady() {
  mpuInterrupt = true;
}
 
 
///////////////////////////////////////////////
 
 
const byte capteurFinDeCourseCW = A0;
const byte capteurFinDeCourseCCW = A1;
float currentError;
const float angleReference = 0;
float pidTotal;
unsigned long currentChrono;
unsigned long deltaT;
float lastError;
float pidProportionnelle = 0;
float pidIntegrale = 0;
float pidDerivee = 0;
/////////////////PID CONSTANTS/////////////////
const double Kp = 3.55; //3.55   // à affiner !
const double Ki = 0.005; //0.003 // à affiner !
const double Kd = 2.05; //2.05 // à affiner !
 
const int PID_TOTAL_MIN = -90; // à affiner !
const int PID_TOTAL_MAX = +90; // à affiner !
 
///////////////////////////////////////////////
 
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();
 
    // 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.println(currentPitch, 0);
      Serial.write("\t");
      lastPitch = currentPitch;
      correcteurPID(currentPitch );
    }
    lastChrono = currentChrono;
  }
}
 
void correcteurPID(float currentPitch) 
{
  currentError = (currentPitch - angleReference);
  pidProportionnelle = Kp * currentError;
  deltaT = currentChrono - lastChrono; // comme on est en microsecondes, on est sûr que ce ne sera pas 0 on peut faire la division
  lastError = (lastPitch - angleReference);
  pidDerivee = Kd * (currentError - lastError) / deltaT;
  pidIntegrale = pidIntegrale + (Ki * currentError);
  pidTotal = pidProportionnelle + pidDerivee + pidIntegrale;
 
  pidTotal = constrain(pidTotal, PID_TOTAL_MIN, PID_TOTAL_MAX);
  int pilotageMoteur = map(pidTotal, PID_TOTAL_MIN, PID_TOTAL_MAX, -255, 255);
  controleMoteur(pilotageMoteur );
}
 
void controleMoteur(int pilotageMoteur ) 
{
  int Val1 = digitalRead(capteurFinDeCourseCW);
  int Val2 = digitalRead(capteurFinDeCourseCCW);
 
  if ( (pilotageMoteur > 0) and (Val2 == LOW)) {
    pilotageMoteur = 0;
  }
 
  if ( (pilotageMoteur < 0) and (Val1 == 0)) {
    pilotageMoteur = 0;
  }
 
  if (pilotageMoteur > 2) {
    digitalWrite(moteurDirectionPin, moteurDirectionCCW);
    analogWrite(moteurVitessePin, pilotageMoteur);
  } else if (pilotageMoteur < -2) {
    digitalWrite(moteurDirectionPin, moteurDirectionCW);
    analogWrite(moteurVitessePin, -pilotageMoteur);
  } else {
    analogWrite(moteurVitessePin, 0);
  }
}

il faudra bien sûr tester, s'assurer qu'on tourne dans un temps raisonnable (un moteur a de l'inertie faut pas que la loop aille trop vite non plus, si c'est un problème il faudra envoyer les ordres au moteurs un peu moins souvent) affiner les coefficients ainsi que le MIN et le MAX ainsi que le +2 ou -2 qui sert de test pour la borne du PWM pour ne pas faire bouger le moteur.

[seb201]

Merci beaucoup Jay ,j'ai testé le code cependant lorsque les boutons sont pressés le moteur ne s'arrête pas de tourner( je ne comprend pas pourquoi) et le sytème ne répond pas correctement aux sollicitations.

[seb201]

vous calculez bien un pidTotal mais le constrain et le map m'interpellent

Code :

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

1
2
  pidTotal = constrain(pidTotal, -90, 90);
  int pilotageMoteur = map(pidTotal, -90, 90, 0, 255);

D'une part qu'est-ce qui vous dit que le pidTotal va être entre -90 et 90 --> avez vous mesuré cela en le laissant tourner un peu ? autant définir les constantes PID_TOTAL_MIN et PID_TOTAL_MAX au début du code.

Code :

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

1
2
const int PID_TOTAL_MIN = -90; // à affiner !
const int PID_TOTAL_MAX = +90; // à affiner !

je pensais que cela correspondait au valeur maximum du capteur d'inclinaison.😅😅😅

[Jay M]

@seb vous avez pu essayer de progresser un peu sur le code ?

[seb201]

@seb vous avez pu essayer de progresser un peu sur le code ?

Je m'embrouillais dans tout les codes que j'avais ,du coup j'ai tout supprimer et tout recommencé à zéro et voici ce que ça donne:

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
150
151
152
153
154
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#define YPR_ANGLES
#define moteurDirectionPin 8
#define moteurVitessePin 11
#define moteurDirectionCW HIGH
#define moteurDirectionCCW LOW
 
////////////////ANGLE_PITCH////////////////////
uint8_t mpuIntStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
uint32_t lastChrono;
 
MPU6050 mpu;
Quaternion q;
VectorFloat gravity;
const float RADIANS_TO_DEGREES = 180.0 / M_PI;
float ypr[3];
float euler[3];
volatile bool mpuInterrupt = false;
 
void dmpDataReady() {
  mpuInterrupt = true;
}
 
#ifdef YPR_ANGLES
float lastPitch;
#endif
///////////////////////////////////////////////
 
 
const byte capteurFinDeCourseCW = A0;
const byte capteurFinDeCourseCCW = A1;
float currentError;
float angleReference = 0;
float pidTotal;
float currentChrono;
float deltaT;
float lastError;
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
///////////////////////////////////////////////
 
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.println(currentPitch, 0);
      Serial.write("\t");
      lastPitch = currentPitch;
      correcteur(currentPitch );
    }
#endif
    lastChrono = currentChrono;
  }
}
 
 
void correcteur(int currentPitch) {
  currentError = (currentPitch - angleReference);
  pidProportionnelle = Kp * currentError;
  deltaT = currentChrono - lastChrono;
           lastError = (lastPitch - angleReference);
                       pidDerivee = Kd * (currentError - lastError) / deltaT;
  pidIntegrale = pidIntegrale + (Ki * currentError);
  pidTotal = pidProportionnelle + pidDerivee + pidIntegrale;
 
  pidTotal = constrain(pidTotal, -90,90);
  int pilotageMoteur = map(pidTotal, -90, 90, 0, 255);
  controlemot(pilotageMoteur );
}
 
void controlemot(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 (Val2 == 0)) {
      pilotageMoteur = 0;
    }
  }
  else if (pilotageMoteur < -2) {
    digitalWrite(moteurDirectionPin, moteurDirectionCW);
    analogWrite(moteurVitessePin, -pilotageMoteur);
    if ( (pilotageMoteur < 0) and (Val1 == 0)) {
      pilotageMoteur = 0;
    }
  }
  else {
    analogWrite(moteurVitessePin, 0);
  }
}

[Jay M]

je suppose que le moteur n'est pas alimenté par 2 piles AA ?

Comment comptez vous tester ? pour que le PID fasse son boulot, il faut que la correction influence la boucle de mesure. tel que votre montage existe, pas simple de le mettre dans l'eau.

[seb201]

Oui effectivement le système est alimenter à l'aide d'une lipo de 3s de11,1V et 5200mAh. Et je vais placer 2 tige traversant la planche en son millieu .Voici le schema que j'avais fait en tout début de conception:Pièce jointe 540747

[seb201]

Bonjour, voici la dernière version 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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
#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;
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;
                  // 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() {
 
  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;
  uint8_t i;
for(i=0; i<255; i++) {
if (abs (int alpha(t) - int alpha(t+1)) < 2 ){
  i+=5
  moteur.setSpeed(i)
}
else if (abs (int alpha(t) - alpha(t+1)) > 2 ){
  i-=5
  moteur.setSpeed(i)
}
previous_error = error;
}

[seb201]

Bonsoir,
Je n'arrive pas à voir d'ou viens le problème ,pourriez vous m'aider a comprendre.

[Jay M]

Bonsoir,
Je n'arrive pas à voir d'ou viens le problème ,pourriez vous m'aider a comprendre.

Il faut mettre les int entre parenthèses pour faire un cast (changement de type)

[Jay M]

C’est à connaître oui, mais les méthodes de Ziegler&Nichols Sont connues pour fournir des dépassements importants et donc vous allez taper dans les détecteur de fin de course sans doute

en y allant petit à petit on y parvient généralement. Cherchez des docs sur «*Réglage PID : méthode du régleur*» (ou méthodes empiriques)

Normalement rien qu’avec P vous devriez être capable de reproduire une réponse dans le bon sens à une sollicitation (vous penchez, le chariot doit partir pour contrebalancer)

[seb201]

Savez vous comment faire pour supprimer les photos partagées?

[Jay M]

'ai désormais un second problème je ne sais pas comment stocké 2angles à deux temps différents pour pouvoir les comparer

oui c'est pas magique comme ça, angle() n'est pas une fonction. est-ce juste la position précédente que vous voulez conserver ?

notez que vous ne pouvez pas gérer le temps correctement avec des float

Code :

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

float elapsedTime, time, timePrev;

il faut prendre des unsigned long (ou uint32_t)

pouvez vous poster la version en cours de votre code ?

[seb201]

Oui je souhaite compter la valeur de l'angle précédent avec celle de l'angle actuel.
Voici le code sans correcteur puis celui du 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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
#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 ( 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);
 
 }
}

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
150
#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;
 
 
 angle-t
 
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
}