Asservissement¶
/*
Travail du Club Robot de l'école Centrale Marseille
****************************************************************************************
Programme d'asservissement de moteurs
Version du 07/12/2015
Définition des constantes:
Constantes Physiques*/
float const d=10;//distance entre les roues en cm
float const Vmax=12.5;//vitesse max en cm/s
float const conversion=0.0486; //permet de transformer le nombre de ticks en vitesse par seconde
/* Définition des constantes pour la correction */
float const decalage=1;//le compteur G délivre moins de ticks que le droit pour une même vitesse
float const correctionRegulationMAv=251.0/200.0;//compense l'erreur de régulation en marche avant - Moteur Gauche trop rapide
float const correctionRegulationMAr=233.0/200.0;//compense l'erreur de régulation en marche arrière - Moteur gauche trop rapide
float const KiG=2.0;// correcteur intégrale du moteur gauche
float const KpG=18.0;// correcteur proportionnel du moteur gauche
float const KiD=2.0;
float const KpD=18.0;
/*
Méthode de calcul de la conversion
tick en 200ms -> tick/s -> tour/s -> cm/s
5 1/1938 6*Pi
*/
/*Définition des variables globales du moteur Gauche*/
int PWMG = 3; //pin du pwm du moteur gauche
int sensG=12;//pin du sens de rotation du moteur. Si sensG à LOW => marche avance. Si sensG à HIGH => marche arrière.
int entreeG = A5;// pin du capteur à effet haul du moteur
boolean etatG;
float consigneG=0;//en cm/s
float integraleG;
/*Définition des variables globales du moteur Droit*/
int PWMD=11;
int sensD=13;//Si sensD à LOW => marche avant. Si sensD à HIGH => marche arrière.
int entreeD=A3;// pin du capteur à effet haul du moteur
boolean etatD;
float consigneD=0;//en cm/s
float integraleD;
#include <Wire.h>
void setup() {
Wire.begin(); //join i2c bus (address optional for master)
pinMode(PWMG, OUTPUT);
pinMode(sensG, OUTPUT);
pinMode(PWMD, OUTPUT);
pinMode(sensD, OUTPUT);
pinMode(entreeG, INPUT);
pinMode(entreeD, INPUT);
Serial.begin(9600);
integraleG=0;
integraleD=0;
AdaptationConsigneRobot(7.0, 0);
}
void loop() {
int ticks[3];
int commandeSortie[3];
float vitesse[3];
AcquisitionTicks(ticks);
TraitementTicks(ticks,commandeSortie,vitesse);
/* Adaptation à l'alimentation par générateur*/
if(consigneG !=0 && consigneD != 0 && ticks[0]==0 && ticks[1]==0) {
analogWrite(PWMG,0);
analogWrite(PWMD, 255);//envoie de la commande au moteur droit
delay(100);
analogWrite(PWMG, 255);
} else{
//Envoi des commandes aux moteurs
analogWrite(PWMD, commandeSortie[1]);//envoie de la commande au moteur droit
analogWrite(PWMG, commandeSortie[0]);//envoie de la commande au moteur gauche
}
//Envoi de données pour correction du code
Serial.print(commandeSortie[0]);//Gauche
Serial.print(" ");
Serial.print(commandeSortie[1]);//Droit
Serial.print(" ");
Serial.print(consigneG);
Serial.print(" ");
Serial.print(consigneD);
Serial.print(" ");
Serial.print(vitesse[0]);//gauche
Serial.print(" ");
Serial.println(vitesse[1]);//droit
//Gestion de la communication i2c
communication(vitesse);
}
// Permet le comptage des ticks effectués lors de la rotation du moteur
void AcquisitionTicks(int ticks[3]) {
unsigned long timeInit=millis();
boolean UpG;
boolean UpD;
int tickG=0;
int tickD=0;
/*
Acquisition des ticks des deux moteurs
*/
while(millis()-timeInit < 100) {
//Détection des ticks pour le moteur de Gauche par détection de fronts descendants
if(analogRead(entreeG)*5.0/1024.0>2.0) {
UpG=true;
}
if(analogRead(entreeG)*5.0/1024.0<2.0 && UpG==true) {
UpG=false;
tickG++;
}
//Détection des ticks pour le moteur de Droite par détection de fronts descendants
if(analogRead(entreeD)*5.0/1024.0>2.0) {
UpD=true;
}
if(analogRead(entreeD)*5.0/1024.0<2.0 && UpD==true) {
UpD=false;
tickD++;
}
}
ticks[0]=tickG;
ticks[1]=tickD;
/*
Fin de la détection
****************************
*/
}
//permet de faire une rectification de la commande du moteur à partir du nombre de ticks
void TraitementTicks(int ticks[3], int commandeSortie[3], float vitesse[3]) {
int tickG=ticks[0];
int tickD=ticks[1];
int commandeSortieD;
int commandeSortieG;
float erreurG;
float erreurD;
float proportionnelleD;
float proportionnelleG;
float vitesseG;
float vitesseD;
tickG=tickG*decalage;
if(consigneG != 0 && tickG == 0) {//permet de vite faire démarrer le moteur en cas de faible vitesse
integraleG=200;
commandeSortieG=255;
} else {//cas normal
vitesseG=tickG*conversion;
erreurG = (consigneG - vitesseG);//application du correcteur;
integraleG =integraleG+ KiG*erreurG;
proportionnelleG = KpG*erreurG;
commandeSortieG = int(integraleG + proportionnelleG);
commandeSortieG = constrain(commandeSortieG, 0, 255);
}
/*
La fonction commande s'occupe de la commande a envoyé au moteur droit pour atteindre la consigne
*/
if(consigneD != 0 && tickD == 0) {//permet de vite faire démarrer le moteur en cas de faible vitesse
integraleD=200;
commandeSortieD=255;
} else {//cas normal
vitesseD=tickD*conversion;
erreurD = (consigneD - vitesseD);//application du correcteur;
integraleD =integraleD+ KiD*erreurD;
proportionnelleD = KpD*erreurD;
commandeSortieD = int(integraleD + proportionnelleD);
commandeSortieD = constrain(commandeSortieD, 0, 255);
}
//mise à jour des tableaux
ticks[0]=tickG;
ticks[1]=tickD;
commandeSortie[0]=commandeSortieG;
commandeSortie[1]=commandeSortieD;
vitesse[0]=vitesseG;
vitesse[1]=vitesseD;
//Fin du traitement du nombre de ticks pour déduire la commande des moteurs.
}
void AdaptationConsigneMoteur(boolean AvG, float consignePourMG, boolean AvD, float consignePourMD) {
//Mise à jour des consignes de vitesse des moteurs
consigneG=consignePourMG;
consigneD=consignePourMD;
int commandeSortieG;
int commandeSortieD;
//Redéfinition des sens de rotation du moteur gauche
if(AvG==false&&etatG != LOW) {
etatG=LOW;
integraleG=50;
commandeSortieG=255;
} else if(AvG==true&&etatG != HIGH) {
etatG=HIGH;
integraleG=50;
commandeSortieG=255;
}
//Redéfinition des sens de rotation du moteur droit
if(AvD==false&&etatD != LOW) {
etatD=LOW;
integraleD=50;
commandeSortieD=255;
} else if(AvD==true&&etatD != HIGH) {
etatD=HIGH;
integraleD=50;
commandeSortieD=255;
}
//Correction de la consigne pour l'adapter à la réalité - f(consigne)=rotation différent pour les deux moteurs-
if(etatD==false){
consigneD=consigneD*correctionRegulationMAv;
} else {
consigneD=consigneD*correctionRegulationMAr;
}
// Correction des consignes pour ne pas saturer les moteurs et obtenir n'importe quoi
if(consigneG>Vmax && consigneG>consigneD) {
consigneD=consigneD*Vmax/consigneG;
consigneG=Vmax;
} else if(consigneD>Vmax) {
consigneG=consigneG*Vmax/consigneD;
consigneD=Vmax;
}
/*
Transmission des consignes aux moteurs
* En cas de changement de sens, on transmet le dirac afin de faire changer le sens des moteurs.
* En cas de maintient du sens des moteurs, la valeur envoyé aux moteurs ne change pas et l'asservissement va se charger d'atteindre la consigne de vitesse
*/
digitalWrite(sensG, etatG);//envoie de la commande au moteur
digitalWrite(sensD, etatD);//envoie de la commande au moteur
analogWrite(PWMG, commandeSortieG);//envoie de la commande au moteur
analogWrite(PWMD, commandeSortieD);//envoie de la commande au moteur
}
void AdaptationConsigneRobot(float vitesseTranslation, float vitesseRotation) {
//vitesse en cm/s et vitesseRotation en rad/s
float vg=vitesseTranslation-vitesseRotation*d/2;
float vd=vitesseTranslation+vitesseRotation*d/2;
boolean etmg=false;
boolean etmd=false;
//mise en forme pour utilisation d'AdaptationConsigneMoteur(boolean AvG, float consignePourMG, boolean AvD, float consignePourMD);
if(vg<0) {
etmg=true;
vg=-vg;
}
if(vd<0) {
etmd=true;
vd=-vd;
}
AdaptationConsigneMoteur(etmg,vg,etmd,vd);
}
//Intégration de la variation de position
void VariationPosition(float vitesseG, float vitesseD, float tab[3]) {
tab[0]=0.2*vitesseG;
tab[1]=0.2*vitesseD;
}
void communication(float vitesse[3]) {
float tabPosition[3];
int tabPositionInt[3];
float vitesseG;
float vitesseD;
vitesseG=vitesse[0];
vitesseD=vitesse[1];
float vitesseTranslation;
float vitesseRotation;
VariationPosition(vitesseG, vitesseD, tabPosition);
tabPositionInt[0]=(int)(tabPosition[0]*10.0);
tabPositionInt[1]=(int)(tabPosition[1]*10.0); // !!!!!!!vérifier l'ajustement pour avoir une bonne précision, si besoin, changer de méthode
Wire.beginTransmission(8); // transmit to device #8
Wire.write(tabPositionInt[0]); // sends speed of left motor
Wire.write(tabPositionInt[1]); // sends speed of right motor
Wire.endTransmission(); // stop transmitting
Wire.requestFrom(8, 1); //request vitesseTranslation
while (Wire.available()) { // slave may send less than requested
int c = Wire.read(); // receive a byte as int
//Serial.print(c);
//Serial.print(" ");
vitesseTranslation=((float) c)/10.0;
}
Wire.requestFrom(8, 1); //request vitesseRotation
while (Wire.available()) { // slave may send less than requested
int c = Wire.read(); // receive a byte as int
//Serial.print(c);
//Serial.print(" ");
vitesseRotation=((float) c)/100.0;
}
//Serial.println("Et on repart");
AdaptationConsigneRobot( vitesseTranslation, vitesseRotation);
}