To test Robocar build at CoderDojo Saint Paul

Obstacle_Avoidance.ino

// This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International (CC BY-SA 4.0)
// https://creativecommons.org/licenses/by-sa/4.0/

// This code was put together to test a build of https://smile.amazon.com/dp/B01CXVA6IO/
// as part of CoderDojo Saint Paul Event 00000010 https://www.coderdojosaintpaul.org/

// A complete example build video was posted by Dallas Personal Robotics Group: https://www.youtube.com/watch?v=d9TXThtSUNM

// Includes control for:
// 2 TT Motors w/PWM connected to L293D Motor Driver, with EN1 and EN2 jumpers connected
// 1 HC-SR04 Ultrasonic Sensor mounted on SG90 Servo
//
// setup() tests motors and servo.
// loop() runs obstacle avoidance.
//
// Pin Notes: https://www.arduino.cc/en/reference/board
//   PWM available on pins 3, 5, 6, 9, 10, 11.
//   When using Servo library, PWM is unavailable on 9, 10.
//   LED_BUILTIN=13.
//   For Serial communications, digital pins 0 (RX) and 1 (TX) are unavailable for other use.
//   Pins 2 and 3 can be configured to trigger an interrupt.
//   10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK) pins support SPI communication.
//   Analog pins 6 and 7 (present on the Nano) cannot be used as digital pins.
//   Analog pins 4 (SDA) and 5 (SCL) support I2C (TWI) communication using the Wire library.


#include <Arduino.h>
// https://playground.arduino.cc/Code/NewPing/
// Install via Arduino IDE Library Manager.
#include <NewPing.h>
// https://www.arduino.cc/en/Reference/Servo
#include <Servo.h>

// From https://forum.arduino.cc/index.php?topic=155268.0
#define DEBUG false
#define Serial if(DEBUG)Serial

#define SHOULD_TEST_PING   false
#define SHOULD_TEST_SERVO  true
#define SHOULD_TEST_MOTORS true

// Ultrasonic Sensor Info: https://howtomechatronics.com/tutorials/arduino/ultrasonic-sensor-hc-sr04/
// HC-SR04 Ultrasonic Sensor Wiring:
// Red:    Vcc  <-> V
// Yellow: Trig <-> A0 @ URF01 // TRIGGER_PIN
// Orange: Echo <-> A1 @ URF01 // ECHO_PIN
// Brown:  Gnd  <-> G
#define TRIGGER_PIN  A0
#define ECHO_PIN     A1
#define MAX_DISTANCE 50 // Maybe capable out to 500 cm, but the greater the max distance, the greater the wait time for each reading.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
#define OBJECT_TOO_CLOSE 30 // cm

// Servo Wiring:
// Orange: D9 // SERVO_PIN
// Red:    5V
// Brown:  GND
#define SERVO_PIN     9
#define SERVO_FORWARD 96
#define SERVO_RIGHT   5
#define SERVO_LEFT    180
Servo servo;

// L293D Motor Driver Wiring:
// White:  VCC <-> Not Connected!  L293D logic electronics fed by VIN (unexpectedly).
// Black:  GND <-> G
// Orange: IN1 <-> A5 S // R_FORWARD_PIN
// Yellow: IN2 <-> A4 S // R_REVERSE_PIN
// Green:  IN3 <-> D7 S // L_REVERSE_PIN
// Blue:   IN4 <-> D8 S // L_FORWARD_PIN
// Purple: EN1 <-> D6 (PWM) // L_SPEED_PIN
// Gray:   EN2 <-> D5 (PWM) // R_SPEED_PIN

#define R_FORWARD_PIN A5
#define R_REVERSE_PIN A4
#define R_SPEED_PIN   5
#define L_FORWARD_PIN 8
#define L_REVERSE_PIN 7
#define L_SPEED_PIN   6

// values: 0-255 // higher for faster
#define MOTOR_POWER_LEVEL_OFF LOW
#define MOTOR_POWER_LEVEL_ON HIGH
#define MOTOR_POWER_LEVEL_FULL 255
#define MOTOR_POWER_LEVEL_FORWARD MOTOR_POWER_LEVEL_FULL/2
#define MOTOR_POWER_LEVEL_REVERSE MOTOR_POWER_LEVEL_FULL/2
#define MOTOR_POWER_LEVEL_TURN MOTOR_POWER_LEVEL_FULL/2

#define TURN_90_DURATION 1000

void flash(int ledPin, int flashes)
{
  for (int i = 0; i < flashes; i++)
  {
    digitalWrite(ledPin, HIGH);
    delay(100);
    digitalWrite(ledPin, LOW);
    delay(200);
  }
}

void flash(int flashes)
{
  flash(LED_BUILTIN, flashes);
}

void stop_all_motors()
{
  analogWrite(R_SPEED_PIN, MOTOR_POWER_LEVEL_OFF);
  analogWrite(L_SPEED_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(R_FORWARD_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(R_REVERSE_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(L_FORWARD_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(L_REVERSE_PIN, MOTOR_POWER_LEVEL_OFF);
}

void go_forward(int power_level)
{
  digitalWrite(R_REVERSE_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(L_REVERSE_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(R_FORWARD_PIN, MOTOR_POWER_LEVEL_ON);
  digitalWrite(L_FORWARD_PIN, MOTOR_POWER_LEVEL_ON);
  analogWrite(R_SPEED_PIN, power_level);
  analogWrite(L_SPEED_PIN, power_level);
}

void go_forward(int power_level, int duration)
{
  go_forward(power_level);
  delay(duration);
}

void go_reverse(int power_level)
{
  digitalWrite(R_FORWARD_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(L_FORWARD_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(R_REVERSE_PIN, MOTOR_POWER_LEVEL_ON);
  digitalWrite(L_REVERSE_PIN, MOTOR_POWER_LEVEL_ON);
  analogWrite(R_SPEED_PIN, power_level);
  analogWrite(L_SPEED_PIN, power_level);
}

void go_reverse(int power_level, int duration)
{
  go_reverse(power_level);
  delay(duration);
}

void pivot_left(int power_level)
{
  digitalWrite(L_FORWARD_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(R_REVERSE_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(R_FORWARD_PIN, MOTOR_POWER_LEVEL_ON);
  digitalWrite(L_REVERSE_PIN, MOTOR_POWER_LEVEL_ON);
  analogWrite(R_SPEED_PIN, power_level);
  analogWrite(L_SPEED_PIN, power_level);
}

void pivot_left(int power_level, int duration)
{
  pivot_left(power_level);
  delay(duration);
}

void pivot_right(int power_level)
{
  digitalWrite(R_FORWARD_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(L_REVERSE_PIN, MOTOR_POWER_LEVEL_OFF);
  digitalWrite(L_FORWARD_PIN, MOTOR_POWER_LEVEL_ON);
  digitalWrite(R_REVERSE_PIN, MOTOR_POWER_LEVEL_ON);
  analogWrite(R_SPEED_PIN, power_level);
  analogWrite(L_SPEED_PIN, power_level);
}

void pivot_right(int power_level, int duration)
{
  pivot_right(power_level);
  delay(duration);
}

void turn_around()
{
  pivot_left(MOTOR_POWER_LEVEL_TURN, TURN_90_DURATION * 2);
}

void testPing()
{
  Serial.println("BEGIN testPing()");
  for (int i = 0; i < 100; i++)
  {
    delay(100);
    Serial.print("Ping: ");
    Serial.print(sonar.ping_cm());
    Serial.println("cm");
  }
  Serial.println("END testPing()");
}

void testServo()
{
  Serial.println("BEGIN testServo()");
  int servo_test_delay = 1500;

  servo.write(SERVO_FORWARD);
  delay(servo_test_delay);
  servo.write((SERVO_FORWARD + SERVO_RIGHT) / 2);
  delay(servo_test_delay);
  servo.write(SERVO_RIGHT);
  delay(servo_test_delay);
  servo.write(SERVO_LEFT);
  delay(servo_test_delay);
  servo.write(SERVO_FORWARD);

  Serial.println("END testServo()");
}

void testMotors()
{
  Serial.println("BEGIN testMotors()");
  stop_all_motors();
  int motors_test_delay = 1500;

  go_forward(MOTOR_POWER_LEVEL_FULL / 2, motors_test_delay);
  go_forward(MOTOR_POWER_LEVEL_FULL, motors_test_delay);

  stop_all_motors();
  delay(motors_test_delay);

  go_reverse(MOTOR_POWER_LEVEL_FULL / 2, motors_test_delay);
  go_reverse(MOTOR_POWER_LEVEL_FULL, motors_test_delay);

  stop_all_motors();
  delay(motors_test_delay);

  pivot_left(MOTOR_POWER_LEVEL_FULL / 2, TURN_90_DURATION);
  pivot_left(MOTOR_POWER_LEVEL_FULL, TURN_90_DURATION);

  stop_all_motors();
  delay(motors_test_delay);

  pivot_right(MOTOR_POWER_LEVEL_FULL / 2, TURN_90_DURATION);
  pivot_right(MOTOR_POWER_LEVEL_FULL, TURN_90_DURATION);

  stop_all_motors();
  delay(motors_test_delay);

  turn_around();

  stop_all_motors();
  Serial.println("END testMotors()");
}

int turn_servo_and_ping(int servo_position)
{
  servo.write(servo_position);
  delay(500);
  int cm_to_object = sonar.ping_cm();
  return cm_to_object > 0 ? cm_to_object : MAX_DISTANCE;
}

void avoid_obstacle()
{
  go_reverse(MOTOR_POWER_LEVEL_REVERSE, 250);
  stop_all_motors();
  int cm_to_object_left = turn_servo_and_ping(SERVO_LEFT);
  int cm_to_object_right = turn_servo_and_ping(SERVO_RIGHT);
  servo.write(SERVO_FORWARD);
  if (cm_to_object_left >= OBJECT_TOO_CLOSE && cm_to_object_left >= cm_to_object_right) pivot_left(MOTOR_POWER_LEVEL_TURN, TURN_90_DURATION);
  else if (cm_to_object_right >= OBJECT_TOO_CLOSE) pivot_right(MOTOR_POWER_LEVEL_TURN, TURN_90_DURATION);
  else turn_around();
}

void setup()
{
  Serial.begin(115200);
  Serial.println("BEGIN setup()");
  pinMode(LED_BUILTIN, OUTPUT);
  flash(5);
  servo.attach(SERVO_PIN);
  
  pinMode(R_SPEED_PIN, OUTPUT);
  pinMode(L_SPEED_PIN, OUTPUT);
  pinMode(R_FORWARD_PIN, OUTPUT);
  pinMode(R_REVERSE_PIN, OUTPUT);
  pinMode(L_FORWARD_PIN, OUTPUT);
  pinMode(L_REVERSE_PIN, OUTPUT);
  stop_all_motors();

  if (SHOULD_TEST_PING)
  {
    testPing();
    delay(1500);
  }
  if (SHOULD_TEST_SERVO)
  {
    testServo();
    delay(1500);
  }
  if (SHOULD_TEST_MOTORS)
  {
    testMotors();
    delay(1500);
  }

  flash(2);
  Serial.println("END setup()");
}

void loop()
{
  int cm_to_object = sonar.ping_cm();
  Serial.print("Obstacle at Distance: ");
  Serial.println(cm_to_object);
  if (cm_to_object > 0 && cm_to_object < OBJECT_TOO_CLOSE)
  {
    stop_all_motors();
    avoid_obstacle();
  }
  else
  {
    go_forward(MOTOR_POWER_LEVEL_FORWARD);
    delay(100);
  }
}