/*
Complete RC Control with..
Autonomous guidance with simple if statements and variable speeds
3/14/17 by Brian Patton
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 Rwheel; // Variable to hold R wheel speed
int Lwheel; // Variable to hold L wheel speed
const int lPhoto = A1; // Left Photoresistor
const int rPhoto = A0; // Right Photoresistor
const int sharpPin = A2; // Sharp Sensor
const int LED = 13; // Onboard LED location
int lPhotoVal; // Variable to store L photoresistor value
int rPhotoVal; // Variable to store R photoresistor value
int sharpVal; // Variable to store Sharp Sensor value
int valDif; // Variable to store difference between photo values
int rSpeed, lSpeed; // Variables to hold autonomous speed changes for each wheel
// 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(LED, OUTPUT);//Onboard LED to output for diagnostics
// Attach Speed controller that acts like a servo to the board
R_Servo.attach(2); //Pin 0
L_Servo.attach(1); //Pin 1
rSpeed = 1450;
lSpeed = 1620;
//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();
// fowardSlow();
// 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() {
Ch5 = pulseIn(8, HIGH, 21000); // Capture pulse width on Channel 5
if (Ch5 > 1600) {
digitalWrite(LED, HIGH);
autonomous();
}
else {
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
digitalWrite(LED, LOW);
DriveServosRC();
}
}
//********************** autoMode() **************************
//********************** Autonomous Mode **********************
//**************************************************************
void autonomous() {
checkSensors();
while (lPhotoVal > 1000) {
TLeftSlow(1500, 1);
checkSensors();
Ch5Check();
if (Ch5 < 1600) {
break;
}
}
if (valDif > 70) {
if (lPhotoVal > rPhotoVal) {
rSpeed = rSpeed + 5;
if (rSpeed >= 1480) {
rSpeed = 1480;
}
TLeftSlow(rSpeed, 1);
// Serial.println(rSpeed);
}
else {
lSpeed = lSpeed - 5;
if (lSpeed <= 1540) {
lSpeed = 1540;
}
TRightSlow(lSpeed, 1);
// Serial.println(lSpeed);
}
}
else {
rSpeed = 1450;
lSpeed = 1620;
Forward(10);
// Serial.println("forward");
}
// printSensors();
}
//********************** 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
}
//******************* 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();
}
}
//***************** Forward(int Dlay) ***********************
// Move the robot Slowly Forward
//**************************************************************
void Forward(int Dlay)
{
R_Servo.writeMicroseconds(1450); // sets the servo position
L_Servo.writeMicroseconds(1620); // sets the servo position
delay(Dlay);
}
//***************** Reverse(int Dlay) ***********************
// Reverse the robot
//**************************************************************
void Reverse(int Dlay)
{
R_Servo.writeMicroseconds(2000); // sets the servo position
L_Servo.writeMicroseconds(1000); // sets the servo position
delay(Dlay);
}
//***************** stopBot(int Dlay) ***********************
// Stop the robot
//**************************************************************
void stopBot(int Dlay)
{
R_Servo.writeMicroseconds(1500); // sets the servo position
L_Servo.writeMicroseconds(1500); // sets the servo position
delay(Dlay);
}
//************* TLeftSlow(int rVal,int Dlay) *******************
// left turn with tapering speed and a duration
//**************************************************************
void TLeftSlow(int rVal, int Dlay)
{
R_Servo.writeMicroseconds(rVal); // sets the servo position
L_Servo.writeMicroseconds(1600); // sets the servo position
delay(Dlay);
}
//************* TRightSlow(int lVal,int Dlay) *******************
// Right turn with tapering speed and a duration
//**************************************************************
void TRightSlow(int lVal, int Dlay)
{
R_Servo.writeMicroseconds(1450); // sets the servo position
L_Servo.writeMicroseconds(lVal); // sets the servo position
delay(Dlay);
}
//******************** checkSensors() **************************
// Check value of Sensors Stop bot if object is close
//**************************************************************
void checkSensors()
{
rPhotoVal = analogRead(rPhoto);
lPhotoVal = analogRead(lPhoto);
valDif = abs(rPhotoVal - lPhotoVal); // looking for threshold
sharpVal = analogRead(sharpPin);
for (int i = 0; i <= 3; i++) {
sharpVal = sharpVal + analogRead(sharpPin);
}
sharpVal = sharpVal / 5;
if (sharpVal >= 400) {//changed from 500
stopBot(100);
}
}
//******************** printSensors() **************************
// Print the sensor values Slows robot when in use!!!
//**************************************************************
void printSensors() {
Serial.println("Right Value = " + (String)rPhotoVal);
Serial.println("Left Value = " + (String)lPhotoVal);
Serial.println("Difference Value = " + (String)valDif);
Serial.println("Sharp Value = " + (String)sharpVal);
Serial.println(" ");
delay(100);
}
//********************** 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);
}