// NRF24 based radio receiver for radio controlled tractors
// by modelleicher
// NRF24 Library: https://github.com/TMRh20/RF24
// Inspiration by: "RC Tractor Guy" on youtube https://www.youtube.com/user/magicalmachines/videos
// Also great thanks to Antony Cartwright for the best NRF24 tutorial on youtube (bare basics explanation) https://www.youtube.com/watch?v=Qn2YLSYz3WM

// work in progress, just a start for now.
// March 2017



#include <SPI.h>
#include "RF24.h"

#include <Servo.h>

// start of package and end of package symbols
#define SOP '<'
#define EOP '>'

// defining all the pins, change pins here if you use different pinout!
#define PIN_driveMotor_dir1  5
#define PIN_driveMotor_dir2  4
#define PIN_driveMotor_pwm  3

#define PIN_steering_servo  7

#define PIN_lights_lowBeam  8

#define PIN_batt_readVcc  A0

// all the settings for deadzone on motor and limits on servos
#define SET_driveMotor_deadZoneMax  132
#define SET_driveMotor_deadZoneMin  124

// steering trim on Deutz D16006 - min 50 max 120
#define SET_steering_servoMin  50
#define SET_steering_servoMax  120



// right now we are only receiving 6 bytes..
uint8_t dataPackage[7];
// dataPackage[0] = adress
// dataPackage[1] = steering (X Axis)
// dataPackage[2] = lifting arms (Y Axis)
// dataPackage[3] = lifting arms 2 (RX Axis)
// dataPackage[4] = driving (RY Axis)
// dataPackage[5] = 8 Buttons (2 Joystick Buttons and 6 unused)
// dataPackage[6] = 8 more buttons (6 lights/general buttons and 2 unused)

// initialize the RF24
RF24 radio(9, 10);

// again, the pipe. Important, Sender and Receiver have to use the same, obviously.
const uint64_t pipe = 1;

// address
byte address = B00000001;

// battery voltage reading
int batt_internalVCC = 0;
int batt_rawAnalogRead = 0;
int batt_milisecondsLow = 0;

// not used yet
int dt = 0;
int lastMillis = 0;

// initialize the steering servo
Servo servo1;

void setup() {
  // serial for debug
  Serial.begin(9600);
  // begin radio
  radio.begin();
  // no need to set PA level anything higher than min as long as the communication is only one way.
  radio.setPALevel(RF24_PA_MIN);
  // debug
  Serial.println("started..");
  // since we are the receiver, open a reading pipe
  radio.openReadingPipe(1, pipe);
  // debug
  Serial.println("opened..");
  // start listening (now we are a receiver and listening for packets)
  radio.startListening();
  //debug
  Serial.println("listening..");

  // attach the steering servo at pin 7
  servo1.attach(PIN_steering_servo);

  //
  pinMode(PIN_lights_lowBeam, OUTPUT);
  digitalWrite(PIN_lights_lowBeam, LOW);
}

void loop() {
  // not used yet
  dt = millis() - lastMillis;
  lastMillis = millis();

  // check if there are packets received. radio.availiable() will return true if there is anything in the buffer
  if (radio.available())
  {
    //read the data and store in dataPackage array.
    radio.read(dataPackage, sizeof(dataPackage));

    // check if the address matches ours
    if (dataPackage[0] == address)
    {


        // current array index to axis:
        // 1 = X
        // 2 = Y
        // 3 = RX
        // 4 = RY

        // control drive motor
        if (dataPackage[4] > SET_driveMotor_deadZoneMax) // larger than 130 -> forwards
        {
            analogWrite(PIN_driveMotor_pwm, map(dataPackage[4], SET_driveMotor_deadZoneMax, 255, 0, 255)); // write the PWM value
            // set direction:
            digitalWrite(PIN_driveMotor_dir1, HIGH);
            digitalWrite(PIN_driveMotor_dir2, LOW);
        }
        else if (dataPackage[4] < SET_driveMotor_deadZoneMin) // smaller than 120 -> backwards
        {
            analogWrite(PIN_driveMotor_pwm, map(dataPackage[4], SET_driveMotor_deadZoneMin, 0, 0, 255)); // write PWM value
            // set direction:
            digitalWrite(PIN_driveMotor_dir1, LOW);
            digitalWrite(PIN_driveMotor_dir2, HIGH);
        }
        else // stop driving
        {
            analogWrite(PIN_driveMotor_pwm, 0);
            digitalWrite(PIN_driveMotor_dir1, LOW);
            digitalWrite(PIN_driveMotor_dir2, LOW);
        }



        // map the steering value to servo min/max values
        dataPackage[1] = map(dataPackage[1], 0, 255, SET_steering_servoMin, SET_steering_servoMax);
        // set the servo value
        servo1.write(dataPackage[1]);


         // send the data package to the trailer
        sendImplementData(dataPackage);


        // light
        digitalWrite(PIN_lights_lowBeam, bitRead(dataPackage[6], 3));

    // debug
    //Serial.println(dataPackage[1]);
    //Serial.println(dataPackage[2]);
    //Serial.println(dataPackage[3]);
    //Serial.println("----------------------");
    }
  }
  else
  {
     // this is called each time the loop runs and there is no data in the buffer from the NRF24
  }


  // check the battery voltage to not over discharge the lipo cells!
  checkBatteryVoltage(dt);



  // not sure if the dalay is needet.
  delay(10);



}

void checkBatteryVoltage(int dt)
{
         // since LiPo cells can take damage or even explove if over-discharged we need to monitor the voltage
        // some LiPo cells come with protection circuits on the cell itself, but the cheap ones i use don't.
        // also most multiple cell lipos dont. I use a 2S Lipo in this tractor.
        // so we read the current battery voltage with analogRead and a voltage divider since the arduino runs on 3.3V and the LiPo fully charged has 8.4V
        // My Arduino runs on 3.27V usually, that means that our lower limit of 6.6V is reached at a analogRead value of 687
        // I did want to take the internal voltage into account but floating point math isn't that great on the arduino, and since i had a conservative number for the lower limit i think we'll be fine.
        // you need to change this whole thing if you are not running the arduino off of a 3.3V regulator!!!

        //batt_internalVCC = getVCCReading();

        batt_rawAnalogRead = analogRead(PIN_batt_readVcc);
        if (batt_rawAnalogRead <= 687)
        {
             Serial.println(batt_milisecondsLow);
            batt_milisecondsLow = batt_milisecondsLow + dt;
            if (batt_milisecondsLow > 1000) // battery is low for more than 1 second (to avoid shutting off on current spikes)
            {
                goBlinkLowBatteryLights(); // blink the lights really fast to show that the battery is empty
                batt_milisecondsLow = 0; // reset the battery low timer.. So the lights will start again after 1 second until battery is changed
            }
        }
        else
        {
            batt_milisecondsLow = 0;
        }
        //Serial.println(batt_rawAnalogRead);
}

// send Implement data, serial communication by RC-Tractor Guy
void sendImplementData(uint8_t dataPackage[16])
{
  Serial.write(SOP);
  Serial.write(0x01);
  Serial.write(dataPackage[1]);
  Serial.write(dataPackage[2]);
  Serial.write(dataPackage[3]);
  Serial.write(dataPackage[4]);
  Serial.write(dataPackage[5]);
  Serial.write(dataPackage[6]);
  Serial.write(dataPackage[7]);
  Serial.write(dataPackage[8]);
  Serial.write(dataPackage[9]);
  Serial.write(dataPackage[10]);
  Serial.write(EOP);
}

void goBlinkLowBatteryLights()
{

    digitalWrite(PIN_lights_lowBeam, HIGH);
    delay(50);
    digitalWrite(PIN_lights_lowBeam, LOW);
    delay(250);
    digitalWrite(PIN_lights_lowBeam, HIGH);
    delay(50);
    digitalWrite(PIN_lights_lowBeam, LOW);
    delay(250);
}

// from RCTractorGuy Library, not used right now since i can't get the floating point stuff to work properly
long getVCCReading(){
  long result;
  // Read 1.1V reference against AVcc
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  delay(2); // Wait for Vref to settle
  ADCSRA |= _BV(ADSC); // Convert
  while (bit_is_set(ADCSRA,ADSC));
  result = ADCL;
  result |= ADCH<<8;
  result = (1126400L / result); // Back-calculate AVcc in mV
  return result;
}