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
| #include <SimpleTimer.h>
// asservissement en position angulaire un moteur à courant continu.
SimpleTimer timer; // Timer pour échantillonnage
//definition des entrées
#define pi 3.14
#define r 20
#define Rpoul 0.5
#define PPR 11 //Nombre de ticks par tour de rotor (20*11 ticks par tour d'arbre de sortie)
#define tourparcm 1/(2*pi*Rpoul)
const int pinInput1 = 6; // Commande de sens moteur, Input 1
const int pinInput2 = 7; // Commande de sens moteur, Input 2
const int pinPower = 9; // Commande de vitesse moteur, Output Enabled1
const int encoderPinA = 2; // compteur 1
const int encoderPinB = 3; // compteur 2
//init echantillonage
unsigned int time = 0;
const int frequence_echantillonnage = 20;
//init compteur :
int encoder0Pos = 0; //position de départ=0
int lastReportedPos = 0;
boolean A_set = false;
boolean B_set = false;
//consigne
const double target_cm = -10;
double target_tour = tourparcm * target_cm;
int target_ticks; //plus simple d'asservir en ticks car ce sera toujours un nombre entier
// init calculs asservissement PID
int erreur = 0; //erreur
float erreurPrecedente = 0;
float somme_erreur = 0;
//Definition des constantes du correcteur PID
const float kp = 1; // Coefficient proportionnel (choisis par essais successifs)
const float ki = 0; // Coefficient intégrateur
const float kd = 0; // Coefficient dérivateur
/* Routine d'initialisation */
void setup()
{
target_ticks = target_tour * r * PPR ; //Consigne en ticks
Serial.begin(9600); // Initialisation port COM
Serial.println(F("CLEARDATA"));
Serial.println(F("LABEL, temps, consigne, erreur, encoder0pos, vitesse_moteur, angle_tour, distance"));
pinMode(pinPower, OUTPUT); // Sorties commande moteur
pinMode(pinInput1, OUTPUT);
pinMode(pinInput2, OUTPUT);
pinMode(encoderPinA, INPUT); //sorties encodeur
pinMode(encoderPinB, INPUT);
digitalWrite(encoderPinA, HIGH); // Resistance interne arduino ON
digitalWrite(encoderPinB, HIGH); // Resistance interne arduino ON
// Interruption de l'encodeur A en sortie 0 (pin 2)
attachInterrupt(0, doEncoderA, CHANGE);
// Interruption de l'encodeur A en sortie 1 (pin 3)
attachInterrupt(1, doEncoderB, CHANGE);
analogWrite(pinPower, 0); // Initialisation sortie moteur à 0
delay(300); // Pause de 0,3 sec pour laisser le temps au moteur de s'arréter si celui-ci est en marche
// Interruption pour calcul du PID et asservissement appelée toutes les 10ms
timer.setInterval(1000 / frequence_echantillonnage, asservissement);
}
/* Fonction principale */
void loop()
{
timer.run(); //on fait tourner l'horloge
}
//---- Cette fonction est appelée toutes les 20ms pour calcul du correcteur PID
void asservissement ()
{
time += 20; // pratique pour graphes excel après affichage sur le moniteur
erreur = target_ticks - encoder0Pos;
somme_erreur += erreur;
// Calcul de la vitesse courante du moteur
int vitMoteur = kp * erreur + kd * (erreur - erreurPrecedente) + ki * (somme_erreur);
erreurPrecedente = erreur; // Ecrase l'erreur précedente par la nouvelle erreur
// Normalisation et contrôle du moteur
if ( vitMoteur > 255 ) vitMoteur = 255; // on est branché sur un pont en H L293D ici, il s'agit de la saturation
else if ( vitMoteur < -255 ) vitMoteur = -255;
Tourner (vitMoteur);
float angle_tour = encoder0Pos / PPR / r; //Position angulaire de sortie (ici il s'agit de la sortie du réducteur et non pas de la sortie du moteur)
// pratique pour comparer avec la consigne d'entrée
float distance = angle_tour / tourparcm;
Serial.print(("DATA,TIME"));
Serial.print(F(","));
Serial.print(target_cm);
Serial.print(F(","));
Serial.print(erreur); // affiche sur le moniteur les données voulues
Serial.print(F(","));
Serial.print(encoder0Pos);
Serial.print(F(","));
Serial.print(vitMoteur);
Serial.print(F(","));
Serial.print(angle_tour);
Serial.print(F(","));
Serial.print(distance);
Serial.println();
}
//---- Interruption appelée à tous les changements d'état de A
void doEncoderA()
{
A_set = (digitalRead(encoderPinA) == HIGH);
encoder0Pos += (A_set != B_set) ? -1 : 1 ; //modifie le compteur selon les deux états des encodeurs ( correspond au front montant de A, on compare à B pour déduire le sens de rotation et on en déduit si on incrémente +1 ou -1)
}
//---- Interruption appelée à tous les changements d'état de B
void doEncoderB()
{
B_set = (digitalRead(encoderPinB) == HIGH);
encoder0Pos += (A_set == B_set) ? -1 : 1 ; //modifie le compteur selon les deux états des encodeurs
}
//---- Fonction appelée pour contrôler le moteur
void Tourner( int rapportCyclique )
{
if ( rapportCyclique > 0 )
{
digitalWrite(pinInput1, LOW);
digitalWrite(pinInput2, HIGH);
}
else
{
digitalWrite(pinInput1, HIGH);
digitalWrite(pinInput2, LOW);
rapportCyclique = -rapportCyclique;
}
analogWrite(pinPower, rapportCyclique);
} |
