Patton Robotics Resource Site

/*
  RC Receiver V1.4 DC motor Control via RC speed controller
  and 2 Sharp Sensors.
  by Brian Patton
  3/24/14
  Captures the Pulse signal from a RC controller
  Feel free to do whatever you want with this code example
 */
#include <Servo.h> //The Servo Library
// Create Variables to hold the Receiver signals 
int Ch1,Ch2,Ch3,Ch4,Ch5,Ch6; 
int L_SharpValue; // Variable to hold Left Sharp data
int C_SharpValue; // Variable to hold Center Sharp data
int R_SharpValue; // Variable to hold Right Sharp data
const int L_SharpPin = A9; //Pin connecting the sharp
const int C_SharpPin = A8; //Pin connecting the sharp
const int R_SharpPin = A7; //Pin connecting the sharp
int CalcHold;        //Variable to remp hold calculations for steering stick corections

// for the MAP function!
const int minSig = 0;              // Min Inches to Object
const int maxSig = 1024;             // Max Inches to Object
const int minPW = 1000;            // Motor clockwise
const int maxPW = 2000;            // Motor counterclockwise

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


void setup() {
// Set the pins that the transmitter will be connected to all to input 
  pinMode(12, INPUT); //I connected this to Chan1 of the Receiver
  pinMode(11, INPUT); //I connected this to Chan2 of the Receiver
  pinMode(10, INPUT); //I connected this to Chan3 of the Receiver
  pinMode(9, INPUT); //I connected this to Chan4 of the Receiver
  pinMode(8, INPUT); //I connected this to Chan5 of the Receiver
  pinMode(6, INPUT); //I connected this to Chan6 of the Receiver
// Attach Speed controller that acts like a servo to the board  
  R_DCMotor.attach(1); //Pin 1
  L_DCMotor.attach(0); //Pin 0

  Serial.begin(9600); 
}
void DriveServosRC()
{
  if (Ch1 < 1550 && Ch1 > 1450)
    R_DCMotor.writeMicroseconds(Ch2);  // sets the servo position
    L_DCMotor.writeMicroseconds(Ch2);  // Set so that stearing would be horz stick

  if (Ch1 >1550)
    L_DCMotor.writeMicroseconds(Ch2);  // Set so that stearing would be horz stick
    CalcHold = 1450-Ch1;
    R_DCMotor.writeMicroseconds(Ch2-CalcHold);  // sets the servo position

  if (Ch1 <1450)
    L_DCMotor.writeMicroseconds(Ch2);  // Set so that stearing would be horz stick
    CalcHold = Ch1-1550;
    R_DCMotor.writeMicroseconds(Ch2-CalcHold);  // sets the servo position
}

void DriveServosAuto()
{ 
  if(L_SharpValue < 250 && R_SharpValue < 250)
    {
      R_DCMotor.writeMicroseconds(2000);
      L_DCMotor.writeMicroseconds(2000);
    }
    if(L_SharpValue > 250 || R_SharpValue > 250)
    {
      mapECHOLeftMotor();
      mapECHORightMotor();
    }   
}

void mapECHOLeftMotor(){
  int PW = map(L_SharpValue, minSig, maxSig, maxPW, minPW);      // left servo max is sensor min for correct motion!
  R_DCMotor.writeMicroseconds(PW);
//  Serial.print("Left PW Value = ");
//  Serial.println(PW);
//  Serial.println(" ");
}

void mapECHORightMotor(){
  int PW = map(R_SharpValue, minSig, maxSig, maxPW, minPW);
  L_DCMotor.writeMicroseconds(PW);
//  Serial.print("Right PW Value = ");
//  Serial.println(PW);
//  Serial.println(" ");
//  delay(500);
}

void PrintAuto()
{ // print out the values you read in:
  Serial.println(" FULL AUTO MODE ");
  Serial.print("Left Max Value = ");
  Serial.println(L_SharpValue);
  Serial.print("Right Max Value = ");
  Serial.println(R_SharpValue); 
  Serial.println(" ");
  delay(1000);
}
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() {
  Ch5 = pulseIn(8, HIGH, 21000); 
  
  if(Ch5 <= 1600) //Going to be in Autonomous Mode
    {
      L_SharpValue = analogRead(L_SharpPin); //Read the value of the sharp sensor
      C_SharpValue = analogRead(C_SharpPin); //Read the value of the sharp sensor
      R_SharpValue = analogRead(R_SharpPin); //Read the value of the sharp sensor
      DriveServosAuto();  //Drive Motors under Autonomous control   
//      PrintAuto(); //Print Values for Auto Mode Diagnostics
    }
  if(Ch5 > 1600) //Going to be in RC Control Mode
    {
      Ch1 = pulseIn(12, HIGH, 21000); 
      Ch2 = pulseIn(11, HIGH, 21000); 
//      Ch3 = pulseIn(10, HIGH, 21000);
//      Ch4 = pulseIn(9, HIGH, 21000);
//      Ch5 = pulseIn(8, HIGH, 21000);
//      Ch6 = pulseIn(7, HIGH, 21000); 
      
      DriveServosRC(); // Drive Motors under RC control
//      PrintRC(); //Print Values for RC Mode Diagnostics
    }
}