Teensy and Ardunio 2 Way Communication

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



#define CE_PIN   9    //4 for nano
#define CSN_PIN 10    //2 for nano

//Radio variables declaration

const byte slaveAddress[5] = {'R', 'x', 'A', 'A', 'A'};

RF24 radio(CE_PIN, CSN_PIN); // Create a Radio

struct Package
{
  int8_t   thrust = 0;
  int8_t   x = 0;
  int8_t   y = 0;
  int8_t   z = 0;
};

struct Sent_1
{
  int8_t   gyro_x = 0;
  int8_t   gyro_y = 0;
  int8_t   gyro_z = 0;
  int8_t   Front_Left;
  int8_t   Front_Right;
  int8_t   Rear_Left;
  int8_t   Rear_Right;
};

Package package;
Sent_1 sent_1;

bool newData = false;

long unsigned int current_time = 0;
const int signal_confimation_rate = 1000;          //how often to send a radio confimation signal
long unsigned int signal_confimation_time = 1000;  //counts how often this signal is sent

//Joystick variables declaration

//Input pins
const int XL_pin = 0; // analog pin connected to XL output
const int YL_pin = 1; // analog pin connected to YL output
const int XR_pin = 2; // analog pin connected to XR output
const int YR_pin = 3; // analog pin connected to YR output

//Pot values
float xr, yr;
float xl, yl;
float thrustRate;

//Raw pot values
float xr_raw, yr_raw;
float xl_raw, yl_raw;

//Smoothing Vars
const int numReadings = 20;
int readIndex = 0;              // the index of the current reading

float readings_xr[numReadings];      // the readings from the analog input
float total_xr = 0;                  // the running total
float average_xr = 0;                // the average

float readings_yr[numReadings];      // the readings from the analog input
float total_yr = 0;                  // the running total
float average_yr = 0;                // the average

float readings_xl[numReadings];      // the readings from the analog input
float total_xl = 0;                  // the running total
float average_xl = 0;                // the average

//Controller Values
int xr_mid;
int yr_mid;
int xl_mid;
int yl_mid;
//Controller angle setting
int max_tilt = 10;
int min_tilt = 1;
//Values for linearly mapping pot values to X,Y target angles

void setup()
{
  Serial.begin(115200);
  delay(500);
  Serial.println("Serial \t OK");

  radio.begin();
  radio.setDataRate( RF24_250KBPS );
  radio.enableAckPayload();
  radio.setRetries(5, 5); // delay, count
  // 5 gives a 1500 µsec delay which is needed for a 32 byte ackPayload
  radio.openWritingPipe(slaveAddress);
  Serial.println("Radio \t OK");
  delay(500);

  xr_mid = analogRead(XR_pin);
  yr_mid = analogRead(YR_pin);
  xl_mid = analogRead(XL_pin);
  yl_mid = analogRead(YL_pin);
}

void loop() {

  //Reading Joystick
  processingJoyStick();

  //Packing data
  package.y = -average_yr;
  package.x = -average_xr;
  package.z = -average_xl;

  //Thrust rate is a stationary value
  thrustRate = yl;
  package.thrust = package.thrust + thrustRate;

  if (package.thrust >= 1800) {
    package.thrust = 1800;
  }
  if (package.thrust <= 1000) {
    package.thrust = 1000;
  }

  send();

  printPackage();
}

void processingJoyStick()
{
  xr_raw = analogRead(XR_pin);
  yr_raw = analogRead(YR_pin);
  xl_raw = analogRead(XL_pin);
  yl_raw = analogRead(YL_pin);

  if (xr_raw > (xr_mid + 50)) {
    xr = (0.02 * xr_raw) - 10;
  }
  if (xr_raw < (xr_mid - 50)) {
    xr = (0.02 * xr_raw) - 10;
  }
  if (xr_raw > 471 && xr_raw < 569) {
    xr = 0;
  }
  if (yr_raw > 570) {
    yr = (0.02 * yr_raw) - 10;
  }
  if (yr_raw < 470) {
    yr = (0.02 * yr_raw) - 10;
  }
  if (yr_raw > 471 && yr_raw < 569) {
    yr = 0;
  }
  if (xl_raw > 570) {
    xl = (0.02 * xl_raw) - 10;
  }
  if (xl_raw < 470) {
    xl = (0.02 * xl_raw) - 10;
  }
  if (xl_raw > 471 && xl_raw < 569) {
    xl = 0;
  }
  if (yl_raw > 570) {
    yl = (-0.02 * yl_raw) + 10;
  }
  if (yl_raw < 400) {
    yl = (-0.02 * yl_raw) + 10;
  }
  if (yl_raw > 401 && yl_raw < 569) {
    yl = 0;
  }

  //Control values must be smoothed or they cause the PDI control system to spaz out when it receives a 10 straight away
  //Smoothing XR values
  total_xr  = total_xr  - readings_xr[readIndex];
  // read from the sensor:
  readings_xr[readIndex] = xr;
  // add the reading to the total:
  total_xr  = total_xr  + readings_xr[readIndex];

  //Smoothing YR values
  total_yr  = total_yr  - readings_yr[readIndex];
  // read from the sensor:
  readings_yr[readIndex] = yr;
  // add the reading to the total:
  total_yr  = total_yr  + readings_yr[readIndex];

  //Smoothing XL values
  total_xl  = total_xl  - readings_xl[readIndex];
  // read from the sensor:
  readings_xl[readIndex] = xl;
  // add the reading to the total:
  total_xl  = total_xl  + readings_xl[readIndex];
  // advance to the next position in the array:
  readIndex = readIndex + 1;

  // if we're at the end of the array...
  if (readIndex >= numReadings) {
    // ...wrap around to the beginning:
    readIndex = 0;
  }

  // calculate the xr average:
  average_xr = total_xr  / numReadings;

  // calculate the yr average:
  average_yr = total_yr  / numReadings;

  // calculate the xl average:
  average_xl = total_xl  / numReadings;

}

void printPackage()
{
  Serial.print(sent_1.gyro_x);
  Serial.print("\t");
  Serial.print(sent_1.gyro_y);
  Serial.print("\t");
  Serial.print(sent_1.gyro_z);
  Serial.print("\t");
  Serial.print(package.x);
  Serial.print("\t");
  Serial.print(package.y);
  Serial.print("\t");
  Serial.print(package.z);


  Serial.println("  ");

}

void send() {

  bool rslt;
  rslt = radio.write( &package, sizeof(package) );
  // Always use sizeof() as it gives the size as the number of bytes.
  // For example if dataToSend was an int sizeof() would correctly return 2

  if (rslt) {
    if ( radio.isAckPayloadAvailable() ) {
      radio.read(&sent_1, sizeof(sent_1));
      newData = true;
    }
    else {
      Serial.println("  Acknowledge but no data ");
    }
  }
  else {
    Serial.println("  Tx failed");
  }
}