Xbox controller sketch (частично работает)

#include <XBOXRECV.h>

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

USB Usb;
XBOXRECV Xbox(&Usb);

int motorPin = 3;
int motorPin1 = 5;
int reversePin = 4;
int reversePin1 = 6;

int motorSpeed;
int reverse;
bool is_stick;

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


void loop() {
  Usb.Task();
  if (Xbox.XboxReceiverConnected) {
    for (uint8_t i = 0; i < 4; i++) {

      is_stick = false;

      if (Xbox.Xbox360Connected[i]) {

          if (Xbox.getAnalogHat(LeftHatY, i) > 7500 || Xbox.getAnalogHat(LeftHatY, i) < -7500) {

            if(Xbox.getAnalogHat(LeftHatY, i) > 7500){
              // Если стик тянем вперед
              digitalWrite(reversePin, HIGH);
              analogWrite(motorPin, motorSpeed-255);
            }

            if(Xbox.getAnalogHat(LeftHatY, i) < -7500){
              // Если стик тянем назад
              digitalWrite(reversePin, LOW);
              analogWrite(motorPin, motorSpeed);
            }

            is_stick = true;

            Serial.print(F("LeftHatY: "));
            Serial.print(Xbox.getAnalogHat(LeftHatY, i));
            Serial.print("\t");
            Serial.println();
          }

        if (Xbox.getButtonClick(A, i)){
          Serial.println(F("A"));
          // Если меняем скорость
          if(motorSpeed == 255){
            motorSpeed = 127;
          }else if(motorSpeed == 127){
            motorSpeed = 255;
          }

        }

      }

      if(!is_stick){
        analogWrite(motorPin, 0);
      }
    }
  }
}