ShotDeliverSystem_Car

// A demonstration of this project can be found here: https://bit.ly/2Ynx09x

/*
    Parts used to make the car:
    [4] 6v DC motors
    [4] wheels to attach to the motors
    [1] servo
    [1] vans shoe box
    [1] 9v battery
    [16] AA batteries
    [3] power switches
    [2] L298N motor drivers
    [1] Arduino Mega
        some silicon wrap
        some electrical tape
*/

#include <Servo.h>
#include <nRF24L01.h>
#include <printf.h>
#include <RF24.h>
#include <RF24_config.h>
#include <SPI.h>

RF24 radio(7, 8);
Servo robotArm;
const byte address[6] = "111111";
int States[3]; // States[0] = X_JOY
               // States[1] = Y_JOY
               // States[2] = Button

// rear motors
int enableMotor1 = 10;
int enableMotor2 = 9;
int rotate1ClockWise = 22;
int rotate1CounterClockWise = 23;
int rotate2ClockWise = 40;
int rotate2CounterClockWise = 41;

// front motors
int enableMotor3 = 11;
int enableMotor4 = 6;
int rotate3ClockWise = 24;
int rotate3CounterClockWise = 25;
int rotate4ClockWise = 42;
int rotate4CounterClockWise = 43;


bool currentArmState = false;
bool buttonIsPressed = false;

void setup()
{
  robotArm.attach(3);
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_HIGH);
  pinMode(enableMotor1, OUTPUT);
  pinMode(enableMotor2, OUTPUT);
  pinMode(enableMotor3, OUTPUT);
  pinMode(enableMotor4, OUTPUT);
  pinMode(rotate1ClockWise, OUTPUT);
  pinMode(rotate2ClockWise, OUTPUT);
  pinMode(rotate3ClockWise, OUTPUT);
  pinMode(rotate4ClockWise, OUTPUT);
  pinMode(rotate1CounterClockWise, OUTPUT);
  pinMode(rotate2CounterClockWise, OUTPUT);
  pinMode(rotate3CounterClockWise, OUTPUT);
  pinMode(rotate4CounterClockWise, OUTPUT);
  robotArm.write(180);
  turnMotorsOff();
}

void loop()
{
  delay(5);

  radio.startListening();
  if ( radio.available()) {
    while (radio.available()) {
      radio.read(&States, sizeof(States));
    }
    if(States[1] <= 490)
    {
      int motorPowerForward = map(States[1], 0, 490, 255, 0);
      int turningValRight = map(States[0], 490  , 1023, 0, 255);
      int turningValLeft = map(States[0], 0, 490, 255, 0);
      if(States[0] > 490) {
        int motorPowerNet = motorPowerForward - turningValRight;
        if(motorPowerNet < 0)
          motorPowerNet = 0;
        analogWrite(enableMotor1, motorPowerNet);
        analogWrite(enableMotor2, motorPowerForward);
        analogWrite(enableMotor3, motorPowerForward);
        analogWrite(enableMotor4, motorPowerNet);
      }
      else {
        int motorPowerNet = motorPowerForward - turningValLeft;
        if(motorPowerNet < 0)
          motorPowerNet = 0;
        analogWrite(enableMotor1, motorPowerForward);
        analogWrite(enableMotor2, motorPowerNet);
        analogWrite(enableMotor3, motorPowerNet);
        analogWrite(enableMotor4, motorPowerForward);
      }
      spinMotors(1,0); // reverse
    }
    else if(States[1] >= 512)
    {
      int motorPowerReverse = map(States[1], 512, 1023, 0, 255);
      int turningValRight = map(States[0], 490  , 1023, 0, 255);
      int turningValLeft = map(States[0], 0, 490, 255, 0);
      if(States[0] > 490) {
        int motorPowerNet = motorPowerReverse - turningValRight;
        if(motorPowerNet < 0)
          motorPowerNet = 0;
        analogWrite(enableMotor1, motorPowerNet);
        analogWrite(enableMotor2, motorPowerReverse);
        analogWrite(enableMotor3, motorPowerReverse);
        analogWrite(enableMotor4, motorPowerNet);
      }
      else {
        int motorPowerNet = motorPowerReverse - turningValLeft;
        if(motorPowerNet < 0)
          motorPowerNet = 0;
        analogWrite(enableMotor1, motorPowerReverse);
        analogWrite(enableMotor2, motorPowerNet);
        analogWrite(enableMotor3, motorPowerNet);
        analogWrite(enableMotor4, motorPowerReverse);
      }
      spinMotors(0,1); // forward
    }
    else
    {
      turnMotorsOff();
    }
  }

  if(States[2] == 0)
  {
    buttonIsPressed = true;
  }
  if(States[2] == 1 && buttonIsPressed == true)
  {
    toggleRobotArm();
    buttonIsPressed = false;
  }
}

void turnMotorsOff()
{
  digitalWrite(enableMotor1, LOW);
  digitalWrite(enableMotor2, LOW);
  digitalWrite(enableMotor3, LOW);
  digitalWrite(enableMotor4, LOW);
}

void toggleRobotArm()
{
  currentArmState = !currentArmState;
  if(currentArmState == 1)
  {
    robotArm.write(85);
  }
  else
  {
    robotArm.write(180);
  }

}

void spinMotors(int boolValOne, int boolValTwo)
{
  digitalWrite(rotate1CounterClockWise, boolValOne);
  digitalWrite(rotate1ClockWise, boolValTwo);
  digitalWrite(rotate2CounterClockWise, boolValOne);
  digitalWrite(rotate2ClockWise, boolValTwo);
  digitalWrite(rotate3CounterClockWise, boolValOne);
  digitalWrite(rotate3ClockWise, boolValTwo);
  digitalWrite(rotate4CounterClockWise, boolValOne);
  digitalWrite(rotate4ClockWise, boolValTwo);
}