Servo Angle Controller

#include <Arduino.h>
#include <Wire.h>
#include <U8g2lib.h>
#include <Servo.h>

struct Button {
  uint8_t pin;
  bool stableState;
  bool lastReading;
  unsigned long changedMs;
};


// Forward declarations
bool pressed(Button &button);
int pulseForAngle(int angle);
void drawScreen();
void showSplash();

const uint8_t SERVO_PIN = 9;
const uint8_t PLUS_BUTTON_PIN = 2;
const uint8_t MINUS_BUTTON_PIN = 3;
const uint8_t SELECT_BUTTON_PIN = 4;
const uint8_t OLED_SDA_PIN = A4;
const uint8_t OLED_SCL_PIN = A5;

const unsigned long DEBOUNCE_MS = 35;
const unsigned long BOTH_HOLD_MS = 1000;
const int MIN_ANGLE = 0;
const int MAX_ANGLE = 180;
const int SERVO_MIN_US = 544;
const int SERVO_MAX_US = 2400;

// Full-buffer mode is required by this OLED driver; the screen is sent only when it changes.
U8G2_SSD1306_128X64_NONAME_F_HW_I2C display(U8G2_R0, U8X8_PIN_NONE);
Servo servo;

Button plusButton = {PLUS_BUTTON_PIN, HIGH, HIGH, 0};
Button minusButton = {MINUS_BUTTON_PIN, HIGH, HIGH, 0};
Button selectButton = {SELECT_BUTTON_PIN, HIGH, HIGH, 0};

int selectedAngle = 90;
int appliedAngle = 90;
int angleStep = 5;
unsigned long bothPressedSinceMs = 0;
bool stepChangedForCurrentHold = false;

bool pressed(Button &button) {
  bool reading = digitalRead(button.pin);
  bool pressedEvent = false;

  if (reading != button.lastReading) {
    button.changedMs = millis();
    button.lastReading = reading;
  }

  if (millis() - button.changedMs > DEBOUNCE_MS && reading != button.stableState) {
    button.stableState = reading;
    if (button.stableState == LOW) {
      pressedEvent = true;
    }
  }
  return pressedEvent;
}

int pulseForAngle(int angle) {
  return map(angle, MIN_ANGLE, MAX_ANGLE, SERVO_MIN_US, SERVO_MAX_US);
}

void drawScreen() {
  const bool pending = selectedAngle != appliedAngle;
  char pulseText[8];
  char angleText[4];

  display.clearBuffer();
  display.setFontMode(1);
  display.setDrawColor(1);
  display.setFont(u8g2_font_unifont_t_cyrillic);
  display.drawUTF8(0, 12, "УГОЛ_СЕРВО...");
  display.drawHLine(0, 15, 128);

  snprintf(angleText, sizeof(angleText), "%d", selectedAngle);
  display.setFont(u8g2_font_logisoso20_tn);
  display.drawStr(0, 37, angleText);

  display.setFont(u8g2_font_unifont_t_cyrillic);
  // Short labels fit beside the large angle number without clipping.
  display.drawUTF8(52, 36, pending ? "ОЖИДАНИЕ" : "ВЫБРАНО");

  snprintf(pulseText, sizeof(pulseText), "%d", pulseForAngle(selectedAngle));
  display.drawUTF8(0, 51, "ИМПУЛЬС");
  display.drawStr(61, 51, pulseText);
  display.drawUTF8(96, 51, "МКС");

  display.drawUTF8(0, 64, angleStep == 1 ? "ШАГ  1 ГРАДУС" : "ШАГ  5 ГРАДУСОВ");

  display.sendBuffer();
}

void showSplash() {
  const char *splashText = "Squad - service";

  display.clearBuffer();
  // This is the largest bold Latin font that keeps the complete text on one 128-pixel line.
  display.setFont(u8g2_font_helvB12_tf);
  const int textX = (128 - display.getStrWidth(splashText)) / 2;
  display.drawStr(textX, 38, splashText);
  display.sendBuffer();
  delay(2000);
}

void setup() {
  pinMode(PLUS_BUTTON_PIN, INPUT_PULLUP);
  pinMode(MINUS_BUTTON_PIN, INPUT_PULLUP);
  pinMode(SELECT_BUTTON_PIN, INPUT_PULLUP);

  Wire.begin();
  display.begin();
  display.enableUTF8Print();
  showSplash();

  servo.attach(SERVO_PIN, SERVO_MIN_US, SERVO_MAX_US);
  servo.write(appliedAngle);
  drawScreen();
}

void loop() {
  bool redraw = false;

  if (pressed(plusButton)) {
    selectedAngle = min(MAX_ANGLE, selectedAngle + angleStep);
    redraw = true;
  }
  if (pressed(minusButton)) {
    selectedAngle = max(MIN_ANGLE, selectedAngle - angleStep);
    redraw = true;
  }
  if (pressed(selectButton)) {
    appliedAngle = selectedAngle;
    servo.write(appliedAngle);
    redraw = true;
  }

  const bool bothHeld = digitalRead(PLUS_BUTTON_PIN) == LOW && digitalRead(MINUS_BUTTON_PIN) == LOW;
  if (bothHeld) {
    if (bothPressedSinceMs == 0) {
      bothPressedSinceMs = millis();
    }
    if (!stepChangedForCurrentHold && millis() - bothPressedSinceMs >= BOTH_HOLD_MS) {
      angleStep = angleStep == 5 ? 1 : 5;
      stepChangedForCurrentHold = true;
      redraw = true;
    }
  } else {
    bothPressedSinceMs = 0;
    stepChangedForCurrentHold = false;
  }

  if (redraw) {
    drawScreen();
  }
}