Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
finding wm of pendulun by esp32
#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());
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment