v3_inverted_pendulum.ino

// wasin wongkum demo inverted pendulum   on nano32(ESP32)

#include <ESP32Encoder.h> // https://github.com/madhephaestus/ESP32Encoder
#include <math.h>
hw_timer_t *timer = NULL;
hw_timer_t *timer1 = NULL;

portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
ESP32Encoder encoder;

const int encoderA_pin = 21;
const int encoderB_pin = 22;
const int encoder_CPR = 2400;

const int user_button_pin = 19;

const int driver_dir_pin = 14;
const int driver_step_pin = 12;
const int driver_EN_PIN = 13;

const int limit_A = 34;
const int limit_B = 35;

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;

const int dt = 5;                      // ms
const float J_inertial = 0.0005242317; // kg.m^2
const float C_damp = 0;                // N.m/(rad/s)
const float M_mass = 0.02791;          // kg
const float R_cg = 0.115;              // m
const float gravity = 9.81;            // m/s^2

volatile float theta = 0;        // rad
volatile float angular_velo = 0; // rad/s

static volatile float w_tmp1, w_tmp2, w_tmp3, w_tmp4;

void IRAM_ATTR update_angular_velo(void)
{
  static volatile int32_t cnt;
  portENTER_CRITICAL_ISR(&timerMux);
  timerWrite(timer1, 0); // reset encoder wdt

  float w_tmp = (float)timerRead(timer) / 40000000.0f;
  timerWrite(timer, 0);

  if (w_tmp > 0.0000001)
  {

    w_tmp = 2.0f * PI / encoder_CPR / w_tmp;
    if (cnt < encoder.getCount())
    {
      w_tmp = -w_tmp;
    }
    cnt = encoder.getCount();

    w_tmp4 = w_tmp3;
    w_tmp3 = w_tmp2;
    w_tmp2 = w_tmp1;
    w_tmp1 = w_tmp;
  }
  portEXIT_CRITICAL_ISR(&timerMux);
}

void IRAM_ATTR onTimer(void)
{
  portENTER_CRITICAL_ISR(&timerMux);
  w_tmp4 = w_tmp3;
  w_tmp3 = w_tmp2;
  w_tmp2 = w_tmp1;
  w_tmp1 = 0;
  portEXIT_CRITICAL_ISR(&timerMux);
}

float count_to_rad(int32_t cnt)
{
  float rad = (float)cnt * 2.0f * PI / (float)encoder_CPR;
  return rad;
}

void setup()
{
  // put your setup code here, to run once:
  pinMode(driver_dir_pin, OUTPUT);

  pinMode(encoderA_pin, INPUT_PULLUP);
  pinMode(encoderB_pin, INPUT_PULLUP);

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

  timer = timerBegin(0, 2, true);
  timerAlarmWrite(timer, UINT32_MAX, true);

  timerAlarmEnable(timer);
  attachInterrupt(digitalPinToInterrupt(encoderA_pin), update_angular_velo, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderB_pin), update_angular_velo, CHANGE);

  timer1 = timerBegin(1, 80, true);
  timerAttachInterrupt(timer1, &onTimer, true);
  timerAlarmWrite(timer1, dt * 1000, true);
  timerAlarmEnable(timer1);

  pinMode(user_button_pin, INPUT_PULLUP);

  Serial.begin(115200);
}

uint32_t prevt = 0;

void loop()
{
  portENTER_CRITICAL_ISR(&timerMux);

  /**
 * 
    const int dt = 5;                      // ms
    const float J_inertial = 0.0005242317; // kg.m^2
    const float C_damp = 0;                // N.m/(rad/s)
    const float M_mass = 0.02791;          // kg
    const float R_cg = 0.115;              // m
    const float gravity = 9.81;            // m/s^2
 * 
 **/

  angular_velo = (w_tmp1 + w_tmp2 + w_tmp3 + w_tmp4) / 4;
  theta = count_to_rad(encoder.getCount());

  float Ek = 0.5f * (J_inertial)*angular_velo * angular_velo * 1000; //mJ

  float Ep = M_mass * gravity * R_cg * (-cosf(theta) - 1) * 1000; //mJ

  float E_total = Ek + Ep;

  Serial.print(Ek);
  Serial.print(",    ");
  Serial.print(Ep);
  Serial.print(",    ");
  Serial.print(E_total);
  Serial.println();

  portEXIT_CRITICAL_ISR(&timerMux);

  while (millis() < prevt) //constant sampling frequncy
    ;
  prevt = millis() + dt;
}