xbox

#include "Servo.h"
#include <XBOXRECV.h>
#include <math.h>
#include <arduino.h>
#ifdef dobogusinclude
    #include <spi4teensy3.h>
    #include <SPI.h>
#endif

Servo servoBridge;
// Servo blade;
USB Usb;
XBOXRECV Xbox(&Usb);

// Перечисление пинов

// Пины для колёс
const int motorPin = 3;
const int reversePin = 4;
const int motorPin1 = 5;
const int reversePin1 = 6;

// Пин серво мотора, держащего мост
const int servoBridge_pin = 7;

// Пин серво мотора-лопатки
const int bladePin = 12;

// Пины моторов, которые крутят конвеер
const int conveerReversePin = 9;
const int conveerMotorPin = 8;

enum StickStay{
    POS,
    NON,
    NEG
};

StickStay y;
StickStay x;

// true - нажата, false - не нажата
bool is_y;
bool is_x;
bool is_b;
bool is_a;

// Отвечает за состояние моста
bool bridgeMode;
// Отвечает за состояние лопатки
bool bladeMode;

void setup() {
    // Инициализация моторов
    pinMode(motorPin, OUTPUT);
    pinMode(reversePin, OUTPUT);

    pinMode(motorPin1, OUTPUT);
    pinMode(reversePin1, OUTPUT);

    pinMode(conveerMotorPin, OUTPUT);
    pinMode(conveerReversePin, OUTPUT);

    // Остановка моторов
    motor_stop(motorPin, reversePin);
    motor_stop(motorPin1, reversePin1);
    motor_stop(conveerMotorPin, conveerReversePin);
    motor_stop(bladeMotorPin, bladeReversePin);

    // Инициализация сервомотора
    servoBridge.attach(servoBridge_pin);
    // blade.attach(bladePin);

    // Инициализация моста
    bridgeMode = false;
    servoBridge.write(40);

    // Подключение серийного COM-порта для отладки скетча
    Serial.begin(115200);

    // Проверка на подключение библиотеки
    if (Usb.Init() == -1) {
        Serial.print(F("\r\nOSC did not start"));
        while (1);
    }
    Serial.print(F("\r\nXbox Wireless Receiver Library Started"));
}

void motors(){
    if(y == POS){
        motor_forward(motorPin, reversePin, 250);
        motor_forward(motorPin1, reversePin1, 250);
    }else if(y == NEG){
        motor_back(motorPin, reversePin, 250);
        motor_back(motorPin1, reversePin1, 250);
    }

    if(x == POS){
        motor_forward(motorPin, reversePin, 250);
        motor_back(motorPin1, reversePin1, 250);
    }else if(x == NEG){
        motor_back(motorPin, reversePin, 250);
        motor_forward(motorPin1, reversePin1, 250);
    }

    if(y == NON && x == NON){
        motor_stop(motorPin, reversePin);
        motor_stop(motorPin1, reversePin1);
    }
}

void buttons(){
    if(is_x){
      if(!bridgeMode){
        for(int i = 40; i <= 110; i+=10){
            servoBridge.write(i);
            delay(200);
        }
        bridgeMode = true;
      }else{
        servoBridge.write(40);
        bridgeMode = false;
      }
    }

    if(is_y){
        motor_forward(conveerMotorPin, conveerReversePin, 150);
    }

    if(!is_y){
        motor_stop(conveerMotorPin, conveerReversePin);
    }

    if(is_b){

    }

    if(is_a){

    }

    if(!is_a && !is_b){

    }
}

void loop() {
  Usb.Task();
  if(Xbox.XboxReceiverConnected){

    y = NON;
    x = NON;
    is_x = false;
    is_b = false;
    is_y = false;
    is_a = false;

    // Опрос 4-х ячеек памяти host-shield'а
    for(uint8_t i = 0; i < 4; i++){
        if(Xbox.Xbox360Connected[i]){

            if(Xbox.getAnalogHat(LeftHatY, i) >=  20000){
                y = POS;
            }
            if(Xbox.getAnalogHat(LeftHatY, i) <= -20000){
                y = NEG;
            }
            if(Xbox.getAnalogHat(LeftHatX, i) >=  20000){
                x = POS;
            }
            if(Xbox.getAnalogHat(LeftHatX, i) <= -20000){
                x = NEG;
            }

            if(Xbox.getButtonClick(X, i)){
                is_x = true;
                Serial.println("x presed");
            }

            if(Xbox.getButtonPress(B, i)){
                is_b = true;
                Serial.println("B pressed");
            }

            if(Xbox.getButtonPress(Y, i)){
                is_y = true;
                Serial.println("Y pressed");
            }

            if(Xbox.getButtonPress(A, i)){
                is_a = true;
                Serial.println("A pressed");
            }

        }
    }

    motors();
    buttons();

  }
}


// Функции для работы с моторами
void motor_back(int motorPin, int reversePin, int speed){
  digitalWrite(reversePin, HIGH);
  analogWrite(motorPin, 255-speed);
}

void motor_forward(int motorPin, int reversePin, int speed){
  digitalWrite(reversePin, LOW);
  analogWrite(motorPin, speed);
}

void motor_stop(int motorPin, int reversePin){
  digitalWrite(reversePin, LOW);
  analogWrite(motorPin, 0);
}