/*
  Testing RC Connection
  7/15/14
  Captures the Pulse signal from a RC controller
  Feel free to do whatever you want with this code example
 */
 #include <Servo.h>

// Create Variables to hold the Receiver signals 
int Ch1,Ch2,Ch3,Ch4,Ch5,Ch6; 
//Create variabes and pin locations for te tri-color LED
int led1 = 4;     // Pin one color of the tri-color LED is connected to.
int led2 = 5;     // Pin one color of the tri-color LED is connected to.
int led3 = 6;     // Pin one color of the tri-color LED is connected to.

// Create Servo Objects as defined in the Servo.h files
Servo L_Servo;  // Servo DC Motor Driver (Designed for RC cars)
Servo R_Servo;  // Servo DC Motor Driver (Designed for RC cars)


void setup() {
// Set the pins that the transmitter will be connected to all to input 
  pinMode(7, INPUT); //I connected this to Chan1 of the Receiver
  pinMode(8, INPUT); //I connected this to Chan2 of the Receiver
  pinMode(9, INPUT); //I connected this to Chan3 of the Receiver
  pinMode(10, INPUT); //I connected this to Chan4 of the Receiver
  pinMode(11, INPUT); //I connected this to Chan5 of the Receiver
  pinMode(12, INPUT); //I connected this to Chan6 of the Receiver
// Attach Speed controller that acts like a servo to the board  
  R_Servo.attach(0); //Pin 0
  L_Servo.attach(1); //Pin 1
//Set the pin directions for the tri-color LED    
  pinMode(led1, OUTPUT);  // Tell the LED Pin to be an output. 
  pinMode(led2, OUTPUT);  // Tell the LED Pin to be an output.  
  pinMode(led3, OUTPUT);  // Tell the LED Pin to be an output. 
//Initialize the serial port    
  Serial.begin(9600); 
}
//**********************************
//Subroutine for the autonomous test
//**********************************
void AutoM()
{
    digitalWrite(led1, HIGH); // Tell the LED color to turn on.   
    delay(100);               // the number is milliseconds on. 
    digitalWrite(led1, LOW);  // Tell the LED color to turn off.
//Random motion for the servo to see if is working   
    R_Servo.writeMicroseconds(1800);  // sets the servo position
    L_Servo.writeMicroseconds(1800);  // sets the servo position
   
    digitalWrite(led2, HIGH); // Tell the LED color to turn on.   
    delay(100);               // the number is milliseconds on. 
    digitalWrite(led2, LOW);  // Tell the LED color to turn off.
  
    digitalWrite(led3, HIGH); // Tell the LED color to turn on.   
    delay(100);               // the number is milliseconds on. 
    digitalWrite(led3, LOW);  // Tell the LED color to turn off. 
//Random motion for the servo to see if is working      
    R_Servo.writeMicroseconds(1200);  // sets the servo position
    L_Servo.writeMicroseconds(1200);  // sets the servo position 
}

void DriveServosRC()
{
  if (Ch1 < 1550 && Ch1 > 1450)
  {
    R_Servo.writeMicroseconds(1500);  // sets the servo position to a stop position
  }
  else
  {
   R_Servo.writeMicroseconds(Ch1);  // sets the servo position Right Stick
  }
  if (Ch4 < 1550 && Ch4 > 1450)
  {
    L_Servo.writeMicroseconds(1500);  // sets the servo position to a stop position
  }
  else
  {
    L_Servo.writeMicroseconds(Ch4);  // sets the servo position Left Stick
  }
}

void PrintRC()
{ // print out the values you read in:
 Serial.println(" RC Control Mode "); 
 Serial.print("Value Ch1 = "); 
 Serial.println(Ch1);
 Serial.print("Value Ch2 = "); 
 Serial.println(Ch2);
 Serial.print("Value Ch3 = "); 
 Serial.println(Ch3); 
 Serial.print("Value Ch4 = "); 
 Serial.println(Ch4);
 Serial.print("Control = "); 
 Serial.println(Ch5);
 Serial.print("Value Ch6 = "); 
 Serial.println(Ch6);
 Serial.println(" ");
 delay(1000);
}

void loop() 
{
// Collect data from used RC channels. Comment out ones that are not used.       
      Ch1 = pulseIn(7, HIGH, 21000); 
      Ch2 = pulseIn(8, HIGH, 21000); 
      Ch3 = pulseIn(9, HIGH, 21000);
      Ch4 = pulseIn(10, HIGH, 21000);
      Ch5 = pulseIn(11, HIGH, 21000);  //I am using this channel to test for Autonomous on or off
      Ch6 = pulseIn(12, HIGH, 21000); 

if (Ch5 > 1800)  //Test if switch is flipped...if so then....
  {
      DriveServosRC(); //Jump to RC control routine
  }
else
  {
    AutoM();          //Jump to Autonomous control
  }


//      PrintRC(); //Print Values for RC Mode Diagnostics

}