BluetoothStateMachine.cpp

uint8_t byteRead;
uint8_t byteIndex;
uint8_t checksum;
uint8_t buttonNum;
bool buttonVal;
bool validState;

void loop() {
  t = millis();
  if (ble.available()) {
    validState = false;
    byteRead = ble.read();
    if (byteIndex == 0 && byteRead == '!') {
      validState = true;
    } else if (byteIndex == 1 && byteRead == 'B') {
      validState = true;
    } else if (byteIndex == 2) {
      validState = true;
      buttonNum = byteRead - '0';
    } else if (byteIndex == 3) {
      validState = true;
      buttonVal = byteRead - '0';
    }

    if (byteIndex <= 3 && validState) {
      byteIndex++;
      checksum += byteRead;
      return;
    } else if (byteIndex == 4) {
      checksum = ~checksum;
      if (checksum != byteRead) {
        Serial.print("Checksum mismatch. Received: ");
        Serial.print(checksum);
        Serial.print(" Expected: ");
        Serial.println(byteRead);
        return;
      }

      checksum = byteIndex = 0;
    }

    Serial.print((char) (buttonNum + '0'));
    Serial.print(": ");
    Serial.println(buttonVal);
    Serial.print("This took: ");
    Serial.print(millis() - t);
    Serial.println(" mS");
  }
}