XBOX robot control sketch

/*
 Example sketch for the Xbox Wireless Reciver library - developed by Kristian Lauszus
 It supports up to four controllers wirelessly
 For more information see the blog post: http://blog.tkjelectronics.dk/2012/12/xbox-360-receiver-added-to-the-usb-host-library/ or
 send me an e-mail:  kristianl@tkjelectronics.com
 */

#include <XBOXRECV.h>
// Satisfy IDE, which only needs to see the include statment in the ino.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif

USB Usb;
XBOXRECV Xbox(&Usb);

#define motorPinOne 5 //The chosen pin must have PWM
#define motorPinTwo 6 //The chosen pin must have PWM

#define motor2PinOne 4
#define motor2PinTwo 7
#define BRAKE        2

/* Define the rotational speed of the motor. MUST be between 0 and 255. */
int pulse = 0;
int forward = 0;
int brake_on = 0;


void setup() {
  TCCR1B = TCCR1B & 0b11111000 | 0x02;
  Serial.begin(115200);
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while(1); //halt
    motors_stop();
    center();
  }
  Serial.print(F("\r\nXbox Wireless Receiver Library Started"));
  pinMode (motorPinOne, OUTPUT);
  pinMode (motorPinTwo, OUTPUT);
  pinMode (motor2PinOne, OUTPUT);
  pinMode (motor2PinTwo, OUTPUT);
  pinMode (BRAKE, OUTPUT);

  digitalWrite (BRAKE, LOW);
}

void loop() {
  Usb.Task();
  if(Xbox.XboxReceiverConnected)
    {
    for(uint8_t i=0;i<4;i++)
      {
      if(Xbox.getButtonPress(L2, i))
        {
        Serial.print("L2: ");
        Serial.print(Xbox.getButtonPress(L2, i));
        pulse = Xbox.getButtonPress(L2, i);
        forward = 0;
        clockwise();
        }

      if(Xbox.getButtonPress(R2, i))
        {
        Serial.print("R2: ");
        Serial.println(Xbox.getButtonPress(R2, i));
        pulse = Xbox.getButtonPress(R2, i);
        forward = 1;
        counterClockwise();
        }

       if(Xbox.getAnalogHat(LeftHatX, i) > 7500 || Xbox.getAnalogHat(LeftHatX, i) < -7500 || Xbox.getAnalogHat(LeftHatY, i) > 7500 || Xbox.getAnalogHat(LeftHatY, i) < -7500 || Xbox.getAnalogHat(RightHatX, i) > 7500 || Xbox.getAnalogHat(RightHatX, i) < -7500 || Xbox.getAnalogHat(RightHatY, i) > 7500 || Xbox.getAnalogHat(RightHatY, i) < -7500)
         {
          if(Xbox.getAnalogHat(LeftHatX, i) > 7500)
            {
            Serial.print(F("LeftHatX: "));
            Serial.print(Xbox.getAnalogHat(LeftHatX, i));
            Serial.print("\t");

            if (Xbox.getAnalogHat(LeftHatX, i) > 26000)
              turn_right();
            else
              center();
            }

        if (Xbox.getAnalogHat(LeftHatX, i) < -7500)
          {
          Serial.print(F("LeftHatX: "));
          Serial.print(Xbox.getAnalogHat(LeftHatX, i));
          Serial.print("\t");

          if (Xbox.getAnalogHat(LeftHatX, i) < -26000)
            turn_left();
          else
            center();
        }

        if(Xbox.getAnalogHat(LeftHatY, i) > 7500 || Xbox.getAnalogHat(LeftHatY, i) < -7500)
          {
          Serial.print(F("LeftHatY: "));
          Serial.print(Xbox.getAnalogHat(LeftHatY, i));
          Serial.print("\t");
          }

        if(Xbox.getAnalogHat(RightHatX, i) > 7500)
          {
          Serial.print(F("RightHatX: "));
          Serial.print(Xbox.getAnalogHat(RightHatX, i));
          Serial.print("\t");
          }

         if (Xbox.getAnalogHat(RightHatX, i) < -7500)
           {
           Serial.print(F("RightHatX: "));
           Serial.print(Xbox.getAnalogHat(RightHatX, i));
           Serial.print("\t");
           }

        if(Xbox.getAnalogHat(RightHatY, i) > 7500 || Xbox.getAnalogHat(RightHatY, i) < -7500) {
          Serial.print(F("RightHatY: "));
          Serial.print(Xbox.getAnalogHat(RightHatY, i));
        }
        Serial.println();
      }

      if(Xbox.getButtonClick(UP, i))
        {
        Serial.println(F("Up"));
        }

      if(Xbox.getButtonClick(DOWN, i))
        {
        Serial.println(F("Down"));
        }

      if(Xbox.getButtonClick(LEFT, i))
        {
        Serial.println(F("Left"));
        }

      if(Xbox.getButtonClick(RIGHT, i))
        {
        Serial.println(F("Right"));
        }

      if(Xbox.getButtonClick(START, i))
        {
        Serial.println(F("Start"));
        }

      if(Xbox.getButtonClick(BACK, i))
        {
        Serial.println(F("Back"));
        }

      if(Xbox.getButtonClick(L3, i))
        Serial.println(F("L3"));

      if(Xbox.getButtonClick(R3, i))
        Serial.println(F("R3"));

      if(Xbox.getButtonClick(L1, i))
        Serial.println(F("L1"));

      if(Xbox.getButtonClick(R1, i))
        Serial.println(F("R1"));

      if(Xbox.getButtonClick(XBOX, i))
        {
        Xbox.setLedMode(ROTATING, i);
        Serial.println(F("Xbox"));
        }

      if(Xbox.getButtonClick(A, i))
        {
        Serial.println(F("A"));
      }

    if(Xbox.getButtonClick(B, i))
      {
      Serial.println(F("B"));
      digitalWrite(BRAKE, HIGH);
      brake_on = 1;
      pulse = 100;

      if (forward)
        clockwise();
      else
        counterClockwise();
      }

    if(Xbox.getButtonClick(X, i))
      Serial.println(F("X"));

    if(Xbox.getButtonClick(Y, i))
      {
      Serial.println(F("Y"));
      motors_stop();
      }
    }
  }
  delay(1);
}

void clockwise()
{
  Serial.print("Rotation is clockwise and speed is ");
  Serial.println(pulse);
  analogWrite(motorPinOne,pulse);   // set leg 1 of the H-bridge low
  analogWrite(motorPinTwo,0);

  if (brake_on)
    {
    delay(600);
    digitalWrite(BRAKE, LOW);
    brake_on = 0;
    pulse = 0;
    }
}

void counterClockwise()
{
  Serial.print("Rotation is counter-clockwise and speed is ");
  Serial.println(pulse);
  analogWrite(motorPinOne,0);   // set leg 1 of the H-bridge low
  analogWrite(motorPinTwo,pulse);

  if (brake_on)
    {
    delay(600);
    digitalWrite(BRAKE, LOW);
    brake_on = 0;
    pulse = 0;
    }


}

void turn_left()
{
  digitalWrite(motor2PinOne,HIGH);   // set leg 1 of the H-bridge low
  digitalWrite(motor2PinTwo,LOW);
}

void turn_right()
{
  digitalWrite(motor2PinOne,LOW);   // set leg 1 of the H-bridge low
  digitalWrite(motor2PinTwo,HIGH);
}

void motors_stop()
{
  analogWrite(motorPinOne,0);
  analogWrite(motorPinTwo,0);
  pulse = 0;
}

void center()
{
  digitalWrite(motor2PinOne, LOW);
  digitalWrite(motor2PinTwo, LOW);
}