Club Robotique

h1. 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 

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