V2 W/Calibration

#include <Wire.h>
#include <Adafruit_ADS1X15.h>
#include <Adafruit_SSD1306.h>
#include <BleGamepad.h>
#include <Adafruit_TinyUSB.h>
#include "tusb.h"
#include <EEPROM.h>

// PROGMEM strings
const char gasStr[] PROGMEM = "Gas";
const char brakeStr[] PROGMEM = "Brake";
const char clutchStr[] PROGMEM = "Clutch";
const char* const pedalNames[] PROGMEM = {gasStr, brakeStr, clutchStr};
const char simPedalsStr[] PROGMEM = "Sim Pedals";
const char adcErrorStr[] PROGMEM = "ADC Error";
const char pressPedalStr[] PROGMEM = "Press pedal fully";
const char releasePedalStr[] PROGMEM = "Release pedal";
const char resetToDefaultsStr[] PROGMEM = "to Defaults";

// OLED Config
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ADS1115 Instance
Adafruit_ADS1115 ads;

// BLE Gamepad Instance
BleGamepad bleGamepad("Generic Sim Pedals BLE", "xAI", 100);

// USB HID Instance
Adafruit_USBD_HID usb_hid;

// HID Report Descriptor (3-axis joystick)
static const uint8_t hid_report_descriptor[] = {0x05, 0x01, 0x09, 0x05, 0xA1, 0x01, 0x85, 0x01, 0x09, 0x30, 0x09, 0x31, 0x09, 0x32, 0x15, 0x00, 0x26, 0xFF, 0x7F, 0x75, 0x10, 0x95, 0x03, 0x81, 0x02, 0xC0};

// Pins
#define SDA_PIN 9
#define SCL_PIN 10
#define GAS_BUTTON_PIN 2
#define BRAKE_BUTTON_PIN 3
#define CLUTCH_BUTTON_PIN 4

// Constants
const uint16_t DEFAULT_MIN_VOLT = 500;  // mV
const uint16_t DEFAULT_MAX_VOLT = 4500; // mV
const uint8_t DEAD_ZONE_PCT = 5;        // 5%
const uint16_t CALIBRATION_TICKS = 1000;// 10s @ 10ms/tick
const uint16_t RESET_TICKS = 500;       // 5s @ 10ms/tick
const uint16_t DEBOUNCE_TICKS = 50;     // 0.5s @ 10ms/tick

enum PedalType { PEDAL_GAS = 0, PEDAL_BRAKE = 1, PEDAL_CLUTCH = 2, NUM_PEDALS = 3 };
enum State { NORMAL, CALIBRATING, RESETTING };

// Calibration Values (in mV)
uint16_t minVolts[NUM_PEDALS] = {DEFAULT_MIN_VOLT, DEFAULT_MIN_VOLT, DEFAULT_MIN_VOLT};
uint16_t maxVolts[NUM_PEDALS] = {DEFAULT_MAX_VOLT, DEFAULT_MAX_VOLT, DEFAULT_MAX_VOLT};

// Axis Values
int16_t axisValues[NUM_PEDALS];

// EEPROM Structure (packed to save space)
struct CalibrationData {
  uint16_t minVolts[NUM_PEDALS];
  uint16_t maxVolts[NUM_PEDALS];
  uint16_t checksum;
} __attribute__((packed));

// State Variables
volatile uint8_t buttonFlags = 0;
State currentState = NORMAL;
PedalType activePedal = PEDAL_GAS;
uint16_t stateTicks = 0;
uint16_t minV = 5000, maxV = 0;

void IRAM_ATTR handleButton() {
  static uint32_t lastInterrupt[NUM_PEDALS] = {0};
  uint32_t now = millis();
  uint8_t pins[] = {GAS_BUTTON_PIN, BRAKE_BUTTON_PIN, CLUTCH_BUTTON_PIN};

  for(int i = 0; i < NUM_PEDALS; i++) {
    if(!digitalRead(pins[i]) && (now - lastInterrupt[i] > 50)) {
      buttonFlags |= (1 << i);
      lastInterrupt[i] = now;
    }
  }
}

void displayText(const char* line1, const char* line2 = "") {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println(line1);
  if (line2[0]) display.println(line2);
  display.display();
}

void updateCalibration(PedalType pedal) {
  uint16_t voltage = ads.computeVolts(ads.readADC_SingleEnded(pedal)) * 1000;  // mV
  minV = min(minV, voltage);
  maxV = max(maxV, voltage);
}

void saveCalibration() {
  CalibrationData data;
  memcpy(data.minVolts, minVolts, sizeof(minVolts));
  memcpy(data.maxVolts, maxVolts, sizeof(maxVolts));
  data.checksum = 0;
  const uint8_t* bytes = (const uint8_t*)&data;
  for (int i = 0; i < sizeof(CalibrationData) - sizeof(uint16_t); i++) {
    data.checksum += bytes[i];
  }
  EEPROM.put(0, data);
}

bool loadCalibration() {
  CalibrationData data;
  EEPROM.get(0, data);
  uint16_t checksum = 0;
  const uint8_t* bytes = (const uint8_t*)&data;
  for (int i = 0; i < sizeof(CalibrationData) - sizeof(uint16_t); i++) {
    checksum += bytes[i];
  }
  if (checksum == data.checksum) {
    memcpy(minVolts, data.minVolts, sizeof(minVolts));
    memcpy(maxVolts, data.maxVolts, sizeof(maxVolts));
    return true;
  }
  return false;
}

int16_t applyDeadZone(uint16_t value, uint16_t minV, uint16_t maxV) {
  uint32_t range = maxV - minV;
  uint32_t deadZone = (range * DEAD_ZONE_PCT) / 100;
  uint32_t activeRange = range - (2 * deadZone);

  if (value <= minV + deadZone) return 0;
  if (value >= maxV - deadZone) return 32767;

  return ((value - (minV + deadZone)) * 32767UL) / activeRange;
}

void setup() {
  Wire.begin(SDA_PIN, SCL_PIN);
  pinMode(GAS_BUTTON_PIN, INPUT_PULLUP);
  pinMode(BRAKE_BUTTON_PIN, INPUT_PULLUP);
  pinMode(CLUTCH_BUTTON_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(GAS_BUTTON_PIN), handleButton, FALLING);
  attachInterrupt(digitalPinToInterrupt(BRAKE_BUTTON_PIN), handleButton, FALLING);
  attachInterrupt(digitalPinToInterrupt(CLUTCH_BUTTON_PIN), handleButton, FALLING);

  // Initialize display with timeout
  unsigned long start = millis();
  bool displaySuccess = false;
  while (millis() - start < 1000 && !displaySuccess) {
      displaySuccess = display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
      if (!displaySuccess) {
          delay(10);
      }
  }
  if (!displaySuccess) {
      while (1); // Halt
  }

  // Initialize ADC with timeout
  start = millis();
  bool adcSuccess = false;
  while (millis() - start < 1000 && !adcSuccess) {
      adcSuccess = ads.begin(0x48);
      if (!adcSuccess) {
          delay(10);
      }
  }
  if (!adcSuccess) {
      char buffer[20];
      strcpy_P(buffer, adcErrorStr);
      displayText(buffer);
      while (1);
  }
  ads.setGain(GAIN_TWOTHIRDS);

  char buffer[20];
  strcpy_P(buffer, simPedalsStr);
  displayText(buffer);
  usb_hid.setReportDescriptor(hid_report_descriptor, sizeof(hid_report_descriptor));
  usb_hid.begin();
  bleGamepad.begin();

  if (!loadCalibration()) {
    for (int i = 0; i < NUM_PEDALS; i++) {
      minVolts[i] = DEFAULT_MIN_VOLT;
      maxVolts[i] = DEFAULT_MAX_VOLT;
    }
    saveCalibration();
  }
}

void loop() {
  // Handle state machine
  if (buttonFlags) {
    for (int i = 0; i < NUM_PEDALS; i++) {
      if (buttonFlags & (1 << i)) {
        activePedal = (PedalType)i;
        currentState = CALIBRATING;
        stateTicks = 0;
        minV = 5000;
        maxV = 0;
        buttonFlags &= ~(1 << i);
        char pedalName[10];
        strcpy_P(pedalName, (PGM_P)pgm_read_word(&(pedalNames[i])));
        char buffer[20];
        snprintf(buffer, sizeof(buffer), "%s Calibration", pedalName);
        char buffer2[20];
        strcpy_P(buffer2, pressPedalStr);
        displayText(buffer, buffer2);
        break;
      }
    }
  }

  if (currentState != NORMAL) {
    stateTicks++;
    if (currentState == CALIBRATING) {
      updateCalibration(activePedal);
      if (stateTicks >= CALIBRATION_TICKS) {
        minVolts[activePedal] = minV;
        maxVolts[activePedal] = maxV;
        saveCalibration();
        currentState = NORMAL;
      } else if (stateTicks == 300) {  // 3s delay before sampling
        char buffer[20];
        strcpy_P(buffer, releasePedalStr);
        displayText(buffer);
      }
    } else if (currentState == RESETTING) {
      if (stateTicks >= RESET_TICKS) {
        minVolts[activePedal] = DEFAULT_MIN_VOLT;
        maxVolts[activePedal] = DEFAULT_MAX_VOLT;
        saveCalibration();
        currentState = NORMAL;
      }
    }
    static const uint8_t buttonPins[] = {GAS_BUTTON_PIN, BRAKE_BUTTON_PIN, CLUTCH_BUTTON_PIN};
    if (stateTicks >= DEBOUNCE_TICKS && digitalRead(buttonPins[activePedal]) == LOW) {
      currentState = RESETTING;
      stateTicks = 0;
      char pedalName[10];
      strcpy_P(pedalName, (PGM_P)pgm_read_word(&(pedalNames[activePedal])));
      char buffer[20];
      snprintf(buffer, sizeof(buffer), "%s Reset", pedalName);
      char buffer2[20];
      strcpy_P(buffer2, resetToDefaultsStr);
      displayText(buffer, buffer2);
    }
    delay(10);
    return;
  }

  // Normal operation
  int16_t rawValues[NUM_PEDALS];
  bool adcError = false;
  for (int i = 0; i < NUM_PEDALS; i++) {
    rawValues[i] = ads.readADC_SingleEnded(i);
    if (rawValues[i] < 0) adcError = true;
  }

  if (adcError) {
    char buffer[20];
    strcpy_P(buffer, adcErrorStr);
    displayText(buffer, F("Read Error"));
    delay(1000);
    return;
  }

  uint16_t voltages[NUM_PEDALS];
  for (int i = 0; i < NUM_PEDALS; i++) {
    voltages[i] = ads.computeVolts(rawValues[i]) * 1000;  // mV
    axisValues[i] = applyDeadZone(voltages[i], minVolts[i], maxVolts[i]);
  }

  if (usb_hid.ready()) {
    uint8_t report[6];
    for (int i = 0; i < NUM_PEDALS; i++) {
      report[i * 2] = axisValues[i] & 0xFF;
      report[i * 2 + 1] = axisValues[i] >> 8;
    }
    usb_hid.sendReport(1, report, 6);
  }
  if (bleGamepad.isConnected()) {
    bleGamepad.setAxes(axisValues[PEDAL_GAS], axisValues[PEDAL_BRAKE], axisValues[PEDAL_CLUTCH], 0, 0, 0, 0, DPAD_CENTERED);
  }

  float brakeVolts = voltages[PEDAL_BRAKE] / 1000.0f;  // Convert back to volts
  float brakePSI = constrain(((brakeVolts - (minVolts[PEDAL_BRAKE] / 1000.0f)) / ((maxVolts[PEDAL_BRAKE] / 1000.0f) - (minVolts[PEDAL_BRAKE] / 1000.0f))) * 100.0f, 0.0f, 100.0f);
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print(usb_hid.ready() ? "USB" : (bleGamepad.isConnected() ? "BT" : ""));
  display.setTextSize(2);
  display.setCursor(40, 16);
  display.println("Brake");
  display.setCursor(28, 40);
  display.print(brakePSI, 2);
  display.print(F(" PSI"));
  display.display();

  delay(5);
}