Autonomous navigation

Now that we are sure that our robot is correctly wired, it's time to write an application for moving it around while avoiding obstacles in front of the robot. Here is the complete code for this part:

// Motor pins
int speed_motor1 = 6;  
int speed_motor2 = 5;
int direction_motor1 = 7;
int direction_motor2 = 4;

// Sensor pin
int distance_sensor = A0;

void setup(void)
{  
  // Start Serial
  Serial.begin(115200);
  
  // Go forward by default
  forward();
}

void loop() {  
  
  // Measure distance
  int distance = measure_distance(distance_sensor);
  
  // If obstacle in front
  if (distance < 10) {
  
    // Go backward
    backward();
    delay(2000);
    
    // Turn around
    left();
    delay(500);
    
    // Go forward again
    forward();
  }
  
}

// Forward
int forward() {
  
  send_motor_command(speed_motor1,direction_motor1,200,1);
  send_motor_command(speed_motor2,direction_motor2,200,1);
  return 1;
}

// Backward
int backward() {
  
  send_motor_command...