Motor RPM Monitoring Control

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

constexpr uint8_t MOTOR_ENA_PIN = 25;
constexpr uint8_t MOTOR_IN1_PIN = 26;
constexpr uint8_t MOTOR_IN2_PIN = 33;
constexpr uint8_t SPEED_SENSOR_PIN = 27;
constexpr uint8_t OLED_SDA_PIN = 21;
constexpr uint8_t OLED_SCL_PIN = 22;
constexpr uint8_t STATUS_LED_PIN = 32;
constexpr uint8_t BUZZER_PIN = 4;
constexpr uint8_t RESTART_BUTTON_PIN = 13;

constexpr uint8_t OLED_WIDTH = 128;
constexpr uint8_t OLED_HEIGHT = 64;
constexpr uint8_t OLED_ADDRESS = 0x3C;
constexpr uint16_t PWM_FREQUENCY = 20000;
constexpr uint8_t PWM_RESOLUTION = 8;
constexpr uint8_t PWM_MAX = 255;
constexpr uint16_t PULSES_PER_REVOLUTION = 1; // One optical encoder slot or flag per revolution.
// Desired shaft speed. Change this only after verifying the real RPM with the OLED.
constexpr float TARGET_RPM = 120.0F;
constexpr uint32_t SAMPLE_INTERVAL_MS = 200;
// Conservative PI gains: the output is also limited to small steps to prevent hunting.
constexpr float KP = 0.10F;
constexpr float KI = 0.03F;
constexpr uint8_t RESTART_PWM_DUTY = 180;
constexpr uint32_t STARTUP_GRACE_MS = 3000;
constexpr uint32_t SENSOR_TIMEOUT_MS = 2000;
constexpr uint32_t BUTTON_DEBOUNCE_MS = 50;

Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT, &Wire, -1);
volatile uint32_t hallPulses = 0;
volatile uint32_t lastPulseUs = 0;
volatile uint32_t pulsePeriodUs = 0;
volatile uint32_t pulseSequence = 0;
uint32_t lastSampleMs = 0;
uint32_t lastDisplayMs = 0;
uint32_t motorStartMs = 0;
uint32_t lastControlledPulseSequence = 0;
float measuredRpm = 0.0F;
float integralError = 0.0F;
uint8_t pwmDuty = RESTART_PWM_DUTY;
bool displayReady = false;
bool motorStopped = false;
bool lastButtonReading = HIGH;
uint32_t lastButtonPressMs = 0;

void IRAM_ATTR onSpeedPulse() {
  const uint32_t nowUs = micros();
  if (lastPulseUs != 0) {
    pulsePeriodUs = nowUs - lastPulseUs;
  }
  lastPulseUs = nowUs;
  hallPulses++;
  pulseSequence++;
}

void drawDisplay() {
  if (!displayReady) return;

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("DC MOTOR CONTROL");
  display.setCursor(0, 16);
  display.print("RPM: ");
  display.print(measuredRpm, 0);
  display.setCursor(0, 28);
  display.print("SET: ");
  display.print(TARGET_RPM, 0);
  display.setCursor(0, 40);
  display.print("PWM: ");
  display.print((100UL * pwmDuty) / PWM_MAX);
  display.print('%');
  display.setCursor(0, 54);
  if (motorStopped) {
    display.print("STOPPED: PRESS BUTTON");
  } else {
    display.print("Optical: GPIO27  1 pulse/rev");
  }
  display.display();
}

void setup() {
  pinMode(MOTOR_IN1_PIN, OUTPUT);
  pinMode(MOTOR_IN2_PIN, OUTPUT);
  pinMode(STATUS_LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(SPEED_SENSOR_PIN, INPUT_PULLUP);
  pinMode(RESTART_BUTTON_PIN, INPUT_PULLUP);

  // Forward direction. Swap the two motor wires at OUT1/OUT2 to reverse it.
  digitalWrite(MOTOR_IN1_PIN, HIGH);
  digitalWrite(MOTOR_IN2_PIN, LOW);
  digitalWrite(STATUS_LED_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);

  ledcAttach(MOTOR_ENA_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
  // Start automatically; the controller changes this value as it measures speed.
  ledcWrite(MOTOR_ENA_PIN, pwmDuty);

  Wire.begin(OLED_SDA_PIN, OLED_SCL_PIN);
  displayReady = display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS);
  if (displayReady) {
    drawDisplay();
  }

  // The LM393 optical module produces an active-low pulse when its light beam is interrupted.
  attachInterrupt(digitalPinToInterrupt(SPEED_SENSOR_PIN), onSpeedPulse, FALLING);
  lastSampleMs = millis();
  motorStartMs = lastSampleMs;
}

void loop() {
  const uint32_t now = millis();
  const bool buttonReading = digitalRead(RESTART_BUTTON_PIN);
  if (buttonReading == LOW && lastButtonReading == HIGH &&
      now - lastButtonPressMs >= BUTTON_DEBOUNCE_MS) {
    // A press after a stop gives the motor a fresh starting drive.
    motorStopped = false;
    integralError = 0.0F;
    pwmDuty = RESTART_PWM_DUTY;
    motorStartMs = now;
    lastControlledPulseSequence = 0;
    ledcWrite(MOTOR_ENA_PIN, pwmDuty);
    digitalWrite(BUZZER_PIN, LOW);
    lastButtonPressMs = now;
  }
  lastButtonReading = buttonReading;

  if (now - lastSampleMs >= SAMPLE_INTERVAL_MS) {
    noInterrupts();
    const uint32_t latestPulseUs = lastPulseUs;
    const uint32_t latestPeriodUs = pulsePeriodUs;
    const uint32_t latestSequence = pulseSequence;
    interrupts();

    const uint32_t elapsedMs = now - lastSampleMs;
    const float elapsedSeconds = elapsedMs / 1000.0F;
    lastSampleMs = now;
    const uint32_t pulseAgeUs = micros() - latestPulseUs;
    const bool recentPulse = latestPulseUs != 0 && pulseAgeUs < SENSOR_TIMEOUT_MS * 1000UL;

    // With one flag per revolution, pulse spacing gives a stable RPM value even
    // though a new pulse arrives only about twice per second at 120 RPM.
    if (recentPulse && latestPeriodUs > 0) {
      measuredRpm = 60000000.0F / (PULSES_PER_REVOLUTION * latestPeriodUs);
    } else if (!recentPulse) {
      measuredRpm = 0.0F;
    }

    // Change drive only after a new encoder pulse. With one flag per shaft turn,
    // this avoids reacting repeatedly to the same RPM reading.
    if (!motorStopped && latestSequence != lastControlledPulseSequence && latestPeriodUs > 0) {
      const float error = TARGET_RPM - measuredRpm;
      integralError = constrain(integralError + error * elapsedSeconds, -200.0F, 200.0F);
      const float requestedChange = (KP * error) + (KI * integralError);
      // Limit one correction so a noisy pulse cannot cause a large speed swing.
      const int pwmChange = constrain(static_cast<int>(requestedChange), -12, 12);
      pwmDuty = static_cast<uint8_t>(constrain(static_cast<int>(pwmDuty) + pwmChange, 0, PWM_MAX));
      ledcWrite(MOTOR_ENA_PIN, pwmDuty);
      lastControlledPulseSequence = latestSequence;
    }

    // Stop only for a real loss of sensor movement, not for a small RPM error.
    if (!motorStopped && now - motorStartMs >= STARTUP_GRACE_MS && !recentPulse) {
      motorStopped = true;
      pwmDuty = 0;
      ledcWrite(MOTOR_ENA_PIN, 0);
    }

    digitalWrite(STATUS_LED_PIN, recentPulse ? HIGH : LOW);
    // Buzz only while the controller is waiting for the restart button.
    digitalWrite(BUZZER_PIN, motorStopped ? HIGH : LOW);
  }

  if (now - lastDisplayMs >= SAMPLE_INTERVAL_MS) {
    drawDisplay();
    lastDisplayMs = now;
  }
}