#include <Servo.h>
#include <A4988.h>

constexpr uint8_t BTN_GO = 53;
constexpr uint8_t HR_HALL = 49;
constexpr uint8_t SG_HALL = 51;
constexpr uint8_t WG_T = 39;
constexpr uint8_t WG_B = 41;
constexpr uint8_t SL_R = 43;
constexpr uint8_t SL_L = 45;

constexpr uint8_t LED_G = 12;
constexpr uint8_t LED_R = 13;
constexpr uint8_t LED_W = 11;

constexpr uint8_t LEDS[3] = {26, 25, 27};

constexpr bool pressed = LOW;
bool thisT,thisB,thisL,thisR;
bool lastT,lastB,lastL,lastR;

void setup(){
  Serial.begin(9600);
  pinMode(HR_HALL, INPUT);
  pinMode(SG_HALL, INPUT);
  for(int n=0; n<3; n++){
    pinMode(LEDS[n], OUTPUT);
  }
  pinMode(WG_T, INPUT_PULLUP);
  pinMode(WG_B, INPUT_PULLUP);
  pinMode(SL_L, INPUT_PULLUP);
  pinMode(SL_R, INPUT_PULLUP);
  bool lastT = digitalRead(WG_T);
  bool lastB = digitalRead(WG_B);
  bool lastL = digitalRead(SL_L);
  bool lastR = digitalRead(SL_R);
}

void loop(){
  Serial.print("TOP: ");
  Serial.println(!digitalRead(WG_T));
  Serial.print("BOT: ");
  Serial.println(!digitalRead(WG_B));
  Serial.print("LEFT: ");
  Serial.println(!digitalRead(SL_L));
  Serial.print("RIGHT: ");
  Serial.println(!digitalRead(SL_R));
  delay(1000);
}