SMARS - receiver

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <NewPing.h>

#define TRIGGER_PIN 2     // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 10       // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200  // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);  // NewPing setup of pins and maximum distance.

int in1 = 3;  //Motor Input 1
int in2 = 4;  //Motor Input 2
int in3 = 5;  //Motor Input 1
int in4 = 6;  //Motor Input 2

int Array[3];

bool AutoDrive = 0;

unsigned int ping = 0;

unsigned long lastFeedbackTime = 0;
unsigned long delayTime = 750;

RF24 radio(8, 9);
const byte address[6] = "00001";

const int deadZone = 5;
int xAxis, yAxis, Btn;
int baseSpeed, turnSpeed;
String direction;
String yaw;

void setup() {
  //set motor pins to output
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  Serial.begin(9600);

  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();

  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  lastFeedbackTime = millis();

  Serial.println("It has begun");
}

void loop() {

  delay(20);
  ping = sonar.ping_cm();

  if (radio.available()) {

    radio.read(&Array, sizeof(Array));

    xAxis = Array[0];
    yAxis = Array[1];
    Btn = Array[2];
  }

  drive();

  if ((millis() - lastFeedbackTime) > delayTime) {
    Serial.print("Btn: ");
    Serial.print(Array[2]);
    Serial.print(", ");

    Serial.println(ping);
    lastFeedbackTime = millis();
    //feedback();
  }

  // if (Btn == 1) {
  //   AutoDrive = !AutoDrive;
  // }

  // if (AutoDrive == 1) {
  //   AutomatedDriving();
  // } else {
  //   drive();
  // }
}

void feedback() {
  Serial.print("X: ");
  Serial.print(Array[0]);
  Serial.print(", ");
  Serial.print("Y: ");
  Serial.print(Array[1]);
  Serial.print(", ");
  Serial.print("Btn: ");
  Serial.print(Array[2]);
  Serial.print(", ");
  Serial.println(AutoDrive);
}

void drive() {
  if (xAxis > 522) {
    direction = "forward";
    baseSpeed = map(xAxis, 522, 1023, 0, 255);
  } else if (xAxis < 510) {
    direction = "backwards";
    baseSpeed = map(xAxis, 0, 510, 255, 0);
  } else {
    direction = "hold";
    baseSpeed = 0;
  }

  if (yAxis > 522) {
    yaw = "right";
    turnSpeed = map(yAxis, 522, 1023, 0, 225);
  } else if (yAxis < 510) {
    yaw = "left";
    turnSpeed = map(yAxis, 0, 510, 225, 0);
  } else {
    yaw = "hold";
  }

  if (direction == "forward") {
    if (yaw == "left") {
      int motor1Speed = baseSpeed + turnSpeed;
      int motor2Speed = baseSpeed - turnSpeed;
      analogWrite(in1, motor1Speed);
      analogWrite(in2, 0);
      analogWrite(in3, motor2Speed);
      analogWrite(in4, 0);
    } else if (yaw == "right") {
      int motor1Speed = baseSpeed - turnSpeed;
      int motor2Speed = baseSpeed + turnSpeed;
      analogWrite(in1, motor1Speed);
      analogWrite(in2, 0);
      analogWrite(in3, motor2Speed);
      analogWrite(in4, 0);
    } else if (yaw == "hold") {
      analogWrite(in1, baseSpeed);
      analogWrite(in2, 0);
      analogWrite(in3, baseSpeed);
      analogWrite(in4, 0);
    }
  } else if (direction == "backwards") {
    if (yaw == "left") {
      int motor1Speed = baseSpeed + turnSpeed;
      int motor2Speed = baseSpeed - turnSpeed;
      analogWrite(in1, 0);
      analogWrite(in2, motor1Speed);
      analogWrite(in3, 0);
      analogWrite(in4, motor2Speed);
    } else if (yaw == "right") {
      int motor1Speed = baseSpeed - turnSpeed;
      int motor2Speed = baseSpeed + turnSpeed;
      analogWrite(in1, 0);
      analogWrite(in2, motor1Speed);
      analogWrite(in3, 0);
      analogWrite(in4, motor2Speed);
    } else if (yaw == "hold") {
      analogWrite(in1, 0);
      analogWrite(in2, baseSpeed);
      analogWrite(in3, 0);
      analogWrite(in4, baseSpeed);
    }
  } else if (direction == "hold") {
    if (yaw == "left") {
      analogWrite(in1, turnSpeed);
      analogWrite(in2, 0);
      analogWrite(in3, 0);
      analogWrite(in4, turnSpeed);
    } else if (yaw == "right") {
      analogWrite(in1, 0);
      analogWrite(in2, turnSpeed);
      analogWrite(in3, turnSpeed);
      analogWrite(in4, 0);
    } else if (yaw == "hold") {
      analogWrite(in1, 0);
      analogWrite(in2, 0);
      analogWrite(in3, 0);
      analogWrite(in4, 0);
    }
  }
}

void AutomatedDriving() {
  delay(5);  // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  // Serial.print("Ping: ");
  // Serial.print(sonar.ping_cm());  // Send ping, get distance in cm and print result (0 = outside set distance range)
  // Serial.println("cm");

  int ping = sonar.ping_cm();
  Serial.println(ping);

  if (ping < 8) {
    Serial.println("Collision Imminent! Take evasive action!");
    //Back
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    delay(500);

    //Right
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    delay(350);
  } else {
    Serial.println("Steady as she goes...");
    //Forward
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
}

void Sonartest() {
  delay(5);
  ping = sonar.ping_cm();
  Serial.println(ping);
}