Emergency Stopping Signal for ZX6R 1999 using ESP32 (Doesn't work bc too many interrupts

bike-emulator.ino

const int PIN_SPEED_SENSOR_OUT = 12;
const int PIN_BRAKE_LEVER_OUT = 13;

const int PIN_SLOW_TOUCH_1_IN = 4;
const int PIN_SLOW_TOUCH_2_IN = 32;

const int THRESHOLD = 30;
const int MAX_FREQ = 345;

void setup() {
  Serial.begin(115200);
  // put your setup code here, to run once:
  pinMode(PIN_SPEED_SENSOR_OUT, OUTPUT);
  pinMode(PIN_BRAKE_LEVER_OUT, OUTPUT);
}

float hz = 1;
bool out = false;
unsigned long timer = micros();

void loop() {
  int iBrake = touchRead(PIN_SLOW_TOUCH_1_IN);
  int iHardBrake = touchRead(PIN_SLOW_TOUCH_2_IN);

  bool brake = iBrake < THRESHOLD;
  bool hardBrake = iHardBrake < THRESHOLD;
  Serial.print("Brake: ");
  Serial.print(brake);
  Serial.print(", raw: ");
  Serial.print(iBrake);
  Serial.print(", HardBrake: ");
  Serial.print(hardBrake);
  Serial.print(", raw: ");
  Serial.print(iHardBrake);
  Serial.print(", Hz: ");
  Serial.println(hz);

  if (brake || hardBrake) {
    digitalWrite(PIN_BRAKE_LEVER_OUT, true);
  } else {
    digitalWrite(PIN_BRAKE_LEVER_OUT, false);
  }

  if (brake) {
    hz -= 0.3;
  } else if (hardBrake) {
    hz -= 1.3;
  } else {
    hz += 0.01;
    if (hz >= MAX_FREQ) {
      hz = MAX_FREQ;
    }
  }
  if (hz < 1) {
    hz = 1;
  }

  unsigned long now = micros();
  if (now - timer > (1000000/hz)) { // /2?
    out = !out;
    digitalWrite(PIN_SPEED_SENSOR_OUT, out);
    timer = now;
  }
}

ess.ino

const float FREQ_PER_MPH = 2.3; // (2.3 Hz) TODO tuning.

const int PIN_SPEED_SENSOR_IN = 18; // TODO
const int PIN_BRAKE_LIGHT_IN = 4; // TODO

const int PIN_BRAKE_LIGHT_OVERRIDE_OUT = 22; // For saftey, the brake light won't be disconnected from the normal circuit. (Normally closed) TODO
const int PIN_INDICATOR_LIGHT_OUT = 23; // TODO

const float STROBE_BRAKE_LIGHT_SPEED_MIN = 30; // (MPH) Pass this speed before arming. Legally this should be 34...
const float STROBE_HAZARDS_LIGHT_SPEED_ON = 10; // (MPH) If the speed decreases below this speed strobe hazards. Legally this should be on with ABS, but we want to draw attention.

const float DECELLERATION_FIRE_RATE = -15.658; // (MPH/s) ~7 m/s/s

volatile float acceleration;
volatile float speed;
volatile bool brakeLightOn = false;

void setup() {
  Serial.begin(115200);
  pinMode(PIN_BRAKE_LIGHT_OVERRIDE_OUT, OUTPUT);
  pinMode(PIN_INDICATOR_LIGHT_OUT, OUTPUT);

  pinMode(PIN_SPEED_SENSOR_IN, INPUT);
  pinMode(PIN_BRAKE_LIGHT_IN, INPUT);

  digitalWrite(PIN_INDICATOR_LIGHT_OUT, true);
  digitalWrite(PIN_BRAKE_LIGHT_OVERRIDE_OUT, true);
  delay(500);
  digitalWrite(PIN_INDICATOR_LIGHT_OUT, false);
  digitalWrite(PIN_BRAKE_LIGHT_OVERRIDE_OUT, false);
  delay(500);

  attachInterrupt(digitalPinToInterrupt(PIN_SPEED_SENSOR_IN), ISR_speed_sensor, CHANGE);
//  attachInterrupt(digitalPinToInterrupt(PIN_BRAKE_LIGHT_IN), ISR_brake_light, CHANGE);
}

unsigned long blinkTimer = micros();
bool brakeState = false;
bool indicatorState = false;

bool alerting = true;
bool armed = true;
void loop() {
  brakeLightOn = digitalRead(PIN_BRAKE_LIGHT_IN);
  
  float lAcceleration = acceleration;
  float lSpeed = speed;
  bool lBrakeLightOn = brakeLightOn;
  Serial.print("armed: ");
  Serial.print(armed);
  Serial.print(", accel: ");
  Serial.print(lAcceleration);
  Serial.print(", brake light: ");
  Serial.print(lBrakeLightOn);
  Serial.print(", speed: ");
  Serial.println(lSpeed);
  // TODO display?
  if (armed && lAcceleration < DECELLERATION_FIRE_RATE) {
    alerting = true;
  }

  if (alerting) {
    unsigned long now = micros();
    if (now - blinkTimer >= 166666) {
      brakeState = !brakeState;
      if (lSpeed <= STROBE_HAZARDS_LIGHT_SPEED_ON) {
        indicatorState = !brakeState;
      }
      blinkTimer = now;
    }
  }
  
  if (lSpeed >= 30) armed = true;
  if (lSpeed < 30) armed = false;
  
  if (!brakeLightOn) {
    brakeState = false;
    indicatorState = false;
    alerting = false;
  }
  // TODO keep indicators flashing 5 seconds after alert.

  digitalWrite(PIN_BRAKE_LIGHT_OVERRIDE_OUT, s(brakeState));
  digitalWrite(PIN_INDICATOR_LIGHT_OUT, s(indicatorState));
}

int s(bool in) {
  if (in) return HIGH;
  return LOW;
}

void ISR_brake_light() {
  brakeLightOn = digitalRead(PIN_BRAKE_LIGHT_IN);
}

// ============================================================
//                PWM Speed Sensor Interrupt
// ============================================================
volatile bool lastState = false;
volatile bool switched = false;
volatile unsigned long timer = micros();

volatile unsigned long accelerationTimer = micros();

// ISR_speed_sensor will measure the pwm period (and thus frequency) using an interrupt.
// Measures high to high ensuring a low pulse is between them.
// Improvements? Perhaps ensure we have a timeout between invalid states.
// Missed states shouldn't be counted since switched will never = true.
void ISR_speed_sensor() {
    unsigned long now = micros();
    bool stateNow = digitalRead(PIN_SPEED_SENSOR_IN);
    
    if (stateNow == lastState) return;
    
    if (stateNow) {
      if (switched) {
        volatile float lastSpeed = speed;
        // (60000000us - 50000000us) = 10000000 / 1000000us = 10hz * FREQ_PER_MPH = ~23mph?
        speed = (1000000.0 / (now - timer)) * FREQ_PER_MPH;

        volatile float deltaT = 1000000.0 / ((float) now - accelerationTimer); // 0.3;
        volatile float deltaV = speed - lastSpeed; // 4mph - 8.5mph =  -4.5mph

        if (deltaT == 0) {
          acceleration = 0;
        } else {
          acceleration = deltaV/deltaT; // -16/0.3
        }

        accelerationTimer = now;
      }

      // Reset and start counting.
      timer = now;
      switched = false;
    } else {
      switched = true;
    }

    lastState = stateNow;
}