PS2 Arduino radio controller

#include <PS2X_lib.h>

#include <SPI.h>
#include <Mirf.h>
#include <nRF24L01.h>
#include <MirfHardwareSpiDriver.h>

#define PS2_DAT        4
#define PS2_CMD        3
#define PS2_SEL        2
#define PS2_CLK        5

#define MY_LED 6
#define SPEAKER 9

PS2X ps2x; // create PS2 Controller Class

uint8_t tx_address[5] = {0xE7, 0xE7, 0xE7, 0xE7, 0xE7};
uint8_t rx_address[5] = {0xD7, 0xD7, 0xD7, 0xD7, 0xD7};

#define toggle_pin(port, pin) port^=(1<<pin)

struct radio_packet
{
  uint8_t buttons1;
  uint8_t buttons2;
  uint8_t leftX;
  uint8_t leftY;
  uint8_t rightX;
  uint8_t rightY;
}radioData;

enum connection_state {WAITING=0, POOR=1, GOOD=2};

void playMelody(void)
{

  for (uint8_t i=10;i>0;i--)
  {
  // pin, freq (Hz), duration (ms)
    tone(SPEAKER, 100+(uint16_t)i*50,100);
    delay(100);
  }
}

void ConnectedBeep(void) // This function has the potential to block radio calls if we're moving between poor and good
{                       // threshold every second... Can this be made non-blocking?
  for (uint8_t i=0;i<10;i++)
  {
    tone(SPEAKER, 100+(uint16_t)i*50,50);
    delay(50);
  }
}

void WaitingBeep(void)
{
  tone(SPEAKER, 100, 200);
}

void LowSignalBeep(void)
{
  tone(SPEAKER, 900,200);
}


// the setup function runs once when you press reset or power the board
void setup() {
  Serial.begin(115200);

  Serial.println("Setup radio...");

  Mirf.spi = &MirfHardwareSpi;
  Mirf.init();
  Mirf.payload = sizeof(radio_packet);
  Mirf.channel = 2;
  Mirf.config();

  Mirf.configRegister(EN_AA, (1 << ENAA_P0)| (1 << ENAA_P1));

  Mirf.setTADDR(tx_address);
  Mirf.setRADDR(rx_address);

  Mirf.configRegister(SETUP_RETR, 0b0000 | 0b1111); // MAX auto re-transmit time, max 15 retries (MAX)

// Setup PS2 controller:
  uint8_t error=1, pressures=0,rumble=0;

  Serial.println("Connecting to PS2 controller...");
  while(error==1)
  {
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

  if(error == 0){
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
  if (pressures)
    Serial.println("true ");
  else
    Serial.println("false");
  Serial.print("rumble = ");
  if (rumble)
    Serial.println("true)");
  else
    Serial.println("false");
  }
  else if(error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if(error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if(error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

  } // End while
  Serial.println("DONE");

  uint8_t type = ps2x.readType();
  switch(type) {
    case 0:
      Serial.print("Unknown Controller type found ");
      break;
    case 1:
      Serial.print("DualShock Controller found ");
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
  case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
   }

   pinMode(MY_LED, OUTPUT);
   toggleLED(10, 100);

   playMelody();
}

void toggleLED(uint8_t flashes, uint8_t spacing)
{
  for (uint8_t i=0; i<flashes; i++)
  {
    digitalWrite(MY_LED,HIGH);
    delay(spacing); // in milliseconds
    digitalWrite(MY_LED,LOW);
    delay(spacing); // in milliseconds
  }
}

void flushTx(void)
{
    Mirf.csnLow();                    // Pull down chip select
    Mirf.spi->transfer( FLUSH_TX );     // Write cmd to flush tx fifo
    Mirf.csnHi();                    // Pull up chip select
}

// the loop function runs over and over again forever
void loop() {

  enum connection_state radio_state;
  long mytime=0;
  uint16_t packets_sent=0;

  radio_state = WAITING;

  Mirf.flushRx();
  flushTx();

  mytime = millis();

  while(1)
  {
  ps2x.read_gamepad(false, 0); //read controller and set large motor to spin at 'vibrate' speed

  radioData.buttons1 = ps2x.Button(PSB_L1)<<7 | ps2x.Button(PSB_L2)<<6 | ps2x.Button(PSB_R1) << 5 | ps2x.Button(PSB_R2) << 4 | ps2x.Button(PSB_PAD_UP) << 3 | ps2x.Button(PSB_PAD_RIGHT)<<2 | ps2x.Button(PSB_PAD_DOWN)<<1 | ps2x.Button(PSB_PAD_LEFT);
  radioData.buttons2 = ps2x.Button(PSB_CIRCLE)<<7 | ps2x.Button(PSB_CROSS)<<6 | ps2x.Button(PSB_TRIANGLE)<<5 | ps2x.Button(PSB_SQUARE)<<4 | ps2x.Button(PSB_START) << 2 | ps2x.Button(PSB_SELECT)<<1;
  radioData.leftX = ps2x.Analog(PSS_LX);
  radioData.leftY = ps2x.Analog(PSS_LY);
  radioData.rightX = ps2x.Analog(PSS_RX);
  radioData.rightY = ps2x.Analog(PSS_RY);

  // Send radio data
          Mirf.setTADDR(tx_address);
          Mirf.send((uint8_t *)&radioData);

          //while(Mirf.issending());

          uint8_t status, sending;
          sending=1;

          while(sending)
          {
            status=Mirf.getStatus();
            if (status & ((1 << TX_DS)  | (1 << MAX_RT)) ) sending=0;
          }

          Mirf.isSending(); // Make a call to this so that powerUpRx is called and handled correctly (could also just call powerUpRx ourselves but this feels safer)

         if (status & (1 << TX_DS))
            packets_sent++;

         if (millis()-mytime>=1000)
         {
          Serial.print("Packets sent:"); Serial.println(packets_sent);

          if (packets_sent==0) {WaitingBeep(); radio_state=WAITING;}
          else if (packets_sent<200) {LowSignalBeep();radio_state=POOR;}
          else if (radio_state!=GOOD) {ConnectedBeep();radio_state=GOOD;}

          if (radio_state==GOOD) toggle_pin(PORTD, 6); //pin PD6 - slow flash
          else if (radio_state==WAITING) PORTD |= (1<<6); // set pin high - constant if waiting (plus hear low beep)
          packets_sent=0;
          mytime=millis();
         }

} // end while

} // end loop