Commande des moteurs et Correction()

Initialisation des moteurs

void iniMoteur() {
  pinMode(Da, OUTPUT); pinMode(Db, OUTPUT);  // Commande moteur droit
  pinMode(Ga, OUTPUT); pinMode(Gb, OUTPUT);  // Commande moteur gauche
}  // iniMoteur()

Fonction Moteur(droite,gauche)

void Moteur(int droite, int gauche) {
  if (droite>0) {analogWrite(Db,0);analogWrite(Da,droite);}
           else {analogWrite(Db,-droite);analogWrite(Da,0);}
  if (gauche>0) {analogWrite(Gb,0);analogWrite(Ga,gauche);}
           else {analogWrite(Gb,-gauche);analogWrite(Ga,0);}            
}   // Moteur()

Fonction Correction(consigne)

void correction(int consigne) {
  ecart=consigne-angle;
  
  // Le compteur précis permet de valider l'exécution d'une consigne
  if (abs(ecart)<10) precis++; else precis=0;
  
  P=Kp*ecart;
  I=I+Ki*ecart*(dt/1000); // Somme des écarts : correction intégrale
  D =Kd*gz;       // La correction dérivée utilise directement la vitesse angulaire du capteur
  
  if (I>70) I=70;     // Mise en place de butée pour limiter l'instabilité de la correction I
  if (I<-70) I=-70;
  
  delta=P+I+D;  // la correction PID
  Vmax=vitesse-2*abs(D);
  
// Pour les virages <90° diminuer delta pour faire des virages moins brusques...
  if (delta>100+abs(consigne/10) && abs(consigne)<900) delta=100+abs(consigne/10); 
  if (delta<-100-abs(consigne/10) && abs(consigne)<900) delta=-100-abs(consigne/10);   
   
  if (delta >0) {    // delta >0 => Il faut mettre plus de puissance sur le moteur Droite
          Droite = Vmax;
          Gauche = Vmax - abs(delta);     // Moins de puissance sur le moteur Gauche
          }    
      else {        // delta <0 => Il faut mettre plus de puissance sur le moteur Gauche
          Gauche = Vmax;
          Droite = Vmax - abs(delta);   // Moins de puissance sur le moteur Droite
           }
           
   if (Droite>Vmax) Droite=Vmax;
   if (Droite<-Vmax) Droite=-Vmax;
   if (Gauche>Vmax) Gauche=Vmax;   
   if (Gauche<-Vmax) Gauche=-Vmax;
       
   Moteur(Droite,Gauche);        
} // correction()