//Wirelessly Controlled Rover using Arduino and RF Modules
//Coded and tested successfully by Sidharth Makhija

// Version 1 28/2/2014

//Sensor used is HCSR04 Ultrasonic Sensor

/*Working : This is the Receiver code of the Rover.
When the joystick on the remote is moved in the forward direction 
the rover first checks for any obstacle in front of it....if the
path is clear the rover moves forward...else it overrides the user's
control and performs the obstacle avoidance maneuver.

This code uses the VirtualWire and AFMotor(motorshield library).

The motor shield used is self soldered by using Adafruit's motorshield
schemaitic which they have provided on their site since they're awesome!

More pictures,videos and info on the rover here  : http://letsmakerobots.com/node/40002

For any doubts,corrections,etc. shoot a mail here : sidharth1295@gmail.com

May the force be with you!
*/


#include <VirtualWire.h>
#include <AFMotor.h>

//Initializing all variables

//digital pin 16(analog 2) 
int hLight =16;

int distFront;
int distLeft;
int distRight;
//Minimum distance of 20cm from any obstacle.
int distLimit = 20;

//digital pin 14,15 (analog 0 and 1 respectively)
int trig = 14;
int echo =  15;
unsigned long pulsetime = 0;

AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR34_1KHZ);
AF_DCMotor motor4(4, MOTOR34_1KHZ);

void setup() 
{
   
   Serial.begin(9600); //Only for Debugging...Comment out 	
   Serial.println("setup");
   
   pinMode(trig,OUTPUT);
   pinMode(echo,INPUT);
   pinMode(hLight,INPUT);
   
   //Starts Receiving Data
   startRx();
   
   //Fix speed of all motors to 200
   motor1.setSpeed(200);
   motor2.setSpeed(200); 
   motor3.setSpeed(200);
   motor4.setSpeed(200);
}

void loop()
{
  
  
    uint8_t buf[VW_MAX_MESSAGE_LEN];
    uint8_t buflen = VW_MAX_MESSAGE_LEN;
    
    //Check for any Data
    if (vw_get_message(buf, &buflen)) 
    {
	int i;
         // Flash a light to show received good message
        digitalWrite(13, true); 
	
	for (i = 0; i < buflen; i++)
	{
	    
    if(buf[i] == '1')
      {
        //Checks for obtsacle since forward command is received.
        Serial.println("forward");
        distFront = readDistance();
        if(distFront < distLimit)
        {
          //Override the controller since obstacle is detected.
          Serial.println("Override");
          override();         
        }
        else 
        { 
          //No obstacle----> move forward.
        forward(); 
        }
        
      }
    if(buf[i] == '2')
      {
        backward();
        Serial.println("backward");
      }
    if(buf[i] == '4')
      {
        left();
        Serial.println("left");
      }
    if(buf[i] == '3')
      {
        right();
        Serial.println("right");
      }
    if(buf[i] == '5')
       {
         stopMotor();
         Serial.println("stopped");
       }
       if(buf[i]== '6')
      {
         digitalWrite(hLight,HIGH);
         Serial.println("Light em Up");
      }
    if(buf[i]== '7')
      {
         digitalWrite(hLight,LOW);
         Serial.println("Lights OFF");
      }
	    Serial.print(" ");
	}
	Serial.println("");
        digitalWrite(13, false);

   }
}

//////////////////Functions!Functions!Functions/////////////////////////////////////////////

void forward()
{
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
  Serial.println("Forward");
  
}

void backward()
{
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
  Serial.println("Backward");
}

void right()
{
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
  Serial.println("Right");
}

void left()
{
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
  Serial.println("Left");
}

void stopMotor()
{
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
  Serial.println("Stopped");
}

//Returns distance value from ultrasonic sensor
int readDistance()
{
  
  digitalWrite(trig, LOW); 
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);       
  delayMicroseconds(10);               
  digitalWrite(trig,LOW);         
  pulsetime = pulseIn(echo, HIGH);  
  return pulsetime/58.2;//magic                
                                       
}

void override()
{
  /*Note : The time it takes for the rover to turn depends in the
  motors you use...so that will be fixed during debugging.*/
  
  //Stop receiving data from the user .
  vw_rx_stop();
  delay(10);
  //Obstacle avoidance maneuver and all that jazz..
  stopMotor();
  delay(300);
  backward();
  delay(300);
  stopMotor();
  delay(10);
  scanLeft();
  delay(100);
  scanRight();
  if(distLeft > distRight && distLeft > distLimit)
  {
    left();
    delay(500);
    stopMotor();
  }
  else if(distRight > distLeft && distRight > distLimit)
  {
    right();
    delay(500);
    stopMotor();
  }
  else 
  { 
    backward();
    delay(300);
    stopMotor();
  }
  
  startRx();
}

void scanLeft()
{
  Serial.println("Scanning Left....");
  delay(10);
  left();  //turns left to scan
  delay(300);
  stopMotor();
  delay(10);
  distLeft = readDistance();
  Serial.print("Left is");
 Serial.println(distLeft); //scan
  delay(100);
  right();   //turns to the centre again
  delay(300);
  stopMotor();
}

void scanRight()
{
  
  Serial.println("Scanning Right....");
  right();
  delay(300);
  stopMotor();
  delay(10);
  distRight = readDistance();
  Serial.print("Right is ");
  Serial.println(distRight);
  delay(100);
  left();
  delay(300);
  stopMotor();
}

void startRx()
{
  vw_setup(2000);
  //Rx on digital pin 2.
  vw_set_rx_pin(2);
  vw_rx_start();
}