Delay encoder robot labyrinthe

Bonjours,

Mon sujet de TPE consiste à faire sortir un robot d’un labyrinthe, ce dernier a trois capteurs ultrasons, un à gauche un à droite et un devant, il a aussi deux moteurs un du coté droit et un autre du coté gauche dans le sens inverse.

La logique que suit le robot pour sortir du labyrinthe est : Coller le mur de droite

Nous avions un probleme avec un des moteurs qui allait moins vite que l’autre mais nous l’avons reglé grace au PID, notre code en entier :

#include <MeMegaPi.h>

MeMegaPiDCMotor moteurG(PORT1B);
MeMegaPiDCMotor moteurD(PORT2B);

MeEncoderOnBoard Encoder_1(SLOT1);
MeEncoderOnBoard Encoder_2(SLOT2);


MeUltrasonicSensor USg(PORT_7);
MeUltrasonicSensor USa(PORT_8);
MeUltrasonicSensor USd(PORT_6);

void isr_process_encoder1(void)
{
  if(digitalRead(Encoder_1.getPortB()) == 0)
  {
    Encoder_1.pulsePosMinus();
  }
  else
  {
    Encoder_1.pulsePosPlus();;
  }
}

void isr_process_encoder2(void)
{
  if(digitalRead(Encoder_2.getPortB()) == 0)
  {
    Encoder_2.pulsePosMinus();
  }
  else
  {
    Encoder_2.pulsePosPlus();
  }
}

void setup()
{
  
  attachInterrupt(Encoder_1.getIntNum(), isr_process_encoder1, RISING);
  attachInterrupt(Encoder_2.getIntNum(), isr_process_encoder2, RISING);
  
  //Set PWM 8KHz
  TCCR1A = _BV(WGM10);
  TCCR1B = _BV(CS11) | _BV(WGM12);
  TCCR2A = _BV(WGM21) | _BV(WGM20);
  TCCR2B = _BV(CS21);

  Encoder_1.setPulse(8);
  Encoder_2.setPulse(8);
  Encoder_1.setRatio(47);
  Encoder_2.setRatio(47);
  Encoder_1.setPosPid(1.8,0,1.2);
  Encoder_2.setPosPid(1.8,0,1.2);
  Encoder_1.setSpeedPid(0.4,0.2,0);
  Encoder_2.setSpeedPid(0.4,0.2,0);
}

void loop()

{
  
int g = USg.distanceCm();
int a = USa.distanceCm();
int d = USd.distanceCm();
int i=0;

if (a<10)
{
  //tourner
  Encoder_1.runSpeed(0);
  Encoder_2.runSpeed(0);
  Encoder_1.move(-365,200);
  Encoder_2.move(-365,200);
  Encoder_1.loop();
  Encoder_2.loop();
}



else if (d<15)
{ 
  //Aller droit
  Encoder_1.runSpeed(-30);
  Encoder_2.runSpeed(30); 
  Encoder_1.loop();
  Encoder_2.loop();
}



  else if (g<15)
{
 //tourner
  Encoder_1.runSpeed(0);
  Encoder_2.runSpeed(0);
  Encoder_1.move(-365,200);
  Encoder_2.move(-365,200);
  Encoder_1.loop();
  Encoder_2.loop();
}


else
{ 
  //Aller droit
  Encoder_1.runSpeed(-30);
  Encoder_2.runSpeed(30); 
  Encoder_1.loop();
  Encoder_2.loop();
}
}

Sauf que nous avons un probleme quand le robot doit tourner, il détecte instantanément après que le capteur ultrason qui est au milieu du robot ait passé le mur qu’il n’y a plus de mur, donc il tourne trop tot, pour cela nous avons pensé à mettre un

delay(1000)
  //tourner
  Encoder_1.runSpeed(0);
  Encoder_2.runSpeed(0);
  Encoder_1.move(-365,200);
  Encoder_2.move(-365,200);
  Encoder_1.loop();
  Encoder_2.loop();

Sauf que quand on met un delay dans les Encoder, le robot ne tourne plus à 90 degrés, il fait pleins de petit tours à 30 degrés à l’infini.

Comment faire pour que le robot ne tourne pas instantanément ?

Merci d’avance pour vos réponses,

Viktor.