nRF24L01 / Accelerometer RC Car

In this video we build a remote control car using Arduino and the nRF24L01+ transceiver for wireless communication / control of the car. To control this car we won't be using the classic joystick, but instead a glove! The glove will use an accelerometer (MPU-6050) to control the car's direction and speed based on the position of your hand.

//********************Arduino Code for the RC Car********************************
//This code is for a remote control car using the nRF24L01 for wireless communication. The tutorial on this project can be found on the ForceTronics Youtube channel
//This code is free and open for anybody to use or modify at your own risk

#include <Wire.h> //This library is needed for I2C communication (motor shield uses this)
#include <Adafruit_MotorShield.h> //Library for the adafruit motor shield
#include "utility/Adafruit_PWMServoDriver.h" //needed for motor shield, file is found in the library folder of Adafruit_MotorShield.h
#include <SPI.h> //Call SPI library so you can communicate with the nRF24L01+
#include <nRF24L01.h> //nRF2401 libarary found at https://github.com/tmrh20/RF24/
#include <RF24.h> //nRF2401 libarary found at https://github.com/tmrh20/RF24/

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
// create an object for each motor and assign it to a port on the shield 
Adafruit_DCMotor *M1 = AFMS.getMotor(1);
Adafruit_DCMotor *M2 = AFMS.getMotor(2);
Adafruit_DCMotor *M3 = AFMS.getMotor(3);
Adafruit_DCMotor *M4 = AFMS.getMotor(4);

const int pinCE = 9; //This pin is used to set the nRF24 to standby (0) or active mode (1)
const int pinCSN = 10; //This pin is used to tell the nRF24 whether the SPI communication is a command or message 
//The controller sends a "packet" to control the speed and direction of the car. The first and last byte just signal the start and end of the packet
//The second and third bytes represent the speed and direction of the car. The second byte is for forward and backwards
//The third byte is for right and left directions
byte bArray[] = {255, 125, 125, 254}; //This array holds the speed and direction packet
RF24 wirelessSPI(pinCE, pinCSN); // Declare object from nRF24 library (Create your wireless SPI) 
const uint64_t pAddress = 0xB00B1E5000LL;  //Create a pipe addresses for the 2 nodes to communicate over, my address spells boobies :-)
int sCount = 0; //variable to track timer to stop motor, if communication is lost this will shut off motor

void setup() {
 AFMS.begin();  //Start motor shield object, create with the default frequency 1.6KHz
 wirelessSPI.begin();  //Start the nRF24 module
 wirelessSPI.setAutoAck(1);                    // Ensure autoACK is enabled, this means rec send acknowledge packet to tell xmit that it got the packet with no problems
 wirelessSPI.enableAckPayload();               // Allow optional payload or message on ack packet, even though we are not using this
 wirelessSPI.setRetries(5,10);                 // Defines packet retry behavior: first arg is delay between retries at 250us x 5 and max no. of retries
 wirelessSPI.openReadingPipe(1,pAddress);      //open pipe o for recieving meassages with pipe address
 wirelessSPI.startListening();                 // Start listening for messages, because we are the reciever 
 motorStop(); //ensure motor is at stop to start
}

void loop() {
  if(wirelessSPI.available()){ //check to see if a data packet is available from transmitter
     wirelessSPI.read( bArray, 4 ); //read 4 bytes of data and store it in array
     if(verifyPacket) { //verify it is a valid packet to control car
       setMotorSpeed(bArray[1], bArray[2]); //get the forward / backward and right / left speeds
       sCount = 0; //reset count
     }
     else {
      //do something here if a bad packet was recieved 
     }
   }
  
  delay(1); //short delay before looping again
  
  sCount++; //increment the loop count
  if(sCount > 60) {  motorStop(); } //if we do not get a packet from the controller
}

//This function makes sure a packet is valid by checking it has the correct start and end byte
//it also checks to see if the forward / backward and right / left speeds are valid
bool verifyPacket() {
  if(bArray[0] == 255 & bArray[3] == 254 & bArray[1] < 251 & bArray[2] < 251) return true;
  else return false;
}

//This function is used to set the direction and speed based on the two bytes from the controller
//125 means stop, above 125 means right or forward, below 125 means left or backwards
void setMotorSpeed(int upDown, int leftRight) {
  int lR = 0; //left and right direction variable, zero is stop
  int bF = 0; //forward and backward direction variable, zero is stop
  
  if(leftRight == 125) { //if true no left or right turn (stop)
    lR = 0;
  }
   else if(leftRight > 125) { //if this is true right turn
     lR = 1;
     leftRight = leftRight - 125; //scale variable from 0 to 125
   }
   else { //else this is a left
     lR = 2;
   }
   
   if(upDown == 125) { //if true no forward or back (stop)
      bF = 0;
   }
   else if(upDown > 125) { //if this is true go forward
     bF = 1;
     upDown = upDown - 125; //scale variable from 0 to 125
   }
   else { //this is go backwards
     bF = 2;
   }
   
   //We have direction now set speed
   //scale turn and back / forward
   if(lR == 0 && bF == 0) { //stop all motors if no forward / backward and right / left direction
     motorStop();
   }
   else if (bF==1) { //Go forward
     if(lR == 0) { //go straight forward
       goForward(scaleSpeed(upDown)); //Send forward speed 
     }
     else if(lR == 1) { //go forward and right
       goTurn(scaleSpeed(scaleTurn(upDown,leftRight)), scaleSpeed(upDown), 1); //send forward and right turn speeds
     }
     else {
       goTurn(scaleSpeed(upDown),scaleSpeed(scaleTurn(upDown,leftRight)), 1); //send forward and left turn speeds
     }
   }
   else if (bF==2) { //same thing but this is backwards
     if(lR == 0) { //go straight backwards
       goBackward(scaleSpeed(upDown));
     }
     else if(lR == 1) { //go forward and right
       goTurn(scaleSpeed(scaleTurn(upDown,leftRight)), scaleSpeed(upDown), 0);
     }
     else {
       goTurn(scaleSpeed(upDown),scaleSpeed(scaleTurn(upDown,leftRight)), 0);
     }
   }
   else { //No forward or backwards direction so just do a turn
     if(lR==1) { //Right turn
       goRight(scaleSpeed(leftRight));
     }
     else { //left turn
       goLeft(scaleSpeed(leftRight));
     }
   }
}

//This function scales the speed value from controller to a value for the motor
//max motor speed is 250 and max value from controller is 125
int scaleSpeed(int scale) { 
  float r = ((float)scale/125)*250; //scale to value between 1 and 250
  return int(r); //covert to int value and return
}

//Used to scale turn value, based on forward or backward speed as well as turn speed
int scaleTurn(int fBSp, int lRSp) {
  float r =(float)fBSp*(1 - (float)lRSp/125);
  return int(r);
}

//function to stop the motors
void motorStop() {
  M2->run(RELEASE);
  M4->run(RELEASE);
  M1->run(RELEASE);
  M3->run(RELEASE);
}

//function to tell motors to go forward, input is speed
void goForward(int mSpeed) {
  M1->setSpeed(mSpeed);
  M2->setSpeed(mSpeed);
  M3->setSpeed(mSpeed);
  M4->setSpeed(mSpeed);
  M2->run(FORWARD);
  M4->run(FORWARD);
  M1->run(FORWARD);
  M3->run(FORWARD);
}

//function to tell motors to go backward, input is speed
void goBackward(int mSpeed) {
  M1->setSpeed(mSpeed);
  M2->setSpeed(mSpeed);
  M3->setSpeed(mSpeed);
  M4->setSpeed(mSpeed);
  M2->run(BACKWARD);
  M4->run(BACKWARD);
  M1->run(BACKWARD);
  M3->run(BACKWARD);
}

//function for left or right turn. inputs are speed for left tires and speed for right tires
//and whether we are going forward or backwards
void goTurn(int rTire, int lTire, int forward) {
  
  M1->setSpeed(rTire);
  M2->setSpeed(lTire);
  M3->setSpeed(rTire);
  M4->setSpeed(lTire);
   //code to turn Right
  if(forward) {
    M2->run(FORWARD); //M2 and M4 are left tires
    M4->run(FORWARD);
    M1->run(FORWARD); //M1 and M3 are right tires
    M3->run(FORWARD);
  }
  else {
    M2->run(BACKWARD);
    M4->run(BACKWARD);
    M1->run(BACKWARD);
    M3->run(BACKWARD);
  }
}

//right turn function, no forward or backwards motion
void goRight(int tSpeed) {
  M1->setSpeed(tSpeed);
  M2->setSpeed(tSpeed);
  M3->setSpeed(tSpeed);
  M4->setSpeed(tSpeed);
   //code to turn Right
  M2->run(FORWARD); //left tires
  M4->run(FORWARD);
  M1->run(BACKWARD); //right tires
  M3->run(BACKWARD);
}

//left turn function, no forward or backwards motion
void goLeft(int tSpeed) {
  M1->setSpeed(tSpeed);
  M2->setSpeed(tSpeed);
  M3->setSpeed(tSpeed);
  M4->setSpeed(tSpeed);
   //code to turn Right
  M2->run(BACKWARD); //left tires
  M4->run(BACKWARD);
  M1->run(FORWARD); //right tires
  M3->run(FORWARD);
}

//********************Arduino Code for the Glove Controller****************************
//This code is for a remote control car using the nRF24L01 for wireless communication 
//and accel to dictate the cars direction and speed based of hand postion. 
//The tutorial on this project can be found on the ForceTronics Youtube channel
//This code is free and open for anybody to use or modify at your own risk

#include <SPI.h> //Call SPI library so you can communicate with the nRF24L01+
#include <nRF24L01.h> //nRF2401 libarary found at https://github.com/tmrh20/RF24/
#include <RF24.h> //nRF2401 libarary found at https://github.com/tmrh20/RF24/
#include "I2Cdev.h" //the MPU6050 Accel uses I2C communication
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

const int pinCE = 9; //This pin is used to set the nRF24 to standby (0) or active mode (1)
const int pinCSN = 10; //This pin is used to tell the nRF24 whether the SPI communication is a command or message to send out
MPU6050 accelgyro; //declare the object to access and cotrol the accel (we don't use the gyro)
RF24 wirelessSPI(pinCE, pinCSN); // Create your nRF24 object or wireless SPI connection
const uint64_t pAddress = 0xB00B1E5000LL; // Radio pipe addresses for the 2 nodes to communicate. The address spells boobies :-)
//The controller sends a "packet" to control the speed and direction of the car. The first and last byte just signal the start and end of the packet
//The second and third bytes represent the speed and direction of the car. The second byte is for forward and backwards
//The third byte is for right and left directions
byte bArray[] = {255, 125, 125, 254}; //This array holds the speed and direction packet

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
  accelgyro.initialize(); //initialize the accel object
  wirelessSPI.begin();            //Start the nRF24 module
  wirelessSPI.setAutoAck(1); // Ensure autoACK is enabled so rec sends ack packet to let you know it got the transmit packet payload
  wirelessSPI.enableAckPayload(); // Allow optional ack payloads (we don't use this)
  wirelessSPI.setRetries(5,15); // Sets up retries and timing for packets that were not ack'd, current settings: smallest time between retries, max no. of retries
  wirelessSPI.openWritingPipe(pAddress);// pipe address that we will communicate over, must be the same for each nRF24 module (this is a transmitter)
  wirelessSPI.stopListening(); //we are the transmitter so don't need to listen
}

void loop() {
  int x, y, z; //create variables to hold accel values (we don't use z direction)
  accelgyro.getAcceleration(&x, &y, &z); //get accel values, note variables are sent by reference
  buildArray(buildXValue(x), buildYValue(y)); //build speed and direction array or packet, this is what we send to the car to control
  if (!wirelessSPI.write( bArray, 4 )){  //if the send fails let the user know over serial monitor
     //put code here to do something if sending the packet fails
   }
  
  delay(5); //delay a before sending the next packet
}

//This function is used to build the forward / backwards direction and speed value
//The X direction of the accel is used for the forward / backwards direction and speed
//Note that the accel value has to be scaled to fit in a byte of data
byte buildXValue(int xV) {
  if(xV <= 1000 & xV >= -1000) { //This creates a cushion for the stop value so the car is not constantly moving
    return 125; //this is the stop value
  }
  else if (xV > 1000) { //if positive value then car is being directed forward
    xV = xV - 1000;
    if(xV > 15000) { xV = 15000; } //ceiling value for forward speed
    return (scaleSpeed(xV,15000) + 125); //scale speed to send, add 125 since this is forward
  }
  else { //Negative x value is to tell the car to go backwards
    xV = xV * -1; //conver negative value to positive
    xV = xV - 1000;
    if(xV > 15000) { xV = 15000; } //set ceiling on accel value
    return scaleSpeed(xV,15000); //scale to 1 to 125
  }
}

//This function is used to build the right and left direction and speed value
//The Y direction of the accel is used for the right and left direction and speed
//Note that the accel value has to be scaled to fit in a byte of data
byte buildYValue(int yV) {
  if(yV <= 1000 & yV >= -1000) { //This creates a cushion for the stop value so the car is not constantly moving
    return 125; //this is the stop value
  }
  else if (yV > 1000) { //if positive value then car is being directed right
    yV = yV - 1000;
    if(yV > 11000) { yV = 11000; } //ceiling value for right speed
    return scaleSpeed(yV,11000);
  }
  else { //Negative x value is to tell the car to go backwards
    yV = yV * -1;
    yV = yV - 1000;
    if(yV > 11000) { yV = 11000; } //ceiling value for left speed
    return (scaleSpeed(yV,11000)+125);  //scale speed to send, add 125 since this is left
  }
}

//This function scales the accel speed value to a value that can fit in a byte
byte scaleSpeed(int scale, int sVal) {
  float r = ((float)scale/sVal)*125; //speed is between 0 to 125
  return (byte)r;
}

//This function builds the packet that sends the speed and direction
//The first and last byte is used to represent the start and end of the packet
void buildArray(byte xV, byte yV) {
  bArray[0] = 255;
  bArray[1] = xV;
  bArray[2] = yV; 
  bArray[3] = 254;
}

Android / Arduino Remote Control Car

In this post we build a remote control car using Arduino, Bluetooth, and an Android device. The Android device serves as the controller. One cool factor of this project is our Android app uses the position of the Android device to control the car!

RC Car Schematic

/* This sketch is for a remote controlled car with four electric motors that uses the Arduino Uno, RN42 Bluetooth module, and an
Adafruit Motorshield. This code is free for anybody to use or modify
*/

#include <Wire.h> //needed for motors and motor shield
#include <Adafruit_MotorShield.h> //needed for motors and motor shield
#include "utility/Adafruit_PWMServoDriver.h" //needed for motors and motor shield
#include <ctype.h>

int con = 0; //global variable to track connection status
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
// create an object for each motor and assign it to a port on the shield 
Adafruit_DCMotor *M1 = AFMS.getMotor(1);
Adafruit_DCMotor *M2 = AFMS.getMotor(2);
Adafruit_DCMotor *M3 = AFMS.getMotor(3);
Adafruit_DCMotor *M4 = AFMS.getMotor(4);
int count = 0; //counts how long its been since comms from joystick
 String uDSpeed = "500"; //create global variables to hold speed and direction info
 String lRSpeed = "500"; //defulat is 500 because that is stop condition

//setup code only executed once
void setup() {
  Serial.begin(115200); //start serial commm
  
  //This loop runs until a connection from another RN42 is complete and a "#" is recieved from the car
  //The joystick RN42 is the slave
  while(!con) { 
    if((char)Serial.read() == '#') { con = 1; }//once connected change "con" to true
    delay(5);
  }

 AFMS.begin();  //Start motor shield object, create with the default frequency 1.6KHz
} 


void loop() {
  
  //check if a full speed / direction frame is ready to be read
  if(Serial.available() >= 6) {
    String temp; //temperary string to hold incoming data
    char c = (char)Serial.read();
    if(c == 'u') { //If a 'u' was read this is start of an up / down data frame
      for(int i=0; i<5; i++) { //loop to read 5 other bytes of frame
        if(i < 4) { //reads the speed portion of frame into string 
          temp += (char)Serial.read();
        }
        else { //look for end of frame 'd' character, if it is there save this reading as new speed
          if((char)Serial.read() == 'd') { 
           uDSpeed = temp; 
           count = 0; //just got speed so reset count
          }
        }
      }
    }
    else if(c == 'l') { //If a 'l' was read this is start of an left / right data frame
      for(int i=0; i<5; i++) { //following code is the same as above except for direction frame
        if(i < 4) {
          temp += (char)Serial.read();
        }
        else {
          if((char)Serial.read() == 'r') { 
           lRSpeed = temp; 
           count = 0; //just got speed so reset count
          }
        }
      }
    }
  }
  
  delay(1);
  //the following code will stop the car if no comms with joystick for 150ms
  count++;
  if(count > 20) {
    setMotorSpeed(500,500);
  }
  
  //function call to set motor speeds
  setMotorSpeed(uDSpeed.toInt(),lRSpeed.toInt());
}

//This function clears all bytes out of arduino serial read buffer
void clearSerialBuf() {
 while(Serial.available()) { Serial.read(); }
}

//This function uses the ADC values from the joystick and turns them into motor speeds for going 
//forware, right, left, and reverse. Inputs are the left/right and up/down joystick axis
void setMotorSpeed(int upDown, int leftRight) {
  int lR = 0;
  int bF = 0;
  
  //If left/right is 500 no turn 
  if(leftRight == 500) {
    lR = 0;
  }
   else if(leftRight > 500) { //If greater than 500 this is a right turn
     lR = 1;
     leftRight = leftRight - 500;
   }
   else { //less than 500 this is a left turn
     lR = 2;
     leftRight = 500 - leftRight;
   }
   
   if(upDown == 500) { //500 no up/down direction
      bF = 0;
   }
   else if(upDown > 500) {//more than 500 go forward
     bF = 1;
     upDown = upDown - 500;
   }
   else { //less than 500 go backward
     bF = 2;
     upDown = 500 - upDown;
   }
   
   //If direction variables are both 0 the car is stopped
   if(lR == 0 && bF == 0) {
     motorStop();
   }
   else if (bF==1) { //if forward variable is true
     if(lR == 0) { //no turn so go straight forward
       goForward(scaleSpeed(upDown));
     }
     else if(lR == 1) { //go forward and right
       goTurn(scaleSpeed(scaleTurn(upDown,leftRight)), scaleSpeed(upDown), 1);
     }
     else { //go forward and left
       goTurn(scaleSpeed(upDown),scaleSpeed(scaleTurn(upDown,leftRight)), 1);
     }
   }
   else if (bF==2) { //if backwards variable is true
     if(lR == 0) { //go straight backwards
       goBackward(scaleSpeed(upDown));
     }
     else if(lR == 1) { //go backward and right
       goTurn(scaleSpeed(scaleTurn(upDown,leftRight)), scaleSpeed(upDown), 0);
     }
     else { //go backward and left
       goTurn(scaleSpeed(upDown),scaleSpeed(scaleTurn(upDown,leftRight)), 0);
     }
   }
   else { //if no forward or back then just turn
     if(lR==1) { //Right turn, left wheels forward and right wheels backwards
       goRight(scaleSpeed(leftRight));
     }
     else { //left turn, right wheels forward and left wheels backwards
       goLeft(scaleSpeed(leftRight));
     }
   }
}

//function to stop the motors
void motorStop() {
  M2->run(RELEASE);
  M4->run(RELEASE);
  M1->run(RELEASE);
  M3->run(RELEASE);
}

//function to tell motors to go forward, input is speed
void goForward(int mSpeed) {
  M1->setSpeed(mSpeed);
  M2->setSpeed(mSpeed);
  M3->setSpeed(mSpeed);
  M4->setSpeed(mSpeed);
  M2->run(FORWARD);
  M4->run(FORWARD);
  M1->run(FORWARD);
  M3->run(FORWARD);
}

//function to tell motors to go backward, input is speed
void goBackward(int mSpeed) {
  M1->setSpeed(mSpeed);
  M2->setSpeed(mSpeed);
  M3->setSpeed(mSpeed);
  M4->setSpeed(mSpeed);
  M2->run(BACKWARD);
  M4->run(BACKWARD);
  M1->run(BACKWARD);
  M3->run(BACKWARD);
}


//function for left or right turn. inputs are speed for left tires and speed for right tires
//and whether we are going forward or backwards
void goTurn(int rTire, int lTire, int forward) {
  
  M1->setSpeed(rTire);
  M2->setSpeed(lTire);
  M3->setSpeed(rTire);
  M4->setSpeed(lTire);
   //code to turn Right
  if(forward) {
    M2->run(FORWARD); //M2 and M4 are left tires
    M4->run(FORWARD);
    M1->run(FORWARD); //M1 and M3 are right tires
    M3->run(FORWARD);
  }
  else {
    M2->run(BACKWARD);
    M4->run(BACKWARD);
    M1->run(BACKWARD);
    M3->run(BACKWARD);
  }
}

//right turn function, no forward or backwards motion
void goRight(int tSpeed) {
  tSpeed = tSpeed - (tSpeed*.2); //reduce speed by 20%
  M1->setSpeed(tSpeed);
  M2->setSpeed(tSpeed);
  M3->setSpeed(tSpeed);
  M4->setSpeed(tSpeed);
   //code to turn Right
  M2->run(FORWARD); //left tires
  M4->run(FORWARD);
  M1->run(BACKWARD); //right tires
  M3->run(BACKWARD);
}

//left turn function, no forward or backwards motion
void goLeft(int tSpeed) {
  tSpeed = tSpeed - (tSpeed*.2); //reduce speed by 20%
  M1->setSpeed(tSpeed);
  M2->setSpeed(tSpeed);
  M3->setSpeed(tSpeed);
  M4->setSpeed(tSpeed);
   //code to turn Right
  M2->run(BACKWARD); //left tires
  M4->run(BACKWARD);
  M1->run(FORWARD); //right tires
  M3->run(FORWARD);
}

//This function scales the speed values from the joystick ADCs to the speed values of the motors
int scaleSpeed(int scale) {
  float r = ((float)scale/500)*250;
  return int(r);
}

//This scales the turns based on the forward / backward speeds
int scaleTurn(int fBSp, int lRSp) {
  float r =(float)fBSp*(1 - (float)lRSp/500);
  return int(r);
}

Building a Bluetooth Remote Control Car

In this post we build a remote control car and controller / joystick using Bluetooth for communication and Arduino for control. A fun project for all ages! At the end of the post you will find the code and schematics shown in the video.

Bluetooth RC Car Schematics

/* This sketch is for a remote controlled car with four electric motors that uses the Arduino Uno, RN42 Bluetooth module, and an
Adafruit Motorshield. This code is free for anybody to use or modify
*/

#include <Wire.h> //needed for motors and motor shield
#include <Adafruit_MotorShield.h> //needed for motors and motor shield
#include "utility/Adafruit_PWMServoDriver.h" //needed for motors and motor shield
#include <ctype.h>

int con = 0; //global variable to track connection status
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
// create an object for each motor and assign it to a port on the shield 
Adafruit_DCMotor *M1 = AFMS.getMotor(1);
Adafruit_DCMotor *M2 = AFMS.getMotor(2);
Adafruit_DCMotor *M3 = AFMS.getMotor(3);
Adafruit_DCMotor *M4 = AFMS.getMotor(4);
int count = 0; //counts how long its been since comms from joystick
 String uDSpeed = "500"; //create global variables to hold speed and direction info
 String lRSpeed = "500"; //defulat is 500 because that is stop condition

//setup code only executed once
void setup() {
  Serial.begin(115200); //start serial comm
  delay(1000); //delay for serial comm to start up
  do //do while loop for putting RN42 module into command mode
  {
   clearSerialBuf(); //empty serial read buffer
   Serial.print("$$$"); //but BT module in command mode
   delay(1000); //Wait for module to enter command mode
  } while ((char)Serial.read() != 'C'); //look for C from CMD response to confirm in command mode
  
  clearSerialBuf(); //empty serial read buffer

 while(!con) { //Connect to bluetooth device and keep trying until successful
  connectBT("0006666741AD"); //call function to connect, address is hardcoded, if connection is succesful "con" is set
  clearSerialBuf(); //empty serial buffer
  delay(4000); //delay after connect
 }

 Serial.print('#'); //Send this to tell joystick that we are connected, this will cause the joystick to exit setup code

 AFMS.begin();  //Start motor shield object, create with the default frequency 1.6KHz
} 


void loop() {
  
  //check if a full speed / direction frame is ready to be read
  if(Serial.available() >= 6) {
    String temp; //temperary string to hold incoming data
    char c = (char)Serial.read();
    if(c == 'u') { //If a 'u' was read this is start of an up / down data frame
      for(int i=0; i<5; i++) { //loop to read 5 other bytes of frame
        if(i < 4) { //reads the speed portion of frame into string 
          temp += (char)Serial.read();
        }
        else { //look for end of frame 'd' character, if it is there save this reading as new speed
          if((char)Serial.read() == 'd') { 
           uDSpeed = temp; 
           count = 0; //just got speed so reset count
          }
        }
      }
    }
    else if(c == 'l') { //If a 'l' was read this is start of an left / right data frame
      for(int i=0; i<5; i++) { //following code is the same as above except for direction frame
        if(i < 4) {
          temp += (char)Serial.read();
        }
        else {
          if((char)Serial.read() == 'r') { 
           lRSpeed = temp; 
           count = 0; //just got speed so reset count
          }
        }
      }
    }
  }
  
  delay(1);
  //the following code will stop the car if no comms with joystick for 150ms
  count++;
  if(count > 20) {
    setMotorSpeed(500,500);
  }
  
  //function call to set motor speeds
  setMotorSpeed(uDSpeed.toInt(),lRSpeed.toInt());
}

//This function connects with a BT module. Input is the device's address
//If the connection is successful
void connectBT(String address) {
  //module is in command mode send action command to connect with to address
  Serial.print("C," + address + "\r");
  //successful response to connection command
  //TRYING
  //%CONNECT,0006666741AD,0
  int done = 0; //variable to track when connection response is recieved
  
  while(!done) { //wait for reply, read it and set connection variable accordingly
    if(Serial.available()) { //if data is ready to be read
      char c = (char)Serial.read();
      if(c=='%') { //this is variable set in RN42 firmware for connection response
        if((char)Serial.read() == 'C') { //if followed by 'C' connection was successful
          con = 1; //set connection variable
          done = 1; //we can exit loop
        }
        else { //if not a C then connection failed
         con = 0; //not connected
         done = 1; //exit loop
        }
      }
      else if(c == 'f') { //if this is an 'f' connection failed
        con = 0;
        done = 1;
      }
      else { } //do nothing
    }
    delay(50); //delay before running loop again
  }
}

//This function clears all bytes out of arduino serial read buffer
void clearSerialBuf() {
 while(Serial.available()) { Serial.read(); }
}

//This function uses the ADC values from the joystick and turns them into motor speeds for going 
//forware, right, left, and reverse. Inputs are the left/right and up/down joystick axis
void setMotorSpeed(int upDown, int leftRight) {
  int lR = 0;
  int bF = 0;
  
  //If left/right is 500 no turn 
  if(leftRight == 500) {
    lR = 0;
  }
   else if(leftRight > 500) { //If greater than 500 this is a right turn
     lR = 1;
     leftRight = leftRight - 500;
   }
   else { //less than 500 this is a left turn
     lR = 2;
     leftRight = 500 - leftRight;
   }
   
   if(upDown == 500) { //500 no up/down direction
      bF = 0;
   }
   else if(upDown > 500) {//more than 500 go forward
     bF = 1;
     upDown = upDown - 500;
   }
   else { //less than 500 go backward
     bF = 2;
     upDown = 500 - upDown;
   }
   
   //If direction variables are both 0 the car is stopped
   if(lR == 0 && bF == 0) {
     motorStop();
   }
   else if (bF==1) { //if forward variable is true
     if(lR == 0) { //no turn so go straight forward
       goForward(scaleSpeed(upDown));
     }
     else if(lR == 1) { //go forward and right
       goTurn(scaleSpeed(scaleTurn(upDown,leftRight)), scaleSpeed(upDown), 1);
     }
     else { //go forward and left
       goTurn(scaleSpeed(upDown),scaleSpeed(scaleTurn(upDown,leftRight)), 1);
     }
   }
   else if (bF==2) { //if backwards variable is true
     if(lR == 0) { //go straight backwards
       goBackward(scaleSpeed(upDown));
     }
     else if(lR == 1) { //go backward and right
       goTurn(scaleSpeed(scaleTurn(upDown,leftRight)), scaleSpeed(upDown), 0);
     }
     else { //go backward and left
       goTurn(scaleSpeed(upDown),scaleSpeed(scaleTurn(upDown,leftRight)), 0);
     }
   }
   else { //if no forward or back then just turn
     if(lR==1) { //Right turn, left wheels forward and right wheels backwards
       goRight(scaleSpeed(leftRight));
     }
     else { //left turn, right wheels forward and left wheels backwards
       goLeft(scaleSpeed(leftRight));
     }
   }
}

//function to stop the motors
void motorStop() {
  M2->run(RELEASE);
  M4->run(RELEASE);
  M1->run(RELEASE);
  M3->run(RELEASE);
}

//function to tell motors to go forward, input is speed
void goForward(int mSpeed) {
  M1->setSpeed(mSpeed);
  M2->setSpeed(mSpeed);
  M3->setSpeed(mSpeed);
  M4->setSpeed(mSpeed);
  M2->run(FORWARD);
  M4->run(FORWARD);
  M1->run(FORWARD);
  M3->run(FORWARD);
}

//function to tell motors to go backward, input is speed
void goBackward(int mSpeed) {
  M1->setSpeed(mSpeed);
  M2->setSpeed(mSpeed);
  M3->setSpeed(mSpeed);
  M4->setSpeed(mSpeed);
  M2->run(BACKWARD);
  M4->run(BACKWARD);
  M1->run(BACKWARD);
  M3->run(BACKWARD);
}


//function for left or right turn. inputs are speed for left tires and speed for right tires
//and whether we are going forward or backwards
void goTurn(int rTire, int lTire, int forward) {
  
  M1->setSpeed(rTire);
  M2->setSpeed(lTire);
  M3->setSpeed(rTire);
  M4->setSpeed(lTire);
   //code to turn Right
  if(forward) {
    M2->run(FORWARD); //M2 and M4 are left tires
    M4->run(FORWARD);
    M1->run(FORWARD); //M1 and M3 are right tires
    M3->run(FORWARD);
  }
  else {
    M2->run(BACKWARD);
    M4->run(BACKWARD);
    M1->run(BACKWARD);
    M3->run(BACKWARD);
  }
}

//right turn function, no forward or backwards motion
void goRight(int tSpeed) {
  M1->setSpeed(tSpeed);
  M2->setSpeed(tSpeed);
  M3->setSpeed(tSpeed);
  M4->setSpeed(tSpeed);
   //code to turn Right
  M2->run(FORWARD); //left tires
  M4->run(FORWARD);
  M1->run(BACKWARD); //right tires
  M3->run(BACKWARD);
}

//left turn function, no forward or backwards motion
void goLeft(int tSpeed) {
  M1->setSpeed(tSpeed);
  M2->setSpeed(tSpeed);
  M3->setSpeed(tSpeed);
  M4->setSpeed(tSpeed);
   //code to turn Right
  M2->run(BACKWARD); //left tires
  M4->run(BACKWARD);
  M1->run(FORWARD); //right tires
  M3->run(FORWARD);
}

//This function scales the speed values from the joystick ADCs to the speed values of the motors
int scaleSpeed(int scale) {
  float r = ((float)scale/500)*250;
  return int(r);
}

//This scales the turns based on the forward / backward speeds
int scaleTurn(int fBSp, int lRSp) {
  float r =(float)fBSp*(1 - (float)lRSp/500);
  return int(r);
}

Bluetooth Joystick

/*This arduino sketch is for a joystick for controller an RC car. The joystick is Parallax 2 axis
joystick. The RN42 Bluetooth module is used to communicate with the RC car. This code is free for 
anybody to use or modify*/

int UD = 500; //Variable for storing up / down joystick axis for forward / reverse speed
int LR = 500; //Variable for storing left / right joystick axis reading for direction
int con = 0; //Variable to track if RN42 is connected

void setup() {
  Serial.begin(115200); //start serial commm
  
  //This loop runs until a connection from another RN42 is complete and a "#" is recieved from the car
  //The joystick RN42 is the slave
  while(!con) { 
    if((char)Serial.read() == '#') { con = 1; }//once connected change "con" to true
    delay(5);
  }
}

void loop() {
   UD = filter(analogRead(A0)); //Read up / down joystick axis value, apply filter, and store result
   LR = filter(analogRead(A1)); //Read left / right joystick axis value, apply filter, and store result
   Serial.print(formatValue(UD,1)); //format up / down axis value into packet and send it to RC car
   delay(7);
   Serial.print(formatValue(LR,0)); //format left / right axis value into packet and send it to RC car
   delay(7);
}

//This function sets joystick resting axis values to a consistent value (500) for both axis. It also
//keeps extreme values in a consistent range
int filter(int jRead) {
  if(jRead > 485 && jRead < 540) { return 500; }
  else if(jRead < 20) { return 0; }
  else if(jRead > 1000) {return 1000; }
  else { return jRead; }
}

//This function creates the up / down and left / right packets for RC car to read.
//It makes every value four digits and adds a starting and ending character for each packet
//The input is the speed or direction value and packet type (speed or direction)
String formatValue(int val, int udlr) {
 String temp;

  if(val < 10) { //if below 10 add three leading zeros
   temp = "000" + (String)val;
  } 
  else if (val < 100) { //if below 100 add two leading zeros
   temp = "00" + (String)val; 
  }
  else if (val < 1000) { //if below 1000 add one leading zero
   temp = "0" + (String)val; 
  }
  else { temp = (String)val; } //if 1000 add no zeros
  
  if(udlr) { //for speed packet add 'u' to front and 'd' to back
    temp = 'u' + temp + 'd';
  }
  else { //for direction packet add 'l' to front and 'r' to back
    temp = 'l' + temp + 'r';
  }
  
  return temp;
}