Xbox Motor Driver

/*
 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);

//Left Motor
int PWMPin = 6;
int PWMPin2 = 11;

//Right Motor
int PWMPin3 = 3;
int PWMPin4 = 5;

int wheelLock = 14;

int enable1 = 15;
int enable2 = 16;
int enable3 = 17;
int enable4 = 18;

int wheelLockEnable = 0;

int ControllerConnected = A5;

void setup() {
  Serial.begin(115200);
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); //halt
  }
  Serial.print(F("\r\nXbox Wireless Receiver Library Started"));

  pinMode(PWMPin, OUTPUT);
  pinMode(PWMPin2, OUTPUT);
  pinMode(PWMPin3, OUTPUT);
  pinMode(PWMPin4, OUTPUT);

  pinMode(enable1, OUTPUT);
  pinMode(enable2, OUTPUT);
  pinMode(enable3, OUTPUT);
  pinMode(enable4, OUTPUT);

  pinMode(wheelLock, INPUT);

  pinMode(ControllerConnected, OUTPUT);
}
void loop() {
  Usb.Task();
  if (Xbox.XboxReceiverConnected) {
    for (uint8_t i = 0; i < 4; i++) {
      if (Xbox.Xbox360Connected[i]) {

        digitalWrite(ControllerConnected, HIGH); // light up an LED showing the controller is connected
        //the LED responds appropriately, but changes in bightess once controller data is received

        if (Xbox.getButtonPress(L2, i) || Xbox.getButtonPress(R2, i)) {
          Serial.print("L2: ");
          Serial.print(Xbox.getButtonPress(L2, i));
          Serial.print("\tR2: ");
          Serial.println(Xbox.getButtonPress(R2, i));
          //Xbox.setRumbleOn(Xbox.getButtonPress(L2, i), Xbox.getButtonPress(R2, i), i);
        }

        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 || Xbox.getAnalogHat(LeftHatX, i) < -7500) {
            Serial.print(F("LeftHatX: "));
            Serial.print(Xbox.getAnalogHat(LeftHatX, i));
            Serial.print("\t");
          }
          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 || 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)) {
          Xbox.setLedOn(LED1, i);
          Serial.println(F("Up"));
        }
        if (Xbox.getButtonClick(DOWN, i)) {
          Xbox.setLedOn(LED4, i);
          Serial.println(F("Down"));
        }
        if (Xbox.getButtonClick(LEFT, i)) {
          Xbox.setLedOn(LED3, i);
          Serial.println(F("Left"));
        }
        if (Xbox.getButtonClick(RIGHT, i)) {
          Xbox.setLedOn(LED2, i);
          Serial.println(F("Right"));
        }

        if (Xbox.getButtonClick(START, i)) {
          Xbox.setLedMode(ALTERNATING, i);
          Serial.println(F("Start"));
        }
        if (Xbox.getButtonClick(BACK, i)) {
          Xbox.setLedBlink(ALL, 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.print(F("Xbox (Battery: "));
          Serial.print(Xbox.getBatteryLevel(i)); // The battery level in the range 0-3
          Serial.println(F(")"));
        }
        if (Xbox.getButtonClick(SYNC, i)) {
          Serial.println(F("Sync"));
          Xbox.disconnect(i);
        }

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

  //Code used for motor Drivers. 4 Enable pins and 2 drive functions

  wheelLockEnable = digitalRead(wheelLock);

  if (wheelLockEnable == HIGH) {

    digitalWrite(enable1, HIGH);
    digitalWrite(enable2, HIGH);
    digitalWrite(enable3, HIGH);
    digitalWrite(enable4, HIGH);
    //turns ON individual H-Bridge chips
                                            //this chunk needs to run when the controller IS connected
        if (!Xbox.getButtonPress(Y)) {
          ForwardTankDrive ();
        }

        if(Xbox.getButtonPress(Y)) {
          ReverseTankDrive ();
        }

        }//Wheel lock enable
      }//end controller connected

      else {
        digitalWrite(enable1, LOW);
        digitalWrite(enable2, LOW);
        digitalWrite(enable3, LOW);    //these lines need to be written to LOW when the controller is not connected
        digitalWrite(enable4, LOW);

        digitalWrite(ControllerConnected, LOW);
        //without these write statements, my HIGH values are 5 volts
        //while these are here, the HIGH values are only 0.6 volts

      }//end else for when controller is not connected
    }//end for
  }//end receiver
}//end loop

void ForwardTankDrive () {

    analogWrite(PWMPin2, 0);
    analogWrite(PWMPin, Xbox.getButtonPress(R2));
    //left trigger

    analogWrite(PWMPin4, 0);
    analogWrite(PWMPin3, Xbox.getButtonPress(L2));
    //right trigger
}

void ReverseTankDrive () {

    analogWrite(PWMPin, 0);
    analogWrite(PWMPin2, Xbox.getButtonPress(R2));
    //left trigger

    analogWrite(PWMPin3, 0);
    analogWrite(PWMPin4, Xbox.getButtonPress(L2));
    //right trigger
}