Blimp DIY

#include <SPI.h>
#include <AIR430BoostFCC.h>
#include "Energia.h"
#include "Screen_HX8353E.h"
Screen_HX8353E myScreen;

const int joyX = A5;
const int joyY = A3;
uint16_t colour = redColour;
#define ANALOG_HALFVCC_INPUT A11
// -----------------------------------------------------------------------------
/**
 *  Global data
 */

// Data to write to radio TX FIFO (60 bytes MAX.)
//unsigned char txData[6] = { 'S', '\0', '\0', '\0', '\0', '\0' };

struct sPacket
{
  uint8_t from;
  int rud;
  int Dir;
  int lPWM;
  int rPWM;
  int motor;
};

struct sPacket txData;

struct rPacket
{
  uint8_t from;
  int vcc;
};

struct rPacket rxData;

int analogTmp = 0; //temporary variable to store 
int throttle, direct = 0; //throttle (Y axis) and direction (X axis) 

int leftMotor,leftMotorScaled = 0; //left Motor helper variables
float leftMotorScale = 0;

int rightMotor,rightMotorScaled = 0; //right Motor helper variables
float rightMotorScale = 0;

float maxMotorScale = 0; //holds the mixed output scaling factor

int deadZone = 10; //jostick dead zone 

int btn1 = 0;
int btn2 = 0;
int btn3 = 0;
int rud;
int rssi = 0;

int getVCC() {
  // start with the 1.5V internal reference
  analogReference(INTERNAL1V5);
  int data = analogRead(ANALOG_HALFVCC_INPUT);
  // if overflow, VCC is > 3V, switch to the 2.5V reference
  if (data==0xfff) {
    analogReference(INTERNAL2V5);
    data = (int)map(analogRead(ANALOG_HALFVCC_INPUT), 0, 4096, 0, 5000);
  } else {
    data = (int)map(data, 0, 4096, 0, 3000);
  }
  analogReference(DEFAULT);
  return data;  
}

//-----------------------------------------------------------------------------
// Debug print functions

void printTxData()
{
  Serial.print("Rud, Dir, leftPWM, rightPWM, Payl:  ");
  Serial.print(txData.rud); 
  Serial.print(", ");
  Serial.print(txData.Dir);
  Serial.print(", ");
  Serial.print(txData.lPWM);
  Serial.print(", ");
  Serial.print(txData.rPWM);
  Serial.print(", ");
  Serial.println(txData.motor);
}

void printRxData()
{
  Serial.print("RxVcc, rssi:  ");
  Serial.print(rxData.vcc); 
  Serial.print(", ");
  Serial.println(rssi);
}

// -----------------------------------------------------------------------------
// Main example

void setup()
{
  Serial.begin(19200);
  myScreen.begin();
  
  pinMode(33, INPUT_PULLUP);  //configure motor tilt switches
  pinMode(32, INPUT_PULLUP);
  pinMode(P2_1, INPUT_PULLUP); //Pullup for LP S1 and S2
  pinMode(P1_1, INPUT_PULLUP);
  pinMode(5, INPUT_PULLUP);   //Pullup for thumbstick button (payload)
  
  // The radio library uses the SPI library internally, this call initializes
  // SPI/CSn and GDO0 lines. Also setup initial address, channel, and TX power.
  Radio.begin(0x04, CHANNEL_2, POWER_MAX);
  
  delay(100);
  
  txData.from = 0x04;
}

void loop()
{
   //aquire the analog input for Y  and rescale the 0..1023 range to -255..255 range
  analogTmp = analogRead(joyY);
  throttle = -(2048-analogTmp)/8;

  delayMicroseconds(100);
  //...and  the same for X axis
  analogTmp = analogRead(joyX);
  direct = -(2048-analogTmp)/8;
  rud = map(analogTmp, 0,4096,25,155);
  
  //mix throttle and direction
  if(throttle >= -8){
    leftMotor = throttle+direct;
    rightMotor = throttle-direct;
  }
  else{
    leftMotor = throttle-direct;
    rightMotor = throttle+direct;
  }

  //calculate the scale of the results in comparision base 8 bit PWM resolution
  leftMotorScale =  leftMotor/255.0;
  leftMotorScale = abs(leftMotorScale);
  rightMotorScale =  rightMotor/255.0;
  rightMotorScale = abs(rightMotorScale);

  //choose the max scale value if it is above 1
  maxMotorScale = max(leftMotorScale,rightMotorScale);
  maxMotorScale = max(1,maxMotorScale);

  //and apply it to the mixed values
  leftMotorScaled = constrain(leftMotor/maxMotorScale,-255,255);
  rightMotorScaled = constrain(rightMotor/maxMotorScale,-255,255);
  
    //Read button values for motor tilt and payload dropper
    btn1 = digitalRead(32);   //read push button to move motor angle
    btn2 = digitalRead(33);   //read push button to move motor angle
    btn3 = digitalRead(5);    //read push button to activate payload 
    
    if(btn2 == 0){    //increase motor angle
      txData.motor = 1;
    }
    else if(btn1 == 0){  //decrease motor angle
      txData.motor = 2;
    }
    else if(btn3 == 0){   //payload dropper
      txData.motor = 3;
    }
    else{
      txData.motor = 0;
    }

  // Store data in array for transmission
    txData.rud = rud;    
    txData.lPWM = leftMotorScaled;
    txData.rPWM = rightMotorScaled;
    
    // Load the txData into the radio TX FIFO and transmit it to the broadcast
    // address.
    printTxData();                    // TX debug information 
    Radio.transmit(0x03, (unsigned char*)&txData, sizeof(txData));
    
    while (Radio.busy());
    
    if (Radio.receiverOn((unsigned char*)&rxData, sizeof(rxData), 1000) > 0)
    {
      if(rxData.from == 0x03)
      {
        int data = getVCC();
        rssi = Radio.getRssi();
        printRxData();
        myScreen.gText(0, myScreen.screenSizeY()-myScreen.fontSizeY(),
                     "Left=" + i32toa((int16_t)leftMotorScaled, 1, 0, 4),
                     colour);
        myScreen.gText(0, myScreen.screenSizeY()-2*myScreen.fontSizeY(),
                     "Right=" + i32toa((int16_t)rightMotorScaled, 1, 0, 4),
                     colour);
        myScreen.gText(0, myScreen.screenSizeY()-3*myScreen.fontSizeY(),
                     "TX Batt= " + i32toa((int16_t)data, 1000, 3, 5) + "V",
                     colour);
        myScreen.gText(0, myScreen.screenSizeY()-4*myScreen.fontSizeY(),
                     "RX Batt= " + i32toa((int16_t)rxData.vcc, 1000, 3, 5) + "V" ,
                     colour);
        myScreen.gText(0, myScreen.screenSizeY()-5*myScreen.fontSizeY(),
                     "RSSI=" + i32toa((int16_t)rssi, 1, 0, 5) + "dBm",
                     colour);
      }
    }
    
  //To do: throttle change limiting, to avoid radical changes of direction for large DC motors

  delay(10);

}