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);
} |
Partager