// avoidSim
// - need to fix the motors
// - need to replace delay() with event timing

bool DEBUG = 1; // 1 = Serial Monitor debug prints; 0 = NO debug prints

#include <Servo.h> //Servo motor library - for lookLeft and lookRight
Servo lookServo; // Create a Servo "look" object to control
Servo steerServo; // Create a Servo "steer" object to control
#define lookServoPin 11
#define steerServoPin 10
boolean goesForward = false;
boolean movefwd;
#define HOME 90 // servo home

#include <NewPing.h>  //Ultrasonic sensor function library. You must install this library
#define trig_pin A1  //analog input 1
#define echo_pin A2  //analog input 2
#define maximum_distance 200
int distance = 0;
NewPing sonar(trig_pin, echo_pin, maximum_distance);  // Create a NewPing object to control

// DC Motor:    = L298N requires:
// LeftForward  = IN1 HIGH, IN2 GND
// LeftReverse  = IN1 GND , IN2 HIGH
// RightForward = IN3 HIGH, IN4 GND
// RightReverse = IN3 GND, IN4 HIGH

// RIGHT DC motor with L298N H-bridge control pins
// const int LeftMotorEnable = 3; // ENA or PWM of RIGHT motor
const int RightMotorForward = 4;
const int RightMotorBackward = 5;

// LEFT DC motor with L298N H-bridge control pins
// const int LeftMotorEnable = 8; // ENA or PWM of LEFT motor
const int LeftMotorForward = 7;
const int LeftMotorBackward = 6;

// Head lights, tail lights, reverse lights, directional lights
#include <Adafruit_NeoPixel.h>
#define pixPin 2
int pixNum = 32;
Adafruit_NeoPixel pixels (pixNum, pixPin, NEO_GRB + NEO_KHZ800);

int16_t x = 0, y = 0; // counters

void setup() {
  Serial.begin(115200);
  randomSeed(analogRead(0)); // random seed to create random "distance" measurements

  pixels.begin(); // lights

  // pinMode(LeftMotorEnable, OUTPUT); // pin 8 -
  pinMode(LeftMotorForward, OUTPUT); // pin 7
  pinMode(LeftMotorBackward, OUTPUT); // pin 6
  pinMode(RightMotorBackward, OUTPUT); // pin 5
  pinMode(RightMotorForward, OUTPUT); // pin 4
  // pinMode(RightMotorEnable, OUTPUT); // pin 3 -

  lookServo.attach(lookServoPin);  // servo pin
  steerServo.attach(steerServoPin);  // steer pin
  lookServo.write(HOME); // go to HOME
  steerServo.write(HOME); // go to HOME
}

void loop() {
  int distanceRight = 0;
  int distanceLeft = 0;

  distance = readPing();

  // delay(50);

  if (distance <= 20) {

    if (DEBUG)
      Serial.print("\nObject:");
    if (DEBUG)
      if (distance < 10)
        Serial.print(" ");
    if (DEBUG)
      Serial.print(distance);
    if (DEBUG)
      Serial.print("cm ");

    moveStop();
    moveBackward();
    moveStop();

    distanceLeft = lookLeft();
    if (DEBUG) {
      distanceLeft  = random(30);
      Serial.print("Object:");
      if (distanceLeft < 10) Serial.print(" ");
      Serial.print(distanceLeft);
      Serial.print("cm");
    }

    distanceRight = lookRight();
    if (DEBUG) {
      distanceRight = random(30);
      Serial.print("Object:");
      if (distanceRight < 10) Serial.print(" ");
      Serial.print(distanceRight);
      Serial.print("cm");
    }

    if (distanceRight >= distanceLeft) {
      turnRight();
      moveStop();
    } else {
      turnLeft();
      moveStop();
    }
  } else {
    moveForward();
  }
}

int lookRight() {
  if (DEBUG)
    Serial.print(" lookRIGHT ");
  lookServo.write(180); // look right
  delay(500); // wait for device to catch up
  distance = readPing(); // distance is global
  lookServo.write(HOME); // home
  return distance;
}

int lookLeft() {
  if (DEBUG)
    Serial.print(" lookLEFT ");
  lookServo.write(0); // look left
  delay(500); // wait for device to catch up
  distance = readPing();
  lookServo.write(HOME); // homne
  return distance;
}

int readPing() {
  int cm = sonar.ping_cm();
  if (cm == 0) {
    cm = 250; // ?? will this not make the robot roll forward?
  }
  return cm;
}

void moveStop() {
  movefwd = 0;
  if (DEBUG)
    Serial.print("moveSTOP");
  steerServo.write(HOME); // steer left
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(LeftMotorBackward, LOW);
  pixelBrakes();
  delay(500);
}

void moveForward() {
  if (DEBUG) {
    if (!movefwd) {
      movefwd = 1;
      Serial.print("\nmoveFORWARD ");
      pixelForward();
    }

    // these counters are for a cleaner serial monitor
    x++;
    if (x > 10) {
      Serial.print("> ");
      x = 0;
      y++;
      if (y == 45) {
        y = 0;
        movefwd = 0;
      }
    }
  }

  steerServo.write(HOME); // steer forward
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
}

void moveBackward() {
  if (DEBUG)
    Serial.print(" moveBACK  ");
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorForward, LOW);
  pixelReverse();
  delay(500);
}

void turnRight() {
  if (DEBUG)
    Serial.print(" turnRIGHT ");
  steerServo.write(135); // steer right
  // delay(500);
  pixelRight();
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorForward, LOW);
  delay(500);
}

void turnLeft() {
  if (DEBUG)
    Serial.print(" turnLEFT  ");
  steerServo.write(45); // steer left
  // delay(500);
  pixelLeft();
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  delay(500);
}

void pixelForward() {
  for (int i = 0; i < 8; i++) {
    pixels.setPixelColor (i +  8, pixels.Color(255, 255, 255));
    pixels.setPixelColor (i + 16, pixels.Color(255, 255, 255));
  }
  for (int i = 0; i < 8; i++) {
    pixels.setPixelColor (i + 00, pixels.Color(191, 0, 0));
    pixels.setPixelColor (i + 24, pixels.Color(191, 0, 0));
  }
  pixels.show();
  pixels.show();
}

void pixelBrakes() {
  pixels.clear();
  pixels.show();
  for (int i = 0; i < 8; i++) {
    pixels.setPixelColor (i + 00, pixels.Color(255, 0, 0));
    pixels.setPixelColor (i + 24, pixels.Color(255, 0, 0));
  }
  pixels.show();
  delay(500);
  pixels.clear();
  pixels.show();
}

void pixelReverse() {
  for (int i = 0; i < 8; i++) {
    pixels.setPixelColor (i +  0, pixels.Color(255, 255, 255));
    pixels.setPixelColor (i + 24, pixels.Color(255, 255, 255));
  }
  pixels.show();
  delay(500);
  pixels.clear();
  pixels.show();
}

void pixelRight() {
  for (int j = 0; j < 3; j++) { // blink three times
    pixels.clear();
    pixels.show();
    delay(200);
    for (int i = 0; i < 8; i++) {
      pixels.setPixelColor (i +  0, pixels.Color(255, 191, 000));
      pixels.setPixelColor (i +  8, pixels.Color(255, 191, 000));
    }
    pixels.show();
    delay(200);
  }
  delay(200);
  pixels.clear();
  pixels.show();
}

void pixelLeft() {
  for (int j = 0; j < 3; j++) { // blink three times
    pixels.clear();
    pixels.show();
    delay(200);
    for (int i = 0; i < 8; i++) {
      pixels.setPixelColor (i + 16, pixels.Color(255, 191, 000));
      pixels.setPixelColor (i + 24, pixels.Color(255, 191, 000));
    }
    pixels.show();
    delay(200);
  }
  delay(200);
  pixels.clear();
  pixels.show();
}
A4988
A4988
LOOK
STEER