ObstacleAvoidanceRobot.ino

#include "ArduinoMotorShieldR3.h"

// https://store.arduino.cc/arduino-motor-shield-rev3
// https://randomnerdtutorials.com/complete-guide-for-ultrasonic-sensor-hc-sr04/
// https://github.com/gallingern/arduino-motor-shield-r3

ArduinoMotorShieldR3 md;
int trigPin = 5;    // Trigger
int echoPin = 4;    // Echo
int ledPin = 7;
long duration, cm;
int failedTurns;
bool lastTurnLeft;
int failedTurnsOnTurnOrder;
bool motorStalled;

void setup()
{
  motorStalled = false;
  failedTurnsOnTurnOrder = 0;
  failedTurns = 0;
  Serial.begin(115200);
  Serial.println("Arduino Motor Robot Obstacle Avoidance Robot");
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledPin, OUTPUT);
  md.init();
  cm = pingDistanceCm();
}

long pingDistanceCm()
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
  return (duration/2) / 29.1;
}

void loop()
{
  if (cm >= 38)
  {
    failedTurns = 0;
    failedTurnsOnTurnOrder = 0;
    md.setM1Speed(300);
    md.setM2Speed(300);
    delay(12);
  }
  else
  {
    failedTurns++; 
    failedTurnsOnTurnOrder++;
  }
  
  cm = pingDistanceCm();

  if (cm < 38 || motorStalled)
  {
//    md.setM1Brake();
//    md.setM2Brake();
    md.setM1Speed(0);
    md.setM2Speed(0);
    delay(200);
    md.setM1Speed(-200);
    md.setM2Speed(-200);
    delay(100);

    if (failedTurns > 2)
    {
      delay(150);
    }
    
    if (failedTurns > 4)
    {
      delay(175);
    }
    
    if (motorStalled)
    {
      delay(200);
    }

//    if (motorStalled)
//    {
//      return;  
//    }
    
    if (failedTurnsOnTurnOrder > 3)
    {
      // Three consecutive unsuccessful turns
      failedTurnsOnTurnOrder = 0;
      lastTurnLeft = !lastTurnLeft;
    }

    if (lastTurnLeft)
    {
      // Left
      md.setM1Speed(200);
      md.setM2Speed(-200);
    }
    else
    {
      // Right
      md.setM1Speed(-200);
      md.setM2Speed(200);
    }

    motorStalled = md.getM1CurrentMilliamps() > 500 || md.getM2CurrentMilliamps() > 500;
    digitalWrite(ledPin, HIGH);
    delay(200);
    digitalWrite(ledPin, LOW); 
  }
}

ObstacleAvoidanceRobotReverseWheels.ino

#include "ArduinoMotorShieldR3.h"

// https://store.arduino.cc/arduino-motor-shield-rev3
// https://randomnerdtutorials.com/complete-guide-for-ultrasonic-sensor-hc-sr04/
// https://github.com/gallingern/arduino-motor-shield-r3

ArduinoMotorShieldR3 md;
int trigPin = 5;    // Trigger
int echoPin = 4;    // Echo
int ledPin = 7;
long duration, cm;
int failedTurns;
bool lastTurnLeft;
int failedTurnsOnTurnOrder;
bool motorStalled;

void setup()
{
  motorStalled = false;
  failedTurnsOnTurnOrder = 0;
  failedTurns = 0;
  Serial.begin(115200);
  Serial.println("Arduino Motor Robot Obstacle Avoidance Robot");
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledPin, OUTPUT);
  md.init();
  cm = pingDistanceCm();
}

long pingDistanceCm()
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
  return (duration/2) / 29.1;
}

void loop()
{
  if (cm >= 38)
  {
    failedTurns = 0;
    failedTurnsOnTurnOrder = 0;
    md.setM1Speed(-300);
    md.setM2Speed(-300);
    delay(12);
  }
  else
  {
    failedTurns++; 
    failedTurnsOnTurnOrder++;
  }
  
  cm = pingDistanceCm();

  if (cm < 38 || motorStalled)
  {
//    md.setM1Brake();
//    md.setM2Brake();
    md.setM1Speed(0);
    md.setM2Speed(0);
    delay(200);
    md.setM1Speed(200);
    md.setM2Speed(200);
    delay(100);

    if (failedTurns > 2)
    {
      delay(150);
    }
    
    if (failedTurns > 4)
    {
      delay(175);
    }
    
    if (motorStalled)
    {
      delay(200);
    }

//    if (motorStalled)
//    {
//      return;  
//    }
    
    if (failedTurnsOnTurnOrder > 3)
    {
      // Three consecutive unsuccessful turns
      failedTurnsOnTurnOrder = 0;
      lastTurnLeft = !lastTurnLeft;
    }

    if (lastTurnLeft)
    {
      // Left
      md.setM1Speed(-200);
      md.setM2Speed(200);
    }
    else
    {
      // Right
      md.setM1Speed(200);
      md.setM2Speed(-200);
    }

    motorStalled = md.getM1CurrentMilliamps() > 500 || md.getM2CurrentMilliamps() > 500;
    digitalWrite(ledPin, HIGH);
    delay(200);
    digitalWrite(ledPin, LOW); 
  }
}