/*
Code for Part 3 of the BaW-Bot Series of Instructables - Final Code

- Includes start/stop based on inputs received from an IR Remote.
- Includes Ultrasonic rangefinder Code
- Includes Lever Switches


Connections:
Motor Driver
- Pin 5 ---> PWMA
- Pin 6 ---> AIN2
- Pin 7 ---> AIN1
- Pin 8 ---> STBY
- Pin 9 ---> PWMB
- Pin 10 ---> BIN2
- Pin 11 ---> BIN1

Other
- Pin 2 ---> Right Lever-switch
- Pin 3 ---> Left Lever-switch
- Pin 4 ---> Ultrasonic Rangefinder
- Pin 12 ---> IR Sensor pin OUT

Motors controlled by:
- Motor 1: A01 and A02
- Motor 2: B01 and B02

*/

#include <IRremote.h>


//Define the Pins

//Motor 1
byte pinAIN1 = 7; //Direction
byte pinAIN2 = 6; //Direction
byte pinPWMA = 5; //Speed

//Motor 2
byte pinBIN1 = 11; //Direction
byte pinBIN2 = 10; //Direction
byte pinPWMB = 9; //Speed

//Standby
byte pinSTBY = 8;

//Other Pins
byte pinLeverR = 2;  //Right lever-switch
byte pinLeverL = 3;  //Left lever-switch
byte pinPing = 4;  //IR Receiver
byte pinIRRecv = 12;  //IR Receiver

//
//
// ** CHANGE THE LINE BELOW TO INSERT YOUR IR CODE **
// 
//
const unsigned int irCodeStartStop = 2011265621;
unsigned int irCodeRead;

//Constants to help remember the motor parameters
static boolean turnCW = 0;  //for motorDrive function
static boolean turnCCW = 1; //for motorDrive function
static boolean motor1 = 0;  //for motorDrive, motorStop, motorBrake functions
static boolean motor2 = 1;  //for motorDrive, motorStop, motorBrake functions

//Working Variables
boolean bMoving = false;  //Is the Bot moving (t/f) - IR Remote Control sets this
byte delayPingTarget = 100;  //Sets delay between Pings
byte delayPingLast = -1;  //Last Ping Delay

//Variables for averaging the Distance
const int numDistReadings = 5;
int cmDistance;
int distReadings[numDistReadings];
byte indexDistReading=0;
int totalDistReading=0;
int averageDistReading=0;


//Instantiate IR Receiver
IRrecv irrecv(pinIRRecv);
//Variable to store IR Results
decode_results results;

void setup()
{
//Set the PIN Modes
  pinMode(pinPWMA, OUTPUT);
  pinMode(pinAIN1, OUTPUT);
  pinMode(pinAIN2, OUTPUT);

  pinMode(pinPWMB, OUTPUT);
  pinMode(pinBIN1, OUTPUT);
  pinMode(pinBIN2, OUTPUT);

  pinMode(pinSTBY, OUTPUT);

  pinMode(pinLeverR, INPUT);
  pinMode(pinLeverL, INPUT);

  irrecv.enableIRIn(); // Start the receiver

//Make sure motors are in Standby mode (not turning!)
  motorsStandby(); 

//For debugging:
//  Serial.begin(9600);

//Attach Interrupts
//  attachInterrupt(0,reverseTurnRight, RISING);
//  attachInterrupt(1,reverseTurnLeft, RISING);
  
//Zero out distance readings  
  for (byte thisDistReading = 0; thisDistReading < numDistReadings; thisDistReading++)
     distReadings[thisDistReading] = 0;   

}




void loop()
{

//***************************  
//Check for Infrared Signals
//***************************
  if (irrecv.decode(&results)) 
  {
    irCodeRead = results.value;
    Serial.println(irCodeRead);

    //We're using "Switch" statement so we can handle any additional IR codes in future
    switch (irCodeRead) 
    {
      //If the predefined code received, change the motion
      case irCodeStartStop:
        if (bMoving == true)
        {
          motorsStandby(); //If it's moving, stop it
        }
        else
        {
          motorDrive(motor1, turnCW, 200);  //If it's stopped, start moving
          motorDrive(motor2, turnCW, 200);
        }
        bMoving = !bMoving;  //Toggle the Moving flag
        break;
    }
    irrecv.resume(); // Receive the next value
  }

//**************************************
// Check whether whiskers activated
//**************************************

  if (bMoving == true)
  {
    if (digitalRead(pinLeverR)==HIGH)
    {
      motorBrake(motor1);
      motorBrake(motor2);
      reverseTurnLeft();
      delayPingTarget = 50; //Close to obstacles to ping more often
      delayPingLast = millis();
    }
    else if (digitalRead(pinLeverL)==HIGH)
    {
      motorBrake(motor1);
      motorBrake(motor2);
      reverseTurnRight();
      delayPingTarget = 50; //Close to obstacles to ping more often
      delayPingLast = millis();
    }
  }


//**************************************
// Calculate Unltrasound Distance and Decide Action
//**************************************
  if (bMoving == true)
  {
  //Ping for distance if it's time
    if (delayPingReached() == true)
    {
      cmDistance = averageDistance();
//      Serial.print("Distance ");
//      Serial.println(cmDistance);
    }

    //Decide what to do depending on distance...

    //We're close, so stop & reverse
    if (cmDistance <= 10)
    {
      //Stop the motors
      motorStop(motor1);
      motorStop(motor2);
      reverseTurnLeft();
    }

    //Getting closer, slow down and ping more often
    else if (cmDistance <= 50)
    {
      //Ping more frequently
      delayPingTarget = 75;
      //Slow the motors
      motorDrive(motor1, turnCW, 125);
      motorDrive(motor2, turnCW, 125);
    }

    //within a meter, slow down and ping more often
    else if (cmDistance <= 100)
    {
      //Ping more frequently if less than 1m from the obstacle
      delayPingTarget = 100;
      //Slow the motors
      motorDrive(motor1, turnCW, 150);
      motorDrive(motor2, turnCW, 150);
    }

    //Far away, so keep motoring
    else
    {
      delayPingTarget = 500;
      motorDrive(motor1, turnCW, 200);
      motorDrive(motor2, turnCW, 200);
    }

  }
      
}


void reverseTurnLeft()
//Perform a short reverse and a turn.
{
  if (bMoving==true)
  {
    motorDrive(motor1, turnCCW, 100);  //Counter-CW = reverse
    motorDrive(motor2, turnCCW, 100);
    delay(1500);  //Reverse for 1.5 sec
    motorDrive(motor1, turnCW,100);
    delay(1000);  //Turn for 1 sec
  }
//  Serial.println("RTL");
}

void reverseTurnRight()
//Perform a short reverse and a turn.
{
  if (bMoving==true)
  {
    motorDrive(motor1, turnCCW, 100);  //Counter-CW = reverse
    motorDrive(motor2, turnCCW, 100);
    delay(1500);  //Reverse for 1.5 sec
    motorDrive(motor2, turnCW,100);
    delay(1000);  //Turn for 1 sec
  }
//  Serial.println("RTR");
}


void motorDrive(boolean motorNumber, boolean motorDirection, int motorSpeed)
{
  /*
  This Drives a specified motor, in a specific direction, at a specified speed:
    - motorNumber: motor1 or motor2 ---> Motor 1 or Motor 2
    - motorDirection: turnCW or turnCCW ---> clockwise or counter-clockwise
    - motorSpeed: 0 to 255 ---> 0 = stop / 255 = fast
  */

  boolean pinIn1;  //Relates to AIN1 or BIN1 (depending on the motor number specified)

  
//Specify the Direction to turn the motor
  //Clockwise: AIN1/BIN1 = HIGH and AIN2/BIN2 = LOW
  //Counter-Clockwise: AIN1/BIN1 = LOW and AIN2/BIN2 = HIGH
  if (motorDirection == turnCW) 
    pinIn1 = HIGH;
  else 
    pinIn1 = LOW;

//Select the motor to turn, and set the direction and the speed
  if(motorNumber == motor1)
  {
    digitalWrite(pinAIN1, pinIn1);
    digitalWrite(pinAIN2, !pinIn1);  //This is the opposite of the AIN1
    analogWrite(pinPWMA, motorSpeed);
  }
  else
  {
    digitalWrite(pinBIN1, pinIn1);
    digitalWrite(pinBIN2, !pinIn1);  //This is the opposite of the BIN1
    analogWrite(pinPWMB, motorSpeed);
  }
    
  

//Finally , make sure STBY is disabled - pull it HIGH
  digitalWrite(pinSTBY, HIGH);

}

void motorBrake(boolean motorNumber)
{
/*
This "Short Brake"s the specified motor, by setting speed to zero
*/

  if (motorNumber == motor1)
    analogWrite(pinPWMA, 0);
  else 
    analogWrite(pinPWMB, 0);
    
}


void motorStop(boolean motorNumber)
{
  /*
  This stops the specified motor by setting both IN pins to LOW
  */
  if (motorNumber == motor1) {
    digitalWrite(pinAIN1, LOW);
    digitalWrite(pinAIN2, LOW);
  }
  else
  {
    digitalWrite(pinBIN1, LOW);
    digitalWrite(pinBIN2, LOW);
  }  
}


void motorsStandby()
{
  /*
  This puts the motors into Standby Mode
  */
  digitalWrite(pinSTBY, LOW);
}


//Is it time to ping again?
boolean delayPingReached() 
{
  if (delayPingLast == -1)
  delayPingLast = millis();
  if (millis() - delayPingLast >= delayPingTarget)
 {
   //Delay has passed
  delayPingLast = millis();
  return true;
 }
 else
 {
   return false;
 }
}

//Calculate average Distance
int averageDistance()
{
  int thisDistance;
  
  thisDistance = sonarDistance();
  
  totalDistReading = totalDistReading - distReadings[indexDistReading];
  distReadings[indexDistReading] = thisDistance;
  totalDistReading = totalDistReading + thisDistance;
  indexDistReading++;

  if (indexDistReading >= numDistReadings)
  indexDistReading=0;
  
  averageDistReading = totalDistReading/numDistReadings;

  return averageDistReading;
}


//Get the distance for once ping
int sonarDistance()
{
  long duration;

  // The PING is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  
  pinMode(pinPing, OUTPUT);
  digitalWrite(pinPing, LOW);
  delayMicroseconds(2);
  digitalWrite(pinPing, HIGH);
  delayMicroseconds(5);
  digitalWrite(pinPing, LOW);

  // The same pin is used to read the signal from the PING: a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.

  pinMode(pinPing, INPUT);
  duration = pulseIn(pinPing, HIGH);

//  Serial.print("distance");
//  Serial.print("\t");  
//  Serial.println(microsecondsToCentimeters(duration));
  //Convert to cm and return
  return microsecondsToCentimeters(duration);
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}