Terminator Hunter Killer Robot

// Alex Eastabrook
// Ci20 Arduino Robot Firmware.
// Version 0.1a

#include <Adafruit_NeoPixel.h>

// Macros
#define TRACE(msg) { Serial.print(__FUNCTION__); Serial.print("\t"); Serial.print(__LINE__); Serial.println(msg);  }
#define NUMOFELEMENTS(x)  (sizeof(x) / sizeof((x)[0]))

// Pin Definitions
#define MOTOR_L_FORWARD_PIN   2
#define MOTOR_L_BACKWARD_PIN  3
#define MOTOR_R_FORWARD_PIN   4
#define MOTOR_R_BACKWARD_PIN  5

#define ULTRASONIC_TRIG      7
#define ULTRASONIC_ECHO      6

#define PIR_SENSOR_LEFT       11
#define PIR_SENSOR_RIGHT      12

#define MISSILE_PIN         10

#define NEOPIXEL_PIN          9
#define NUMPIXELS             6

#define CMD_BUFFER_SIZE       40
// Globals.
short output_pins[] = {MOTOR_L_FORWARD_PIN, MOTOR_L_BACKWARD_PIN , MOTOR_R_FORWARD_PIN, MOTOR_R_BACKWARD_PIN, NEOPIXEL_PIN , MISSILE_PIN, ULTRASONIC_TRIG };
short input_pins[] = {ULTRASONIC_ECHO, PIR_SENSOR_LEFT , PIR_SENSOR_RIGHT };

Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, NEOPIXEL_PIN, NEO_GRB + NEO_KHZ800);

void setup() {
  // put your setup code here, to run once:

  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  TRACE("Serial Setup Complete : Trace now available");

  for (short i = 0; i < NUMOFELEMENTS(output_pins) ; i++)
  {
    pinMode(output_pins[i], OUTPUT);
  }
  for (short i = 0; i < NUMOFELEMENTS(input_pins) ; i++)
  {
    pinMode(input_pins[i], INPUT);
  }
  pixels.begin();
  pixels.show();


}

// Motor Control.
void stopMotors()
{
  digitalWrite(MOTOR_R_FORWARD_PIN, LOW);
  digitalWrite(MOTOR_R_BACKWARD_PIN, LOW);
  digitalWrite(MOTOR_L_FORWARD_PIN, LOW);
  digitalWrite(MOTOR_L_BACKWARD_PIN, LOW);
  TRACE("Motors Off");
  delay(50);
}

void stop(void) //Stop
{
  stopMotors();
}

void forward( )
{
  stopMotors();
  digitalWrite(MOTOR_R_FORWARD_PIN, HIGH);
  digitalWrite(MOTOR_L_FORWARD_PIN, HIGH);
  TRACE("Forward");

}

void reverse ( )
{
  stopMotors();
  digitalWrite(MOTOR_R_BACKWARD_PIN, HIGH);
  digitalWrite(MOTOR_L_BACKWARD_PIN, HIGH);
  TRACE("Backward");
}

void left ( )
{
  stopMotors();
  digitalWrite(MOTOR_R_BACKWARD_PIN, HIGH);
  digitalWrite(MOTOR_L_FORWARD_PIN, HIGH);
  TRACE("Left");
}

void right ( )
{
  stopMotors();
  digitalWrite(MOTOR_L_BACKWARD_PIN, HIGH);
  digitalWrite(MOTOR_R_FORWARD_PIN, HIGH);
  TRACE("Right");
}
void fire ( )
{

  digitalWrite(MISSILE_PIN, HIGH);
  delay(20);
  digitalWrite(MISSILE_PIN, LOW);
  TRACE("MISSILE FIRED");
}

void setRed()
{
  for (int i = 0; i < NUMPIXELS; i++) {
    pixels.setPixelColor(i, pixels.Color(0, 0, 0)); // Moderately bright green color.
    pixels.show(); // This sends the updated pixel color to the hardware.
  }
  delay(100);
  pixels.setPixelColor(0, 200, 0, 0);
  pixels.setPixelColor(2, 200, 0, 0);
  pixels.setPixelColor(3, 200, 0, 0);
  pixels.setPixelColor(5, 200, 0, 0);
  pixels.show();
  delay(100);
}
void setBlue()
{
  for (int i = 0; i < NUMPIXELS; i++) {
    pixels.setPixelColor(i, pixels.Color(0, 0, 0)); // Moderately bright green color.
    pixels.show(); // This sends the updated pixel color to the hardware.
  }
  delay(300);
  pixels.setPixelColor(0, 0,  200, 0);
  pixels.setPixelColor(2, 0,  200, 0);
  pixels.setPixelColor(3, 0,  200, 0);
  pixels.setPixelColor(5, 0,  200, 0);
  pixels.show();

}

unsigned long ultrasonic_polling_counter;
unsigned long pir_polling_counter;

void loop() {


  // System Running.
  if ((millis() - ultrasonic_polling_counter   ) > 1000)
  {
    ultrasonic_polling_counter = millis();
    int maximumRange = 200; // Maximum range needed
    int minimumRange = 0; // Minimum range needed
    long duration, distance; // Duration used to calculate distance

    /* The following trigPin/echoPin cycle is used to determine the
      distance of the nearest object by bouncing soundwaves off of it. */
    digitalWrite(ULTRASONIC_TRIG, LOW);
    delayMicroseconds(2);

    digitalWrite(ULTRASONIC_TRIG, HIGH);
    delayMicroseconds(10);

    digitalWrite(ULTRASONIC_TRIG, LOW);
    duration = pulseIn(ULTRASONIC_ECHO, HIGH);

    //Calculate the distance (in cm) based on the speed of sound.
    distance = duration / 58.2;

    if (distance >= maximumRange || distance <= minimumRange) {
      /* Send a negative number to computer and Turn LED ON
        to indicate "out of range" */
      Serial.println("Forward Distance : OutOfRange");
    }
    else {
      /* Send the distance to the computer using Serial protocol, and
        turn LED OFF to indicate successful reading. */

      Serial.print("Forward Distance :");
      Serial.println(distance);

    }

    if ((millis() - pir_polling_counter   ) > 5000)
    {
      pir_polling_counter = millis();


      if (digitalRead(PIR_SENSOR_RIGHT));
      Serial.println("THERMAL SIGNATURE DETECTED RIGHT");

      if (digitalRead(PIR_SENSOR_LEFT));
      Serial.println("THERMAL SIGNATURE DETECTED LEFT");

    }

  }

  if (Serial.available() )
  {
    char val = Serial.read();
    switch (val) // Perform an action depending on the command
    {
      case 'w'://Move Forward
      case 'W':
        forward ();
        setRed();
        break;
      case 's'://Move Backwards
      case 'S':
        reverse ();
        setRed();
        break;
      case 'a'://Turn Left
      case 'A':
        left ();
        setRed();
        break;
      case 'd'://Turn Right
      case 'D':
        right ();
        setRed();

        break;
      case 'q'://Turn Right
      case 'Q':
        fire ();

        break;

      default:
        stop();
        setBlue();
        break;
    }
  }
}