//RC Tank Code 

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

Servo myServo;
int In1 = 2;
int In2 = 3;
int ENA = 5;
int In3 = 4;
int In4 = 7;
int ENB = 6;

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

char receivedData[32] = "";
int LXJ, LYJ, RYJ;

int motorSpeedA, motorSpeedB = 0, diff = 0;
 
void setup() {
  myServo.attach(10);

  pinMode(In1, OUTPUT);
  pinMode(In2, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(In3, OUTPUT);
  pinMode(In4, OUTPUT);
  pinMode(ENB, OUTPUT);
  
  Serial.begin(9600);
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();

}

void loop() {
  if (radio.available()){
    radio.read(&receivedData, sizeof(receivedData)); // Read the data and put it into character array
    LXJ = atoi(&receivedData[0]); // Convert the data from the character array (received LXJ value) into integer
    delay(10);

    radio.read(&receivedData, sizeof(receivedData));
    LYJ = atoi(&receivedData[0]);
    delay(10);

    radio.read(&receivedData, sizeof(receivedData));
    RYJ = atoi(&receivedData[0]);
    delay(10);    
  }

//Output to Servo motor
  int angelV = map(RYJ, 0, 1023, 5, 170);
  myServo.write(angelV);

//Make Tank move Forward
  if (LXJ < 470) {
    digitalWrite(In1, HIGH); //red OUT1
    digitalWrite(In2, LOW);  //black OUT2
    digitalWrite(In3, LOW);  //red OUT3
    digitalWrite(In4, HIGH); //black OUT4

    motorSpeedA = motorSpeedB = map(LXJ, 470, 0, 0, 255);
  }
  
//Make Tank move Backward  
  else if (LXJ > 550) {
    digitalWrite(In1, LOW);
    digitalWrite(In2, HIGH);
    digitalWrite(In3, HIGH);
    digitalWrite(In4, LOW);

    motorSpeedA = motorSpeedB = map(LXJ, 550, 1023, 0, 255); 
  }
  
//When Joystick is not moving
  else {
    motorSpeedA = motorSpeedB = diff = 0;
  }
  
//Make Tank turn Right by differential
  if (LYJ < 470) {
    diff = map(LYJ, 470, 0, 0, 255);
    motorSpeedA = motorSpeedA + diff;
    motorSpeedB = motorSpeedB - diff;
    
    if (motorSpeedA > 255) {
      motorSpeedA = 180;
    }
    if (motorSpeedB < 0) {
      motorSpeedB = 0;
    }

  }

//Make Tank turn Left by differential
  if (LYJ > 550) {
    diff = map(LYJ, 550, 1023, 0, 255);
    motorSpeedB = motorSpeedB + diff;
    motorSpeedA = motorSpeedA - diff;

    if (motorSpeedB > 255) {
      motorSpeedB = 180;
    }
    if (motorSpeedA < 0) {
      motorSpeedA = 0;
    }
    
  }
  
//Make Tank turn Right on its own center  
  if (LYJ < 470 && 470 < LXJ && LXJ < 550) {
    digitalWrite(In1, HIGH);
    digitalWrite(In2, LOW);
    digitalWrite(In3, HIGH);
    digitalWrite(In4, LOW);

    motorSpeedA = motorSpeedB = map(LYJ, 470, 0, 0, 255);    
  }

//Make Tank turn Left on its own center
  if (LYJ > 550 && 470 < LXJ && LXJ < 550) {
    digitalWrite(In1, LOW);
    digitalWrite(In2, HIGH);
    digitalWrite(In3, LOW);
    digitalWrite(In4, HIGH);

    motorSpeedA = motorSpeedB = map(LYJ, 550, 1023, 0, 255); 
  }

//get rid of the Buzzing at low speed (Adjust according to your motors)
  if (motorSpeedA < 140) {
    motorSpeedA = 0;
  }
  if (motorSpeedB < 140) {
    motorSpeedB = 0;
  }
  
  analogWrite(ENA, motorSpeedA);
  analogWrite(ENB, motorSpeedB); 

  Serial.print("LXJ: ");
  Serial.print(LXJ);
  Serial.print(" | LYJ: ");
  Serial.print(LYJ);
  Serial.print(" | RYJ: ");
  Serial.print(RYJ);
  Serial.print(" || motorSpeedA: ");
  Serial.print(motorSpeedA);
  Serial.print(" | motorSpeedB: ");
  Serial.print(motorSpeedB);
  Serial.print(" | diff: ");
  Serial.println(diff);

}