/*
Testing RC Connection with 5 channels
2/22/17 by Brian Patton
Captures the Pulse signal from a RC controller
Requires a tri color LED connected to pins 23-21
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;
const int LED1 = 23;
const int LED2 = 22;
const int LED3 = 21;
// 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
pinMode(LED1, OUTPUT); //external LED to output for diagnostics
pinMode(LED2, OUTPUT); //external LED to output for diagnostics
pinMode(LED3, OUTPUT); //external 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
LEDMix3_4(); // Display mixing or LED colors
Ch5Check(); // brightens and darkens 2 LEDs with proportional stick control
// PrintRC(); //Print Values for RC Mode Diagnostics
}
//********************** Ch5Check() **************************
//********************** Test Channel 5 **********************
//**************************************************************
void Ch5Check() {
// This function maps the 1000-2000ms input pulse from Ch3 and Ch4 to
// the range of 0 - 255 required for analogWrite and displays it on
// two of the LEDs of the tri-color LED hooked to PWM pins 23-21
if (Ch5 > 1600) {
digitalWrite(LED3, HIGH);
}
else {
digitalWrite(LED3, LOW);
}
}
//********************** LEDMix3_4() **************************
//***************** Test Channel 3 and 4 *********************
//**************************************************************
void LEDMix3_4() {
// This function maps the 1000-2000ms input pulse from Ch3 and Ch4 to
// the range of 0 - 255 required for analogWrite and displays it on
// two of the LEDs of the tri-color LED hooked to PWM pins 23-21
analogWrite(LED1, map(Ch3, 1000, 2000, 0, 255));
analogWrite(LED2, map(Ch4, 1000, 2000, 0, 255));
}
//********************** 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);
}