//********************Arduino Code

//********************Arduino Code for the RC Car********************************
#include //This library is needed for I2C communication (motor shield uses this)
#include //Library for the adafruit motor shield
#include "utility/Adafruit_MS_PWMServoDriver.h" //needed for motor shield, file is found in the library folder of Adafruit_MotorShield.h
#include //Call SPI library so you can communicate with the nRF24L01+
#include //nRF2401 libarary found at https://github.com/tmrh20/RF24/
#include //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