Réguler la vitesse du MiniQ 2WD PART I⚓

Comment réguler la vitesse du robot ?

Deux paramètres de la fonction Motor_Control permettent de faire varier la vitesse des roues. On a vu qu'il était assez difficile d'obtenir une vitesse identique pour les deux moteurs avec une même valeur de paramètre. L'objectif ici va être  à partir d'une consigne de vitesse Vc :

  1. De mesurer la vitesse des 2 moteurs

  2. D'ajuster la commande en fonction de la différence entre la consigne et les mesures

On parle de régulation de vitesse. Le robot possède un encodeur par roue :

C'est la même technologie que pour le suiveur de ligne : une diode IR et un photo-transistor intégré dans le même boîtier.

Repérer sur le schéma ces deux détecteurs infrarouge.

Sur quel broche du micro-contrôleur sont relié les deux signaux ?

Représenter le schéma d'une cellule lorsqu'il n'y a pas d'IR recu :

Si la roue tourne à une vitesse de 100 tr/min, représenter ci-dessous la tension V0(t).

Exprimer la relation entre T(en s) la période du signal et N( en tr/s) la vitesse de la roue sachant que lorsque la roue fait un tour le signal décrit 12 périodes.

Réaliser un programme qui affiche l'état des signaux reçu au terminal série.

Que constate-t-on lorsque l'on fait tourner les roues (attention à ne pas trop forcer) ?

Interruptions

Ainsi en mesurant la période du signal on pourra déterminer la vitesse de la roue. Pour cela on utilisera une interruption.

A l'aide d'une carte Arduino et du module groove suivre cette activité tirée du site de mon club elec.

Etude du moteur droit

Le programme suivant permet la mesure de la vitesse du moteur droit :

1
// Broches de control du moteur 1
2
const int EN1 = 6; // Broche Marche/Arret Moteur
3
const int PWM1 = 7; // Broche control de la direction
4
5
// Broches de control du moteur 2
6
const int EN2 = 5;  // Broche Marche/Arret Moteur 
7
const int PWM2 = 12; // Broche control de la direction
8
// Constante pour le pilotage 
9
const int AVANCER = 0; //go forward
10
const int RECULER = 1; //go back
11
// Broches encodeurs droit et gauche:
12
const int ENCD = 0;
13
const int ENCG = 1;
14
15
volatile int count = 0;
16
long t0;
17
const int T = 500;
18
/* Fonction pilotage des moteurs :
19
Moteur 1 : Droite 
20
M1_DIR : AVANCER/RECULER
21
M1_EN  : 1-255 (PWM) 0 -> STOP
22
Moteur 2 : Gauche
23
M2_DIR : AVANCER/RECULER
24
M2_EN  : 1-255 (PWM) 0 -> STOP
25
*/
26
void Motor_Control(int M1_DIR,int M1_EN,int M2_DIR,int M2_EN)//control motor
27
{
28
  //////////M1////////////////////////
29
  if(M1_DIR==AVANCER)
30
    digitalWrite(PWM1,AVANCER);
31
  else
32
    digitalWrite(PWM1,RECULER);
33
  if(M1_EN==0)
34
    analogWrite(EN1,LOW);//stop
35
  else
36
    analogWrite(EN1,M1_EN); //Vitesse
37
38
  ///////////M2//////////////////////
39
  if(M2_DIR==AVANCER)
40
    digitalWrite(PWM2,AVANCER);
41
  else
42
    digitalWrite(PWM2,RECULER);
43
  if(M2_EN==0)
44
    analogWrite(EN2,LOW);
45
  else
46
    analogWrite(EN2,M2_EN);
47
}
48
49
void setup()
50
{
51
  pinMode(ENCD,INPUT);// Encoder droit
52
  attachInterrupt(digitalPinToInterrupt(ENCD), incrementer_compteur, CHANGE);
53
54
  Serial.begin(9600);
55
  pinMode(EN1  ,OUTPUT); // Broches moteurs en sortie
56
  pinMode(PWM1 ,OUTPUT);
57
  pinMode(EN2  ,OUTPUT);
58
  pinMode(PWM2 ,OUTPUT);
59
  Motor_Control(AVANCER,80,AVANCER,0); // Moteur droit
60
  t0 = millis();
61
  }
62
void loop()
63
{
64
    if(millis() - t0 > T)
65
    {
66
      Serial.print("count : ");
67
      Serial.println(count);
68
      Serial.print("vitesse :");
69
      float vitesse = (60.0*1000.0*(float)count)/( 24.0 *(float)T);
70
      Serial.print(vitesse);
71
      Serial.println(" tr/min");
72
      count = 0;
73
      t0 = millis();
74
    }
75
 
76
}
77
78
void incrementer_compteur() {
79
  count ++;
80
}
Tester le programme ci-dessus pour différentes valeurs de vitesse (20 à 255)
Commande
30
50
100
255
Vitesse
On souhaite agir sur la commande pour réguler la vitesse de rotation du moteur droit selon une consigne $Vc$ donnée en tr/min.
Réaliser un algorigramme du programme de la régulation de vitesse du moteur droit à l'aide de Draw.io
Implémenter la solution.
Régulation 1 roue
1
// Broches de control du moteur 1
2
const int EN1 = 6; // Broche Marche/Arret Moteur
3
const int PWM1 = 7; // Broche control de la direction
4
5
// Broches de control du moteur 2
6
const int EN2 = 5;  // Broche Marche/Arret Moteur 
7
const int PWM2 = 12; // Broche control de la direction
8
// Constante pour le pilotage 
9
const int AVANCER = 0; //go forward
10
const int RECULER = 1; //go back
11
// Broches encodeurs droit et gauche:
12
const int ENCD = 0;
13
const int ENCG = 1;
14
// Variables pour le calcul de la vitesse
15
volatile int count = 0; // Nb impulsion
16
long t0; 
17
const int T = 100; // Periode d'echantillonnage en ms
18
// Consigne vitesse :
19
20
float Vc = 100.0; // Consigne de vitesse 
21
float E  = 0.0;   // Erreur
22
float K  = 0.1; // Correcteur proportionnel
23
int   C  = 127;   // Commande vitesse
24
/* Fonction pilotage des moteurs :
25
Moteur 1 : Droite 
26
M1_DIR : AVANCER/RECULER
27
M1_EN  : 1-255 (PWM) 0 -> STOP
28
Moteur 2 : Gauche
29
M2_DIR : AVANCER/RECULER
30
M2_EN  : 1-255 (PWM) 0 -> STOP
31
*/
32
void Motor_Control(int M1_DIR,int M1_EN,int M2_DIR,int M2_EN)//control motor
33
{
34
  //////////M1////////////////////////
35
  if(M1_DIR==AVANCER)
36
    digitalWrite(PWM1,AVANCER);
37
  else
38
    digitalWrite(PWM1,RECULER);
39
  if(M1_EN==0)
40
    analogWrite(EN1,LOW);//stop
41
  else
42
    analogWrite(EN1,M1_EN); //Vitesse
43
44
  ///////////M2//////////////////////
45
  if(M2_DIR==AVANCER)
46
    digitalWrite(PWM2,AVANCER);
47
  else
48
    digitalWrite(PWM2,RECULER);
49
  if(M2_EN==0)
50
    analogWrite(EN2,LOW);
51
  else
52
    analogWrite(EN2,M2_EN);
53
}
54
55
void setup()
56
{
57
  pinMode(ENCD,INPUT);// Encoder droit
58
  attachInterrupt(digitalPinToInterrupt(ENCD), incrementer_compteur, CHANGE);
59
60
  Serial.begin(9600);
61
  pinMode(EN1  ,OUTPUT); // Broches moteurs en sortie
62
  pinMode(PWM1 ,OUTPUT);
63
  pinMode(EN2  ,OUTPUT);
64
  pinMode(PWM2 ,OUTPUT);
65
  Motor_Control(AVANCER,0,AVANCER,0); // Moteur droit
66
  t0 = millis();
67
  }
68
void loop()
69
{
70
    if(millis() - t0 > T)
71
    {
72
      float V = (60.0*1000.0*(float)count)/( 24.0 *(float)T); // Vitesse mesuree
73
      E = (Vc - V); // Erreur
74
      C = C + int(K * E);
75
76
      if(C<0)   
77
        C = 0;
78
      if(C > 255)  
79
        C = 255;
80
      Motor_Control(AVANCER,C,AVANCER,0); // Moteur droit
81
//      Serial.print("C : ");
82
//      Serial.println(C);
83
//      Serial.print("V  :");
84
//      Serial.println(V);
85
//      Serial.print("Vc  :");
86
//      Serial.println(Vc);
87
      Serial.println(V);
88
//      Serial.println(" tr/min");
89
      count = 0;
90
      t0 = millis();
91
      
92
    }
93
94
}
95
96
void incrementer_compteur() {
97
  count ++;
98
}
Régulation 2 roues
1
// Broches de control du moteur 1
2
const int EN1 = 6; // Broche Marche/Arret Moteur
3
const int PWM1 = 7; // Broche control de la direction
4
5
// Broches de control du moteur 2
6
const int EN2 = 5;  // Broche Marche/Arret Moteur 
7
const int PWM2 = 12; // Broche control de la direction
8
// Constante pour le pilotage 
9
const int AVANCER = 0; //go forward
10
const int RECULER = 1; //go back
11
// Broches encodeurs droit et gauche:
12
const int ENCD = 0;
13
const int ENCG = 1;
14
15
// Variables pour le calcul de la vitesse
16
volatile int countD = 0; // Nb impulsion
17
volatile int countG = 0; // Nb impulsion
18
long t0; 
19
const int T = 100; // Periode d'echantillonnage en ms
20
// Consigne vitesse :
21
22
float VDc = 150.0; // Consigne de vitesse 
23
float ED  = 0.0;   // Erreur
24
int   CD  = 50;   // Commande vitesse
25
26
float VGc = 150.0; // Consigne de vitesse 
27
float EG  = 0.0;   // Erreur
28
int   CG  = 50;   // Commande vitesse
29
30
float K  = 0.1; // Correcteur proportionnel
31
/* Fonction pilotage des moteurs :
32
Moteur 1 : Droite 
33
M1_DIR : AVANCER/RECULER
34
M1_EN  : 1-255 (PWM) 0 -> STOP
35
Moteur 2 : Gauche
36
M2_DIR : AVANCER/RECULER
37
M2_EN  : 1-255 (PWM) 0 -> STOP
38
*/
39
void Motor_Control(int M1_DIR,int M1_EN,int M2_DIR,int M2_EN)//control motor
40
{
41
  //////////M1////////////////////////
42
  if(M1_DIR==AVANCER)
43
    digitalWrite(PWM1,AVANCER);
44
  else
45
    digitalWrite(PWM1,RECULER);
46
  if(M1_EN==0)
47
    analogWrite(EN1,LOW);//stop
48
  else
49
    analogWrite(EN1,M1_EN); //Vitesse
50
51
  ///////////M2//////////////////////
52
  if(M2_DIR==AVANCER)
53
    digitalWrite(PWM2,AVANCER);
54
  else
55
    digitalWrite(PWM2,RECULER);
56
  if(M2_EN==0)
57
    analogWrite(EN2,LOW);
58
  else
59
    analogWrite(EN2,M2_EN);
60
}
61
62
void setup()
63
{
64
  pinMode(ENCD,INPUT);// Encoder droit
65
  attachInterrupt(digitalPinToInterrupt(ENCD), incrementer_compteurD, CHANGE);
66
  attachInterrupt(digitalPinToInterrupt(ENCG), incrementer_compteurG, CHANGE);
67
  Serial.begin(9600);
68
  pinMode(EN1  ,OUTPUT); // Broches moteurs en sortie
69
  pinMode(PWM1 ,OUTPUT);
70
  pinMode(EN2  ,OUTPUT);
71
  pinMode(PWM2 ,OUTPUT);
72
  Motor_Control(AVANCER,0,AVANCER,0); // Moteur droit
73
  t0 = millis();
74
  }
75
void loop()
76
{
77
    if(millis() - t0 > T)
78
    {
79
      float VD = (60.0*1000.0*(float)countD)/( 24.0 *(float)T); // Vitesse mesuree
80
      ED = (VDc - VD); // Erreur
81
      CD = CD + int(K * ED);
82
83
      if(CD<0)   
84
        CD = 0;
85
      if(CD > 255)  
86
        CD = 255;
87
      //Motor_Control(AVANCER,CD,AVANCER,CG); // Moteur droit
88
89
      float VG = (60.0*1000.0*(float)countG)/( 24.0 *(float)T); // Vitesse mesuree
90
      EG = (VGc - VG); // Erreur
91
      CG = CG + int(K * EG);
92
93
      if(CG<0)   
94
        CG = 0;
95
      if(CG > 255)  
96
        CG = 255;
97
      Motor_Control(AVANCER,CD,AVANCER,CG); // Moteur droit
98
      Serial.print("CD : ");
99
      Serial.println(CD);
100
      Serial.print("VD  :");
101
      Serial.println(VD);
102
      Serial.print("VDc  :");
103
      Serial.println(VDc);
104
      Serial.print("CG : ");
105
      Serial.println(CG);
106
      Serial.print("VG  :");
107
      Serial.println(VG);
108
      Serial.print("VGc  :");
109
      Serial.println(VGc);      //      Serial.println(VD);
110
      //      Serial.println(VG);
111
//      Serial.println(" tr/min");
112
      countD = 0;
113
      countG = 0;
114
      t0 = millis();
115
      
116
    }
117
118
}
119
120
void incrementer_compteurG() {
121
  countG++;
122
}
123
void incrementer_compteurD() {
124
  countD++;
125
}