Wasder Controller programming

wasder.ino

#include "Wire.h"
#include <MPU6050_light.h>
#include <Keyboard.h> 

MPU6050 mpu(Wire);
unsigned long timer = 0;

//led pin-out
int ledFwd = 10, ledLeft = 12, ledRight = 9, ledBkwd = 11;
int emergencyPin = 8;
float thresh = .45;
bool emergency = false;

void allLedsLow(bool val = LOW){
  digitalWrite(ledFwd, val);
  digitalWrite(ledRight, val);
  digitalWrite(ledLeft, val);
  digitalWrite(ledBkwd, val);
}

void setup() {
  //Pin Configuration
  pinMode(ledFwd, OUTPUT);
  pinMode(ledLeft, OUTPUT);
  pinMode(ledRight, OUTPUT);
  pinMode(ledBkwd, OUTPUT);
  pinMode(emergencyPin, INPUT);

  //Keyboard libary startup
  Keyboard.begin();

  Serial.begin(9600);
  Wire.begin();
  
  byte status = mpu.begin();
  Serial.print(F("Controller status: "));
  Serial.println(status);
  while(status!=0){ } // stop everything if could not connect to MPU6050
  
  Serial.println(F("Setting the zero position, do not move the controller :)"));
  delay(1000);
  mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
  mpu.calcOffsets(); // gyro and accelero
  Serial.println("Done calibrating, commencing boot sequence!\n");
  allLedsLow(true);
  delay(800);
  allLedsLow(false);
  delay(500);
  allLedsLow(true);
  delay(800);
  allLedsLow(false);
  Serial.println("Boot sequence complete!\n");
}

void loop() {
  mpu.update();
  float xx = mpu.getAccX(); //pointing down is neg
  float yy = mpu.getAccY(); //roll right is neg
  float zz = mpu.getAccZ(); //turn right is neg

  // // DEBUG INFO
  // Serial.print("      ");
  // Serial.print(xx, 2);
  // Serial.print("      ");
  // Serial.print(yy, 2);
  // Serial.print("      ");
  // Serial.println(zz, 2);

  // // EMERGENCY BTN
  if (digitalRead(emergencyPin) == HIGH) {
    emergency = true;
    Keyboard.releaseAll();
    press("WARNING: needs a reset!");
  }
  if (emergency) {
    allLedsLow(true);
    delay(250);
    allLedsLow(false);
    delay(250);
    return;
  }

  // // READINGS
  if (isThresh(xx)) {
    // press A/D
    if (xx < 0) {
      digitalWrite(ledLeft, HIGH);
      press("A");
      pressKey('A');
    } else {
      digitalWrite(ledRight, HIGH);
      press("D");
      pressKey('D');
    }
  }
  if (isThresh(yy)) {
    // press W/S
    if (yy < 0) {
      digitalWrite(ledFwd, HIGH);
      press("W");
      pressKey('W');
    } else {
      digitalWrite(ledBkwd, HIGH);
      press("S");
      pressKey('S');
    }
  }
  if (isThresh(zz) && !isThresh(xx) && !isThresh(yy)) {
    //unpress all
    press("nada");
    Keyboard.releaseAll();
  }
  Serial.println("      ");
  delay(50);
  allLedsLow();
}

void press(char* c) {
  Serial.print(c);
  Serial.println("      ");
}

void pressKey(char c) {
  Keyboard.press(c);
  Keyboard.releaseAll();
}

bool isThresh(float x) {
  return abs(x) > thresh;
}