Project Details

// Constants
const int enableBridge1 = 11;
const int MotorForward1 = 12;
const int MotorReverse1 = 13;

const int enableBridge2 = 5;
const int MotorForward2 = 6;
const int MotorReverse2 = 7;

const int TrigPin = 2; 
const int EchoPin = 3; 
float distance;

#include <servo.h>

Servo myservo;

int angle = 0;
 
void setup(){
  
 Serial.begin(9600); 
 pinMode(TrigPin, OUTPUT); 
 pinMode(EchoPin, INPUT); 
 Serial.println("Ultrasonic sensor:"); 
  
 pinMode(MotorForward1,OUTPUT);
 pinMode(MotorReverse1,OUTPUT);
 pinMode(enableBridge1,OUTPUT);
  
 pinMode(MotorForward2,OUTPUT);
 pinMode(MotorReverse2,OUTPUT);
 pinMode(enableBridge2,OUTPUT);
  
 myservo.attach(9);
  
 Serial.begin(2000);
}
 
void loop(){ 
  
 digitalWrite(enableBridge1,HIGH); // Active pont en H
 digitalWrite(enableBridge2,HIGH);
 
 
  
  
 digitalWrite(TrigPin, LOW); 
 delayMicroseconds(2); 
 digitalWrite(TrigPin, HIGH); 
 delayMicroseconds(10);
 digitalWrite(TrigPin, LOW); 
     
 distance = pulseIn(EchoPin, HIGH) / 58.00;
 Serial.print(distance); 
 Serial.print("cm"); 
 Serial.println(); 
  
 for (angle = 0; angle <= 180; angle += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(angle);              // tell servo to go to position in variable 'pos'
    delay(15);
   
       if (distance<=50) {
      // Tourne dans le sens indirect pendant 3 secondes
       digitalWrite(MotorForward1,HIGH);
      digitalWrite(MotorReverse1,0);
      digitalWrite(MotorForward2,0);
       digitalWrite(MotorReverse2,HIGH);
   }
    else  {
      digitalWrite(MotorForward1,HIGH);
       digitalWrite(MotorReverse1,0);
       digitalWrite(MotorForward2,HIGH);
       digitalWrite(MotorReverse2,0);
   }
 }
 for (angle = 180; angle >= 0; angle -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(angle);              // tell servo to go to position in variable 'pos'
    delay(15);
   
    if (distance<=50) {
      // Tourne dans le sens indirect pendant 3 secondes
       digitalWrite(MotorForward1,0);
       digitalWrite(MotorReverse1,HIGH);
       digitalWrite(MotorForward2,HIGH);
       digitalWrite(MotorReverse2,0);
    }
    else  {
      digitalWrite(MotorForward1,HIGH);
       digitalWrite(MotorReverse1,0);
       digitalWrite(MotorForward2,HIGH);
       digitalWrite(MotorReverse2,0); 
    }
 }
}</servo.h>

First design on fusion360 (25cm x 12cm)

(inside and support) 

Upgrade :

First prototypes of the circuit

Final Circuit V1

// Constants
const int enableBridge1 = 11;
const int MotorForward1 = 12;
const int MotorReverse1 = 13;

const int enableBridge2 = 5;
const int MotorForward2 = 6;
const int MotorReverse2 = 7;

const int TrigPin = 2;
const int EchoPin = 3;
float distance;

#include <Servo.h>

Servo myservo;

int angle = 0;

void setup(){

  Serial.begin(9600);
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
  Serial.println("Ultrasonic sensor:");

  pinMode(MotorForward1,OUTPUT);
  pinMode(MotorReverse1,OUTPUT);
  pinMode(enableBridge1,OUTPUT);

  pinMode(MotorForward2,OUTPUT);
  pinMode(MotorReverse2,OUTPUT);
  pinMode(enableBridge2,OUTPUT);

  myservo.attach(9);

  Serial.begin(2000);
}

void loop(){

  digitalWrite(enableBridge1,HIGH); // Active pont en H
  digitalWrite(enableBridge2,HIGH);

  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);

  distance = pulseIn(EchoPin, HIGH) / 58.00;
  Serial.print(distance);
  Serial.print("cm");
  Serial.println();

  if (distance<=50) {
    // Si un obstacle est détecté
    // Vérifie s'il y a un obstacle à droite
    myservo.write(90);
    delay(10000);
    digitalWrite(TrigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(TrigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(TrigPin, LOW);
    if (pulseIn(EchoPin, HIGH) / 58.00 > 50) {
      // S'il n'y a pas d'obstacle à droite, tourne à droite
      digitalWrite(MotorForward1, 0);
      digitalWrite(MotorReverse1, HIGH);
      digitalWrite(MotorForward2, HIGH);
      digitalWrite(MotorReverse2, 0);
      delay(3000);
    } else {
      // Sinon, vérifie s'il y a un obstacle à gauche
      myservo.write(180);
      delay(10000);
      digitalWrite(TrigPin, LOW);
      delayMicroseconds(2);
      digitalWrite(TrigPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(TrigPin, LOW);
      if (pulseIn(EchoPin, HIGH) / 58.00 > 50) {
        // S'il n'y a pas d'obstacle à gauche, tourne à gauche
        digitalWrite(MotorForward1, HIGH);
        digitalWrite(MotorReverse1, 0);
        digitalWrite(MotorForward2, 0);
        digitalWrite(MotorReverse2, HIGH);
        delay(3000);
      } else {
        // Sinon, fait demi-tour
        digitalWrite(MotorForward1, 0);
        digitalWrite(MotorReverse1, HIGH);
        digitalWrite(MotorForward2, 0);
        digitalWrite(MotorReverse2, HIGH);
        delay(3000);
      }
    }
  } else {
    // S'il n'y a pas d'obstacle, avance
    digitalWrite(MotorForward1, HIGH);
    digitalWrite(MotorReverse1, 0);
    digitalWrite(MotorForward2, HIGH);
    digitalWrite(MotorReverse2, 0);
  }

// Parameters
const int input_voltage = 9; // V
const int nominal_voltage = 5; // V
const int MAX_SPEED = (nominal_voltage * 255) / input_voltage;
const int directionA = 12;

const int brakeA = 9;
const int brakeB = 8;
const int speedA = 3;
const int speedB = 11;

const int TrigPin = 2;
const int EchoPin = 4;
float distance;

void setup() {
  // Init Serial USB
  Serial.begin(9600);
  Serial.println(F("Initialize System"));
  // Init Motor Shield
  pinMode(directionA, OUTPUT); // Initiate Motor Channel A pin
  pinMode(brakeA, OUTPUT);     // Initiate Brake Channel A pin
  
  pinMode(brakeB, OUTPUT);     // Initiate Brake Channel B pin
  
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
  pinMode(13, OUTPUT);
}

void loop() {
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);
  
  // Measure the distance
  unsigned long duration = pulseIn(EchoPin, HIGH);
  
  // Convert the duration to distance
  distance = (duration * 0.034) / 2;
  
  Serial.print(distance);
  Serial.print(" cm");
  Serial.println();
  
  if (distance <= 25) {
    digitalWrite(13, HIGH);
    dcBackward();
  } 
  else {
    digitalWrite(13, LOW);
    dcForward();
  }
}

void dcForward() {
  // Set forward motion for A and B
  digitalWrite(directionA, HIGH); // Establish forward direction of Channel A
  digitalWrite(brakeA, LOW);       // Disengage the Brake for Channel A
  analogWrite(speedA, MAX_SPEED);
  
  digitalWrite(brakeB, LOW);       // Disengage the Brake for Channel B
  analogWrite(speedB, MAX_SPEED);
}

void dcBackward() {
  // Set backward motion for A and B
  digitalWrite(directionA, LOW); // Establish backward direction of Channel A
  digitalWrite(brakeA, LOW);      // Disengage the Brake for Channel A
  analogWrite(speedA, MAX_SPEED);
  
  digitalWrite(brakeB, LOW);      // Disengage the Brake for Channel B
  analogWrite(speedB, MAX_SPEED);
}