DIY Sync Checker for stereoscopic 3D rig

#include <M5StickC.h>
#include <ESP32Servo.h>
#include "time.h"

Servo servo1;
RTC_TimeTypeDef RTC_TimeStruct;
int motor_speed_min = 11; // Set motor speed to stat-up speed
int motor_speed_max = 25; // Set max motor speed for safty
int scaleNum = 16; // Number of scale per lap
int servo_minus = 1100; // servo Min 1100us
int servo_maxus = 1900; // servo Max 1900us
int led_pin = 10; // Pin No. for LED
int servo_pin = 26; // Pin No. for motor ESC(Electric Speed Controller)
int int_pin = 33; // Pin No. for photo interrupter
int sw_center_pin = 37; // Pin No. for Center SW 
int sw_side_pin = 39; // Pin No. for Side(Right) SW 
int motor_speed = 0;  // Motor speed (0�E�E80degree for ESC)
int lapCount = 0; // Lap counter
int ct;
int pt;
int sec = 0;
int prev_sec = 0;
int rpm;
float dev;
// Set state of the Center Button
uint8_t btn = HIGH;
uint8_t btn_prev = HIGH;


void setup() {
  M5.begin();
  // Initializing LCD
  M5.Lcd.setRotation(0);  // Set LCD rotation to vertical position
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(10, 0, 2);
  M5.Lcd.println("? SYNC ?");
  M5.Lcd.setCursor(0, 30);
  M5.Lcd.println("SPEED");
  M5.Lcd.setCursor(30, 120);
  M5.Lcd.println("STOP >");
  M5.Lcd.setCursor(20, 140);
  M5.Lcd.println("> UP <");
  M5.Lcd.setTextSize(2);

  // Set center button INPUT
  pinMode(sw_center_pin, INPUT_PULLUP);
  pinMode(sw_side_pin, INPUT_PULLUP);
  
  // Set LED pin as OUTPUT
  pinMode(led_pin, OUTPUT);

  // Initializing servo
  servo1.setPeriodHertz(50);
  servo1.attach(servo_pin, servo_minus, servo_maxus);

  // LED OFF
  digitalWrite(led_pin, HIGH );

  // Output 0 to motor ESC
  motorSpeedChange(motor_speed);

  // Set interrupt
  pinMode(int_pin, INPUT);
  attachInterrupt(int_pin, senceRot, RISING);
}


void loop() {
  // Speed up if the center button is pushed
  btn = digitalRead(sw_center_pin);
  if(btn == LOW && btn_prev == HIGH){
    btn_prev = LOW;
    motor_speed = motor_speed + 1;
    digitalWrite(led_pin, LOW ); // LED ON

    if (motor_speed < motor_speed_min){
      motor_speed = motor_speed_min;
    }
    if (motor_speed > motor_speed_max){
      motor_speed = motor_speed_max;
    }
    motorSpeedChange(motor_speed);  // Set motor speed
    delay(100);
  }
  if (btn == HIGH){
    btn_prev = HIGH;
  }

  // Stop motor if the side button is pushed
  if(digitalRead(sw_side_pin) == LOW)
  {
    motor_speed = 0;
    motorSpeedChange(motor_speed);  // set motor speed to 0
    digitalWrite(led_pin, HIGH ); // LED OFF
  }

  // Display RPM & msec/div every second
  M5.Rtc.GetTime(&RTC_TimeStruct);
  sec = RTC_TimeStruct.Seconds;
  if (sec != prev_sec)
  {
    int rot = lapCount;
    lapCount = 0;
    rpm = rot * 60;
    prev_sec = sec;
    M5.Lcd.setTextSize(1);
    M5.Lcd.setCursor(0, 82);
    M5.Lcd.printf("%05d RPM", rpm);
    M5.Lcd.setCursor(0, 100);
    if(rot > 0)
    {
      dev = (float)(1000 / rot) / scaleNum;
      M5.Lcd.printf("%3.1f ms/div  ", dev);
    }
    else
    {
      M5.Lcd.println("-----    "); 
    }

    M5.Lcd.setTextSize(2);
  }  
}

//
// Set motor speed and display motor speed
//
void motorSpeedChange(int sp){
  // Display motor speed
  M5.Lcd.setCursor(20, 45);
  M5.Lcd.printf("%03d", motor_speed);
  // Output motor speed to motor ESC in degree 0 to 180
  servo1.write(sp);
  delay(20);
}

//
// Call back function by inturrapt every lap
//
void senceRot(){
  // Increment rotation counter if the interrupt happens after 5ms or more since the previous interrupt.
  ct = millis();
  if((ct - pt) > 5)
  {
    lapCount++;
  }
  pt = ct;
}