finding wm of pendulun by esp32

finding_wn.ino

#include <ESP32Encoder.h>

hw_timer_t *timer = NULL;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
ESP32Encoder encoder;

const int encoderA_pin = 25;
const int encoderB_pin = 26;

const int encoder_CPR = 1600;
int peroid_time = 0;

volatile uint32_t Counter = 0;
volatile uint8_t printing = 0;
volatile float Td = 0;
volatile float xt1 = 0;
volatile float xt2 = 0;

void IRAM_ATTR read_angle(void)
{
  static int32_t tmp, tmp1, tmp2, tmp3;
  portENTER_CRITICAL_ISR(&timerMux);
  tmp3 = tmp2;
  tmp2 = tmp1;
  tmp1 = encoder.getCount();

  if (tmp2 > tmp3 && tmp2 > tmp1)
  {
    printing = 1;
    Counter = tmp1;
  }

  if (tmp1 == 0 && tmp > 0)
  {
    printing = 2;
    Td = (float)timerRead(timer) / 40000000.0f;
    timerWrite(timer, 0);
  }
  tmp = tmp1;
  portEXIT_CRITICAL_ISR(&timerMux);
}

void IRAM_ATTR onTimer(void)
{
  
}

float count_to_angle(int32_t cnt)
{
  float angle = (float)cnt * 360.0f / (float)encoder_CPR;
  return angle;
}

void setup()
{
  // put your setup code here, to run once:
  pinMode(encoderA_pin, INPUT_PULLUP);
  pinMode(encoderB_pin, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(encoderA_pin), read_angle, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderB_pin), read_angle, CHANGE);

  encoder.setCount(0);
  encoder.attachFullQuad(encoderA_pin, encoderB_pin);

  timer = timerBegin(0, 2, true);
  timerAttachInterrupt(timer, &onTimer, true);
  timerAlarmWrite(timer, UINT32_MAX, true);
  timerAlarmEnable(timer);

  Serial.begin(115200);
}

void loop()
{
  // put your main code here, to run repeatedly:
  delay(10);

  if (printing == 1)
  {
    portENTER_CRITICAL_ISR(&timerMux);
    printing = 0;
    portEXIT_CRITICAL_ISR(&timerMux);

    xt1 = xt2;
    xt2 = count_to_angle(Counter) * (PI / 180.0f);
  }
  if (printing == 2)
  {
    portENTER_CRITICAL_ISR(&timerMux);
    printing = 0;
    portEXIT_CRITICAL_ISR(&timerMux);

    float delta = log(xt1 / xt2);
    float damping_ratio = delta / (sqrtf(4.0f * PI * PI + delta * delta));
    float Wn = 2.0f * PI / (Td * sqrtf(1 - damping_ratio * damping_ratio));
    float Wd = 2.0f * PI / Td;

    Serial.print("x(t)=");
    Serial.print(xt1, 5);
    Serial.print("rad  x(t+Td)=");
    Serial.print(xt2, 5);
    Serial.print("rad  Td=");
    Serial.print(Td, 5);
    Serial.print("s  d=");
    Serial.print(delta, 5);
    Serial.print("  damping_ratio=");
    Serial.print(damping_ratio, 5);
    Serial.print("  Wn=");
    Serial.print(Wn, 5);
    Serial.print("rad/s   Wd=");
    Serial.print(Wd, 5);
    Serial.println("rad/s");
  }
  // Serial.println(count_to_angle(encoder.getCount()));
  // Serial.println(encoder.getCount());
}