Patton Robotics Resource Site

#include <MaxSonarPW.h>

/*
  RC Receiver V1.4 DC motor Control via RC speed controller
  and 2 servos.
  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
MaxSonarPW RightMaxSen(14);
MaxSonarPW LeftMaxSen(15);
// Create Variables to hold the Receiver signals 
int Ch1,Ch2,Ch3,Ch4,Ch5,Ch6; 

int LeftMaxIn;        //Variable to hold Max Data In
int RightMaxIn;      //Variable to hold Max Data In
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 = 24;             // Max Inches to Object
const int minPW = 1000;            // servo clockwise
const int maxPW = 2000;            // servo 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(LeftMaxSen.Inches > 24 && RightMaxSen.Inches > 24)
    {
      R_DCMotor.writeMicroseconds(2000);
      L_DCMotor.writeMicroseconds(2000);
    }
    if(LeftMaxSen.Inches < 25 || RightMaxSen.Inches < 25)
    {
      mapECHOLeftMotor();
      mapECHORightMotor();
    }   
}

void mapECHOLeftMotor(){
  int PW = map(LeftMaxSen.Inches, minSig, maxSig, minPW, maxPW);      // 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(RightMaxSen.Inches, minSig, maxSig, minPW, maxPW);
  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(LeftMaxSen.Inches);
  Serial.print("Right Max Value = ");
  Serial.println(RightMaxSen.Inches); 
  Serial.println(" ");
  if(LeftMaxSen.Inches < 30)
  {
   Serial.println("turning Right"); 
  }
  if(RightMaxSen.Inches < 30)
  {
   Serial.println("turning Left"); 
  }
  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
    {
      RightMaxIn = analogRead(A2);
      RightMaxSen.ReadPulse();
      RightMaxSen.Conv_Inches();
      LeftMaxSen.ReadPulse();
      LeftMaxSen.Conv_Inches();
      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
    }
}