//********************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;
}
No comments:
Post a Comment