Code Example

/*
  Testing RC Connection
  2/22/17 by Brian Patton
  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;
int CalcHold;        //Variable to temp hold calculations for steering stick corections
int Rwheel;
int Lwheel;

// 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)

//**************************************************************
//*****************  Setup  ************************************
//**************************************************************
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(7, INPUT); //I connected this to Chan6 of the Receiver
  pinMode(13, OUTPUT); //Onboard LED to output for diagnostics
  // Attach Speed controller that acts like a servo to the board
  R_Servo.attach(0); //Pin 0
  L_Servo.attach(1); //Pin 1
  //Flash the LED on and Off 10x before entering main loop
  for (int i = 0; i < 10; i++) {
    digitalWrite(13, HIGH);
    delay(200);
    digitalWrite(13, LOW);
    delay(200);
  }
  //Flash the LED on and Off 10x End
  Serial.begin(9600);
}
//************************  loop()  ****************************
//**********************  Main Loop  ***************************
//**************************************************************
void loop()
{
  Ch1 = pulseIn(12, HIGH, 21000); // Capture pulse width on Channel 1
  Ch2 = pulseIn(11, HIGH, 21000); // Capture pulse width on Channel 2
  //      Ch3 = pulseIn(10, HIGH, 21000);  // Capture pulse width on Channel 3
  //      Ch4 = pulseIn(9, HIGH, 21000);  // Capture pulse width on Channel 4
  Ch5 = pulseIn(8, HIGH, 21000); // Capture pulse width on Channel 5
  //      Ch6 = pulseIn(7, HIGH, 21000); // Capture pulse width on Channel 6

//  TestWheels();
    DriveServosRC(); // Drive Motors under RC control
  //    PrintRC(); //Print Values for RC Mode Diagnostics

}
//********************** MixLimits() ***************************
//*******  Make sure values never exceed ranges  ***************
//******  For most all servos and like controlers  *************
//****   control must fall between 1000uS and 2000uS  **********
//**************************************************************
void SetLimits() {
  if (Lwheel < 1000) {// Can be set to a value you don't wish to exceed
    Lwheel = 1000;    // to adjust maximums for your own robot
  }
  if (Lwheel > 2000) {// Can be set to a value you don't wish to exceed
    Lwheel = 2000;    // to adjust maximums for your own robot
  }
  if (Rwheel < 1000) {// Can be set to a value you don't wish to exceed
    Rwheel = 1000;    // to adjust maximums for your own robot
  }
  if (Rwheel > 2000) {// Can be set to a value you don't wish to exceed
    Rwheel = 2000;    // to adjust maximums for your own robot
  }
  pulseMotors();
}
//*******************   pulseMotors  ***************************
//pulses either mapped or direct signals generated from Mixlimits
//**************************************************************
void pulseMotors() {
  //un-comment the next two line to drive the wheels directly with the MaxLimits Set
//  R_Servo.writeMicroseconds(Rwheel);
//  L_Servo.writeMicroseconds(Lwheel);

  //un-comment the next two to map a control range.
  //*** Take the standard range of 1000 to 2000 and frame it to your own minimum and maximum
  //*** for each wheel.
  Rwheel = map(Rwheel, 1000, 2000, 1350, 1650);
  Lwheel = map(Lwheel, 1000, 2000, 1350, 1650);
  R_Servo.writeMicroseconds(Rwheel);
  L_Servo.writeMicroseconds(Lwheel);
 
  // un-comment this line do display the value being sent to the motors
//  PrintWheelCalcs(); //REMEMBER: printing values slows reaction times

}
//*****************  PrintWheelCalcs()  ************************
//*******  Prints calculated wheel output values  **************
//**************************************************************
void PrintWheelCalcs() {
  Serial.print("Right Wheel = ");
  Serial.println(Rwheel);
  Serial.print("Left Wheel = ");
  Serial.println(Lwheel);
  Serial.println(" ");
}
//********************  TestWheels()  **************************
//*  Direct call to Servos to test wheel speed and direction  **
//**************************************************************
void TestWheels() {
  for (int i = 1000; i <= 1500; i += 10) {
    R_Servo.writeMicroseconds(i); // 1000-2000, 1500 should be stop
    L_Servo.writeMicroseconds(i); // 1000-2000, 1500 should be stop
    Serial.println(" Pulse width = " + (String)i);
    delay(200);
  }

  digitalWrite(13, HIGH);
  for (int i = 5; i >= 0; i--) {
    Serial.println("Holding at 1500 for  " + (String)i + " more seconds.");
    delay(1000);
  }
  digitalWrite(13, LOW);

  for (int i = 1500; i <= 2000; i += 10) {
    R_Servo.writeMicroseconds(i); // 1000-2000, 1500 should be stop
    L_Servo.writeMicroseconds(i); // 1000-2000, 1500 should be stop
    Serial.println(" Pulse width = " + (String)i);
    delay(200);
  }
  R_Servo.writeMicroseconds(1500); // 1000-2000, 1500 should be stop
  L_Servo.writeMicroseconds(1500); // 1000-2000, 1500 should be stop

  while (true) {
    Serial.println(" Holding Pulse width at 1500 ");
    digitalWrite(13, HIGH);
    delay(1000);
    digitalWrite(13, LOW);
    delay(100);
  }
}
//*******************  DriveServosRC()  ************************
//******  Use the value collected from Ch1 and Ch2  ************
//******  on a single stick to relatively calculate  ***********
//****  speed and direction of two servo driven wheels *********
//**************************************************************
void DriveServosRC()
{
  if (Ch2 <= 1500) {
    Lwheel = Ch1 + Ch2 - 1500;
    Rwheel = Ch1 - Ch2 + 1500;
    SetLimits();
  }
  if (Ch2 > 1500) {
    int Ch1_mod = map(Ch1, 1000, 2000, 2000, 1000); // Invert the Ch1 axis to keep the math similar
    Lwheel = Ch1_mod + Ch2 - 1500;
    Rwheel = Ch1_mod - Ch2 + 1500;
    SetLimits();
  }
}
//**********************  PrintRC()  ***************************
//***  Simply print the collected RC values for diagnostics  ***
//**************************************************************
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);
}