MPU6050 fonction moteur

// Variables Globales à placer au début du programme
const int right_R1=7; // Selon montage robot   
const int right_R2=6; // droite et gauche peuvent être inversés   
const int PWM_R=9;
const int left_L1=8;
const int left_L2=12;
const int PWM_L=10;
const int BP=13;      // Bouton Poussoir shield V3
const int buz = 11;    // set the buzzer pin 
const int Hall_G_A1= 5; // Capteur effet Hall gauche A1 D5
const int Hall_G_A2 = 2; // B pour déterminer de sens de rotation
const int Hall_D_A1 = 4; // Capteur effet Hall droite A1  D4
const int Hall_D_A2 = 3; 

float Kp=??,Kd=??;      //Correction proportionnelle et dérivée
float consigne=-1;       // Consigne 0° en théorie   
int P,D,PWM ;             // action Proportionnelle, Dérivée

bool balancing=false;
unsigned long t0;
long i, iBP, recu;

/* Pour plus de lisibilité placer les fonctions
   dans un nouvel onglet fonction.*/
void iniPin(){
  // Bouton Poussoir : commute balancing
  pinMode(13,INPUT_PULLUP);
  //set the control motor’s pin to OUTPUT
  pinMode(right_R1,OUTPUT);       
  pinMode(right_R2,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(PWM_L,OUTPUT);
  // pinMode(buz,OUTPUT);  // commenté pour moins de bruit...
  pinMode(Hall_G_A1,INPUT);  
  pinMode(Hall_G_A2,INPUT);
  pinMode(Hall_D_A1,INPUT);  
  pinMode(Hall_D_A2,INPUT);

  //Initial state value
  digitalWrite(right_R1,1);
  digitalWrite(right_R2,0);
  digitalWrite(left_L1,0);
  digitalWrite(left_L2,1);
  analogWrite(PWM_R,0);
  analogWrite(PWM_L,0);
}

void moteur(int droite,int gauche) {
  droite=-droite;gauche=-gauche;   // Selon montage robot
  if (droite> 255) droite= 255;
  if (droite<-255) droite=-255;
  if (gauche> 255) gauche= 255;
  if (gauche<-255) gauche=-255;
  
  if(angle_gyro>50 || angle_gyro<-50) {
    droite=0;gauche=0;  // Arrêt moteur si θ>50 
    balancing=false;  }  

if(gauche>=0)  {
    digitalWrite(left_L1,LOW);
    digitalWrite(left_L2,HIGH);
    analogWrite(PWM_L,gauche);}
  else {
    digitalWrite(left_L1,HIGH);
    digitalWrite(left_L2,LOW);
    analogWrite(PWM_L,-gauche);}

  if(droite>=0){
    digitalWrite(right_R1,LOW);
    digitalWrite(right_R2,HIGH);
    analogWrite(PWM_R,droite);}
  else  {
    digitalWrite(right_R1,HIGH);
    digitalWrite(right_R2,LOW);
    analogWrite(PWM_R,-droite);}    
  }

void reception() {   // Labview envoi : mode;consigne;Kp;Kd;
    int mode=Serial.parseInt();
    digitalWrite(buz,HIGH);
    // Si on fonctionne en autonomie sans liaison série
    // Rx potentiel flottant => reçoit des caractères aléatoires
    // on ferme la liaison à la première erreur
    if (mode<1 || mode>4){Serial.end();recu=i+20;return;}
    recu=i;
    consigne=Serial.parseFloat();
    if (Serial.available()){
      double x=Serial.parseFloat();
      if (x>0) {Kp=x;Serial.print("0;0;0;0;Kp=");Serial.print(Kp);}}
    if (Serial.available()){
      double x=Serial.parseFloat();
      if (x>0) {Kd=x;Serial.print(";Kd=");Serial.print(Kd);}}
    Serial.println();
    while (Serial.available()) Serial.read();  
    if (mode==1) {balancing=!balancing;}
    if (mode==2) {balancing=true;}
    if (mode==3) {balancing=false;}
}

void commute() {    // Commute à chaque appui du Bouton Poussoir 13
  iBP=i;recu=i;
  balancing=!balancing;   // de balancing à !balancing
  digitalWrite(buz,HIGH);
}