Club Robotique

h1. Asservissement

<pre>

/*

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

}

</pre>