Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Joystick_ESP32S2.h>
- #include <SPI.h>
- #include <ADS1220_WE.h>
- #include <Wire.h>
- #include <Adafruit_GFX.h>
- #include <Adafruit_SSD1306.h>
- #include <EEPROM.h>
- #define ADS1220_CS_PIN 10 // ADS1220 Chip Select
- #define ADS1220_DRDY_PIN 9 // ADS1220 Data Ready
- #define SPI_SCK 13 // SPI SCLK
- #define SPI_MISO 12 // SPI MISO
- #define SPI_MOSI 11 // SPI MOSI
- #define OLED_SDA 4 // OLED I2C SDA
- #define OLED_SCL 5 // OLED I2C SCL
- #define BUTTON_PREV 6 // Previous axis button
- #define BUTTON_NEXT_PAGE 7 // Next page button
- #define BUTTON_CALIBRATE 2 // Calibrate/filter button
- #define BUTTON_DEFAULT 3 // Restore default button
- #define SCREEN_WIDTH 128 // OLED width
- #define SCREEN_HEIGHT 64 // OLED height
- #define OLED_ADDRESS 0x3C // OLED I2C address
- #define CALIBRATION_TIME 4000 // 4 seconds per min/max
- #define DONE_TIME 1000 // 1 second for "DONE"
- #define DEBOUNCE_TIME 50 // 50 ms debounce
- #define OLED_REFRESH_MS 500 // OLED update every 500 ms (2 Hz)
- #define DEADBAND_COUNTS 3355 // 2 mV equivalent (2/5000 * 2^23)
- #define MAX_SAMPLES 20 // Max averaging samples
- #define EEPROM_SIZE 64 // Allocate 64 bytes for EEPROM
- #define EEPROM_MAGIC 0x12345678 // Magic number for EEPROM validation
- #define EEPROM_MAGIC_ADDR 0
- #define EEPROM_MIN_CLUTCH_ADDR 4
- #define EEPROM_MAX_CLUTCH_ADDR 8
- #define EEPROM_MIN_BRAKE_ADDR 12
- #define EEPROM_MAX_BRAKE_ADDR 16
- #define EEPROM_MIN_GAS_ADDR 20
- #define EEPROM_MAX_GAS_ADDR 24
- #define EEPROM_MIN_HB_ADDR 28
- #define EEPROM_MAX_HB_ADDR 32
- #define EEPROM_SAMPLES_CLUTCH_ADDR 36
- #define EEPROM_SAMPLES_BRAKE_ADDR 40
- #define EEPROM_SAMPLES_GAS_ADDR 44
- #define EEPROM_SAMPLES_HB_ADDR 48
- Joystick_ joystick(JOYSTICK_DEFAULT_REPORT_ID, JOYSTICK_TYPE_JOYSTICK,
- 0, 0, true, true, true, true, false, false, false, false);
- ADS1220_WE ads(ADS1220_CS_PIN, ADS1220_DRDY_PIN);
- Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
- // Calibration state
- enum CalibState { IDLE, MIN_CALIB, MAX_CALIB, MIN_DONE, MAX_DONE };
- volatile CalibState calibState = IDLE;
- volatile int selectedAxis = 0; // 0=C, 1=B, 2=G, 3=H
- volatile int calibAxis = -1; // Axis being calibrated
- volatile unsigned long calibStartTime = 0;
- volatile int currentScreen = 0; // 0=Filter Adjust, 1=Min/Max Calibration
- // Button states for debouncing
- volatile bool lastPrevState = HIGH;
- volatile bool lastNextPageState = HIGH;
- volatile bool lastCalibrateState = HIGH;
- volatile bool lastDefaultState = HIGH;
- volatile unsigned long lastButtonCheck = 0;
- // Min/max values (defaults: 0 to 2^23-1)
- volatile int32_t minClutch = 0, maxClutch = 8388607;
- volatile int32_t minBrake = 0, maxBrake = 8388607;
- volatile int32_t minGas = 0, maxGas = 8388607;
- volatile int32_t minHB = 0, maxHB = 8388607;
- volatile int32_t currentMin, currentMax;
- // Averaging samples (default: 2)
- volatile uint8_t samplesClutch = 2, samplesBrake = 2, samplesGas = 2, samplesHB = 2;
- // ADC values (shared with Core 0)
- volatile int32_t valueClutch = 0, valueBrake = 0, valueGas = 0, valueHB = 0;
- // Last output values for deadband
- volatile int32_t lastValueClutch = 0, lastValueBrake = 0, lastValueGas = 0, lastValueHB = 0;
- // Circular buffer for ADC samples
- volatile int32_t sampleBuffer[4][MAX_SAMPLES]; // Buffer for C, B, G, H
- volatile uint8_t sampleCount[4] = {0, 0, 0, 0}; // Sample counts per channel
- volatile uint8_t currentChannel = 0; // Current ADC channel (0=C, 1=B, 2=G, 3=H)
- volatile bool dataReady = false;
- // DRDY interrupt
- void IRAM_ATTR drdyISR() {
- if (dataReady) return; // Prevent re-entry
- dataReady = true;
- int32_t data = ads.getRawData();
- sampleBuffer[currentChannel][sampleCount[currentChannel]] = data;
- sampleCount[currentChannel]++;
- if (sampleCount[currentChannel] >= (currentChannel == 0 ? samplesClutch : currentChannel == 1 ? samplesBrake : currentChannel == 2 ? samplesGas : samplesHB)) {
- sampleCount[currentChannel] = 0; // Reset when buffer is full
- }
- currentChannel = (currentChannel + 1) % 4; // Cycle through channels
- ads.setCompareChannels(currentChannel == 0 ? ADS1220_MUX_0_AVSS : currentChannel == 1 ? ADS1220_MUX_1_AVSS : currentChannel == 2 ? ADS1220_MUX_2_AVSS : ADS1220_MUX_3_AVSS);
- }
- // Integer-based mapping
- long customMap(int32_t x, int32_t in_min, int32_t in_max, long out_min, long out_max) {
- int64_t result = (int64_t)(x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
- if (result < out_min) return out_min;
- if (result > out_max) return out_max;
- return (long)result;
- }
- // PSI calculation for BRAKE (0.5V=0 PSI, 4.5V=100 PSI)
- float calculatePSIBrake(int32_t valueBrake) {
- float psi = (float)(valueBrake - 839681) * 100.0 / (7951127 - 839681);
- return constrain(psi, 0.0, 100.0);
- }
- void saveCalibration() {
- EEPROM.put(EEPROM_MIN_CLUTCH_ADDR, minClutch);
- EEPROM.put(EEPROM_MAX_CLUTCH_ADDR, maxClutch);
- EEPROM.put(EEPROM_MIN_BRAKE_ADDR, minBrake);
- EEPROM.put(EEPROM_MAX_BRAKE_ADDR, maxBrake);
- EEPROM.put(EEPROM_MIN_GAS_ADDR, minGas);
- EEPROM.put(EEPROM_MAX_GAS_ADDR, maxGas);
- EEPROM.put(EEPROM_MIN_HB_ADDR, minHB);
- EEPROM.put(EEPROM_MAX_HB_ADDR, maxHB);
- EEPROM.put(EEPROM_SAMPLES_CLUTCH_ADDR, samplesClutch);
- EEPROM.put(EEPROM_SAMPLES_BRAKE_ADDR, samplesBrake);
- EEPROM.put(EEPROM_SAMPLES_GAS_ADDR, samplesGas);
- EEPROM.put(EEPROM_SAMPLES_HB_ADDR, samplesHB);
- EEPROM.commit();
- }
- void loadCalibration() {
- uint32_t magic;
- EEPROM.get(EEPROM_MAGIC_ADDR, magic);
- if (magic != EEPROM_MAGIC) {
- saveCalibration();
- EEPROM.put(EEPROM_MAGIC_ADDR, EEPROM_MAGIC);
- EEPROM.commit();
- } else {
- EEPROM.get(EEPROM_MIN_CLUTCH_ADDR, minClutch);
- EEPROM.get(EEPROM_MAX_CLUTCH_ADDR, maxClutch);
- EEPROM.get(EEPROM_MIN_BRAKE_ADDR, minBrake);
- EEPROM.get(EEPROM_MAX_BRAKE_ADDR, maxBrake);
- EEPROM.get(EEPROM_MIN_GAS_ADDR, minGas);
- EEPROM.get(EEPROM_MAX_GAS_ADDR, maxGas);
- EEPROM.get(EEPROM_MIN_HB_ADDR, minHB);
- EEPROM.get(EEPROM_MAX_HB_ADDR, maxHB);
- EEPROM.get(EEPROM_SAMPLES_CLUTCH_ADDR, samplesClutch);
- EEPROM.get(EEPROM_SAMPLES_BRAKE_ADDR, samplesBrake);
- EEPROM.get(EEPROM_SAMPLES_GAS_ADDR, samplesGas);
- EEPROM.get(EEPROM_SAMPLES_HB_ADDR, samplesHB);
- samplesClutch = constrain(samplesClutch, 1, MAX_SAMPLES);
- samplesBrake = constrain(samplesBrake, 1, MAX_SAMPLES);
- samplesGas = constrain(samplesGas, 1, MAX_SAMPLES);
- samplesHB = constrain(samplesHB, 1, MAX_SAMPLES);
- }
- }
- void resetDefaultCalibration(int axis) {
- if (axis == 0) { minClutch = 0; maxClutch = 8388607; samplesClutch = 2; }
- else if (axis == 1) { minBrake = 0; maxBrake = 8388607; samplesBrake = 2; }
- else if (axis == 2) { minGas = 0; maxGas = 8388607; samplesGas = 2; }
- else { minHB = 0; maxHB = 8388607; samplesHB = 2; }
- saveCalibration();
- }
- // Core 0 task for buttons, calibration, and OLED
- void displayAndControlTask(void *pvParameters) {
- unsigned long lastDisplayUpdate = 0;
- while (1) {
- unsigned long currentMillis = millis();
- // Read buttons with debouncing
- if (currentMillis - lastButtonCheck >= DEBOUNCE_TIME && calibState == IDLE) {
- bool prevState = digitalRead(BUTTON_PREV);
- bool nextPageState = digitalRead(BUTTON_NEXT_PAGE);
- bool calibrateState = digitalRead(BUTTON_CALIBRATE);
- bool defaultState = digitalRead(BUTTON_DEFAULT);
- if (prevState == LOW && lastPrevState == HIGH) {
- selectedAxis = (selectedAxis - 1 + 4) % 4;
- while (digitalRead(BUTTON_PREV) == LOW && millis() - currentMillis < 1000);
- }
- lastPrevState = prevState;
- if (nextPageState == LOW && lastNextPageState == HIGH) {
- currentScreen = (currentScreen + 1) % 2;
- while (digitalRead(BUTTON_NEXT_PAGE) == LOW && millis() - currentMillis < 1000);
- }
- lastNextPageState = nextPageState;
- if (calibrateState == LOW && lastCalibrateState == HIGH) {
- if (currentScreen == 0) {
- if (selectedAxis == 0) samplesClutch = (samplesClutch % MAX_SAMPLES) + 1;
- else if (selectedAxis == 1) samplesBrake = (samplesBrake % MAX_SAMPLES) + 1;
- else if (selectedAxis == 2) samplesGas = (samplesGas % MAX_SAMPLES) + 1;
- else samplesHB = (samplesHB % MAX_SAMPLES) + 1;
- saveCalibration();
- } else {
- calibState = MIN_CALIB;
- calibAxis = selectedAxis;
- calibStartTime = currentMillis;
- currentMin = 8388608;
- currentMax = -8388608;
- }
- }
- lastCalibrateState = calibrateState;
- if (defaultState == LOW && lastDefaultState == HIGH) {
- resetDefaultCalibration(selectedAxis);
- }
- lastDefaultState = defaultState;
- lastButtonCheck = currentMillis;
- }
- // Update calibration (Screen 2 only)
- if (calibState != IDLE && calibAxis >= 0 && calibAxis < 4) {
- int32_t value = (calibAxis == 0) ? valueClutch : (calibAxis == 1) ? valueBrake : (calibAxis == 2) ? valueGas : valueHB;
- if (calibState == MIN_CALIB) {
- if (value < currentMin) currentMin = value;
- if (currentMillis - calibStartTime >= CALIBRATION_TIME) {
- calibState = MIN_DONE;
- calibStartTime = currentMillis;
- }
- } else if (calibState == MIN_DONE && currentMillis - calibStartTime >= DONE_TIME) {
- if (calibAxis == 0) minClutch = currentMin;
- else if (calibAxis == 1) minBrake = currentMin;
- else if (calibAxis == 2) minGas = currentMin;
- else minHB = currentMin;
- calibState = MAX_CALIB;
- calibStartTime = currentMillis;
- } else if (calibState == MAX_CALIB) {
- if (value > currentMax) currentMax = value;
- if (currentMillis - calibStartTime >= CALIBRATION_TIME) {
- calibState = MAX_DONE;
- calibStartTime = currentMillis;
- }
- } else if (calibState == MAX_DONE && currentMillis - calibStartTime >= DONE_TIME) {
- if (calibAxis == 0) maxClutch = currentMax;
- else if (calibAxis == 1) maxBrake = currentMax;
- else if (calibAxis == 2) maxGas = currentMax;
- else maxHB = currentMax;
- saveCalibration();
- calibState = IDLE;
- calibAxis = -1;
- }
- }
- // Update OLED
- if (currentMillis - lastDisplayUpdate >= OLED_REFRESH_MS) {
- display.clearDisplay();
- display.setCursor(0, 0);
- if (calibState == MIN_CALIB) {
- display.print(calibAxis == 0 ? "C" : calibAxis == 1 ? "B" : calibAxis == 2 ? "G" : "H");
- display.print(" MIN: ");
- display.println((calibAxis == 0) ? valueClutch : (calibAxis == 1) ? valueBrake : (calibAxis == 2) ? valueGas : valueHB);
- display.print("Time: ");
- float remaining = (CALIBRATION_TIME - (currentMillis - calibStartTime)) / 1000.0;
- display.print(remaining, 1);
- display.println("s");
- } else if (calibState == MIN_DONE) {
- display.print(calibAxis == 0 ? "C" : calibAxis == 1 ? "B" : calibAxis == 2 ? "G" : "H");
- display.print(" MIN: ");
- display.print(currentMin);
- display.println(" DONE");
- } else if (calibState == MAX_CALIB) {
- display.print(calibAxis == 0 ? "C" : calibAxis == 1 ? "B" : calibAxis == 2 ? "G" : "H");
- display.print(" MAX: ");
- display.println((calibAxis == 0) ? valueClutch : (calibAxis == 1) ? valueBrake : (calibAxis == 2) ? valueGas : valueHB);
- display.print("Time: ");
- float remaining = (CALIBRATION_TIME - (currentMillis - calibStartTime)) / 1000.0;
- display.print(remaining, 1);
- display.println("s");
- } else if (calibState == MAX_DONE) {
- display.print(calibAxis == 0 ? "C" : calibAxis == 1 ? "B" : calibAxis == 2 ? "G" : "H");
- display.print(" MAX: ");
- display.print(currentMax);
- display.println(" DONE");
- } else if (currentScreen == 0) {
- display.println("Filter Settings");
- display.print(selectedAxis == 0 ? ">C: " : " C: "); display.println(samplesClutch);
- display.print(selectedAxis == 1 ? ">B: " : " B: "); display.println(samplesBrake);
- display.print(selectedAxis == 2 ? ">G: " : " G: "); display.println(samplesGas);
- display.print(selectedAxis == 3 ? ">H: " : " H: "); display.println(samplesHB);
- } else {
- display.println("Pedals Min/Max");
- display.print(selectedAxis == 0 ? ">C: " : " C: "); display.print(minClutch); display.print(" "); display.println(maxClutch);
- display.print(selectedAxis == 1 ? ">B: " : " B: "); display.print(minBrake); display.print(" "); display.println(maxBrake);
- display.print(selectedAxis == 2 ? ">G: " : " G: "); display.print(minGas); display.print(" "); display.println(maxGas);
- display.print(selectedAxis == 3 ? ">H: " : " H: "); display.print(minHB); display.print(" "); display.println(maxHB);
- display.print("PSI B: "); display.println(calculatePSIBrake(valueBrake), 1);
- }
- display.display();
- lastDisplayUpdate = currentMillis;
- }
- vTaskDelay(20 / portTICK_PERIOD_MS); // Yield every 20 ms
- }
- }
- void setup() {
- joystick.begin();
- joystick.setXAxisRange(0, 65535);
- joystick.setYAxisRange(0, 65535);
- joystick.setZAxisRange(0, 65535);
- joystick.setRxAxisRange(0, 65535);
- EEPROM.begin(EEPROM_SIZE);
- loadCalibration();
- pinMode(BUTTON_PREV, INPUT_PULLUP);
- pinMode(BUTTON_NEXT_PAGE, INPUT_PULLUP);
- pinMode(BUTTON_CALIBRATE, INPUT_PULLUP);
- pinMode(BUTTON_DEFAULT, INPUT_PULLUP);
- SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI, ADS1220_CS_PIN);
- if (!ads.init()) {
- while (1);
- }
- ads.setSPIClockSpeed(10000000);
- ads.setVRefSource(ADS1220_VREF_REFP0_REFN0);
- ads.setVRefValue_V(5.0);
- ads.setGain(ADS1220_GAIN_1);
- ads.bypassPGA(true);
- ads.setDataRate(ADS1220_DR_LVL_6); // 1200 SPS
- ads.setOperatingMode(ADS1220_TURBO_MODE);
- ads.setConversionMode(ADS1220_CONTINUOUS);
- ads.setFIRFilter(ADS1220_50HZ_60HZ);
- // Initialize first channel
- ads.setCompareChannels(ADS1220_MUX_0_AVSS); // Start with CLUTCH
- pinMode(ADS1220_DRDY_PIN, INPUT);
- attachInterrupt(digitalPinToInterrupt(ADS1220_DRDY_PIN), drdyISR, FALLING);
- Wire.begin(OLED_SDA, OLED_SCL);
- Wire.setClock(400000);
- if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
- while (1);
- }
- display.clearDisplay();
- display.setTextSize(1);
- display.setTextColor(SSD1306_WHITE);
- display.setCursor(0, 0);
- display.println("Pedals Calibration");
- display.display();
- xTaskCreatePinnedToCore(
- displayAndControlTask, "DisplayControlTask", 4096, NULL, 1, NULL, 0
- );
- }
- void loop() {
- static bool toggle = false;
- unsigned long currentMillis = millis();
- if (!dataReady) return; // Process only when new data
- dataReady = false;
- // Compute averages
- int32_t sumClutch = 0, sumBrake = 0, sumGas = 0, sumHB = 0;
- for (int i = 0; i < samplesClutch; i++) sumClutch += sampleBuffer[0][i];
- for (int i = 0; i < samplesBrake; i++) sumBrake += sampleBuffer[1][i];
- for (int i = 0; i < samplesGas; i++) sumGas += sampleBuffer[2][i];
- for (int i = 0; i < samplesHB; i++) sumHB += sampleBuffer[3][i];
- int32_t newValueClutch = samplesClutch > 0 ? sumClutch / samplesClutch : sumClutch;
- int32_t newValueBrake = samplesBrake > 0 ? sumBrake / samplesBrake : sumBrake;
- int32_t newValueGas = samplesGas > 0 ? sumGas / samplesGas : sumGas;
- int32_t newValueHB = samplesHB > 0 ? sumHB / samplesHB : sumHB;
- // Apply 2 mV deadband filter
- if (abs(newValueClutch - lastValueClutch) >= DEADBAND_COUNTS) {
- valueClutch = newValueClutch;
- lastValueClutch = newValueClutch;
- }
- if (abs(newValueBrake - lastValueBrake) >= DEADBAND_COUNTS) {
- valueBrake = newValueBrake;
- lastValueBrake = newValueBrake;
- }
- if (abs(newValueGas - lastValueGas) >= DEADBAND_COUNTS) {
- valueGas = newValueGas;
- lastValueGas = newValueGas;
- }
- if (abs(newValueHB - lastValueHB) >= DEADBAND_COUNTS) {
- valueHB = newValueHB;
- lastValueHB = newValueHB;
- }
- // Map to 0–65535
- int mappedClutch = customMap(valueClutch, minClutch, maxClutch, 0, 65535);
- int mappedBrake = customMap(valueBrake, minBrake, maxBrake, 0, 65535);
- int mappedGas = customMap(valueGas, minGas, maxGas, 0, 65535);
- int mappedHB = customMap(valueHB, minHB, maxHB, 0, 65535);
- // Send joystick data every 2nd loop
- joystick.setXAxis(mappedClutch);
- joystick.setYAxis(mappedBrake);
- joystick.setZAxis(mappedGas);
- joystick.setRxAxis(mappedHB);
- if (toggle) joystick.sendState();
- toggle = !toggle;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement