Piloter le moteur à l'aide de son codeur

/* Déclaration des constantes et variables
   à placer au début du programme
   Pour plus de lisibilité placer les fonctions
   dans un nouvel onglet fonction.*/

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; 

//TB6612 pins
const int right_R1=7;    
const int right_R2=6;
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 

unsigned long t0;
long i, iBP, recu;
float dt=0.005;

bool Had,Had0,Hag,Hag0;
int Cd,Cg,Cd0,Cg0;
int consigne,vmax=255;

/* 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);
  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 cablage robot
  if (droite> vmax) droite= vmax;
  if (droite<-vmax) droite=-vmax;
  if (gauche> vmax) gauche= vmax;
  if (gauche<-vmax) gauche=-vmax;
  
//  if(angle_gyro>50 || angle_gyro<-50) {
//    droite=0;gauche=0;
//    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 raz() {    // RAZ à chaque appui du Bouton Poussoir 13
  iBP=i;recu=i;
  consigne=0;
  moteur(0,0);
  delay(500);
  Cd=0;Cd0=0;Cg=0;Cg0=0;  
  digitalWrite(buz,HIGH);
}