Thursday, April 9, 2015

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;
}