https://www.aiiotshop.com/p/62

demo_ep8_inverted_pendulum.ino

#define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))

#define off 0
#define on 1

#include "Arduino.h"
#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;
portMUX_TYPE sync = 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 vr1_button_pin = 32;
const int vr2_button_pin = 33;
const int vr3_button_pin = 25;

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;

volatile float limit_locking = 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;

int freq = 1;
int channel = 0;
int resolution = 3;

#define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))

float cart_ac_max = 0;
float cart_ac = 0;
float cart_velo = 0;
float cart_position = 0;

float Ek = 0;      //mJ
float Ep = 0;      //mJ
float E_total = 0; //mJ
float driver_frequncy = 0;

int microstep = 64;

volatile uint8_t en_controller = 0;
volatile uint8_t AB_lock = 0;
volatile uint8_t AB_lock_anti_bounce = 0;

volatile uint16_t auto_stop = 0; // auto stop when run in Stabilzer 60 sec

uint8_t on_off = 0;
uint8_t bt_state = 0;

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.000006)
  {
    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)
{

  w_tmp4 = w_tmp3;
  w_tmp3 = w_tmp2;
  w_tmp2 = w_tmp1;
  w_tmp1 = 0; //manual set angular_velo to zero
}

void IRAM_ATTR limit_sw_lock(void)
{
  if (!AB_lock_anti_bounce)
  {
    portENTER_CRITICAL_ISR(&sync);
    ledcSetup(channel, driver_frequncy, 0);
    ledcWrite(channel, 0);
    cart_velo = 0;
    limit_locking = 1;

    cart_position = -0.35 / 2;
    AB_lock = 1;
    if (digitalRead(limit_B) == 0)
    {
      cart_position = 0.35 / 2;
      AB_lock = 2;
    }
    AB_lock_anti_bounce = 1;
    portEXIT_CRITICAL_ISR(&sync);
  }
}

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

float count_to_rad(int32_t cnt)
{
  float rad = (float)cnt * 2.0f * PI / (float)encoder_CPR;
  return rad;
}
float swing_up(float angle, float cart_position, float cart_velo);
float Stabilzer(float cart_ref, float angle, float cart_position, boolean en_controller);

void setup()
{

  delay(100);

  // put your setup code here, to run once:
  pinMode(driver_dir_pin, OUTPUT);
  pinMode(driver_EN_PIN, OUTPUT);
  pinMode(encoderA_pin, INPUT_PULLUP);
  pinMode(encoderB_pin, INPUT_PULLUP);

  pinMode(limit_A, INPUT_PULLUP);
  pinMode(limit_B, INPUT_PULLUP);

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

  timer = timerBegin(2, 2, true); // change to use timer 2
  timerAlarmWrite(timer, UINT32_MAX, true);
  timerAlarmEnable(timer);

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

  attachInterrupt(digitalPinToInterrupt(limit_A), limit_sw_lock, FALLING);
  attachInterrupt(digitalPinToInterrupt(limit_B), limit_sw_lock, FALLING);

  timer1 = timerBegin(1, 80, true); // change to use timer 1
  timerAttachInterrupt(timer1, &onTimer, true);
  timerAlarmWrite(timer1, dt * 1000, true);
  timerAlarmEnable(timer1);

  pinMode(user_button_pin, INPUT_PULLUP);

  ledcSetup(channel, freq, resolution); // change to use timer 0
  ledcAttachPin(driver_step_pin, channel);
  ledcWrite(channel, 2);

  Serial.begin(115200);
  Serial.print("start");
}

uint32_t prevt = 0;
static float error, error_dot, error_sum, offset, offset_dot, sum_offset, ref_2;

void loop()
{
  cart_position = cart_position + cart_velo * (float)dt * 0.001; // m
  float angle = count_to_rad(encoder.getCount());
  while (angle < 0)
    angle += 2 * PI;
  while (angle >= 2 * PI)
    angle -= 2 * PI;

  angular_velo = (w_tmp1 + w_tmp2 + w_tmp3 + w_tmp4) / 4;

  Ek = 0.5f * (J_inertial)*angular_velo * angular_velo * 1000; //mJ
  Ep = M_mass * gravity * R_cg * (-cosf(angle) - 1) * 1000;    //mJ
  E_total = Ek + Ep;

  float controller_output = Stabilzer(0, angle, cart_position, en_controller);

  if (fabs(PI - angle) * 180 / PI < 9 && E_total < 0.5)
  {
    cart_ac = controller_output;
    en_controller = 1;
    if (auto_stop > 0)
      auto_stop--;
    else
    {
      on_off = off; //turn off system
    }
  }
  else
  {
    cart_ac = swing_up(angle, cart_position, cart_velo);
    en_controller = 0;
    auto_stop = 200 * 60; // countdown to stop // 200 hz * time(1 * 60 sec)
  }

  if (AB_lock == 0)
  {
    cart_ac = cart_ac;
  }
  else if (AB_lock == 2 && cart_ac > 0)
  {
    cart_ac = 0;
  }
  else if (AB_lock == 1 && cart_ac < 0)
  {
    cart_ac = 0;
  }

  cart_ac = constrain(cart_ac, -7.0, 7.0);                                   // m/s/s
  cart_velo = constrain(cart_velo + cart_ac * (float)dt * 0.001, -0.7, 0.7); // m/s

  // Control speed of cart
  if (fabs(cart_velo) > 0.001f) // limit minimum speed at 1 mm/s
  {
    driver_frequncy = fabs(cart_velo) * microstep * 5 * 1000.0f;

    if (AB_lock == 0)
    {
      ledcSetup(channel, driver_frequncy, 3); // increase pwm resolution to 3 bit
      ledcWrite(channel, 4);                  // 50% duty = 2^3 / 2 = 4
    }
    else if (AB_lock == 1 && cart_velo > 0)
    {
      ledcSetup(channel, driver_frequncy, 3);
      ledcWrite(channel, 4);
    }
    else if (AB_lock == 2 && cart_velo < 0)
    {
      ledcSetup(channel, driver_frequncy, 3);
      ledcWrite(channel, 4);
    }
    else
    {
      cart_velo = 0;
      ledcWrite(channel, 0);
    }

    if (digitalRead(limit_A) && digitalRead(limit_B))
    {
      AB_lock = 0;
      limit_locking = 0;
      AB_lock_anti_bounce = 0;
    }

    //// Control Direction
    if (cart_velo > 0)
      digitalWrite(driver_dir_pin, HIGH);
    else
      digitalWrite(driver_dir_pin, LOW);
  }
  else
    ledcWrite(channel, 0);

  //// User button reset
  if (on_off == off)
  {
    //// reset all state
    cart_ac = 0;
    cart_velo = 0;
    cart_position = 0;
    limit_locking = 0;
    error_sum = 0;
    sum_offset = 0;
    digitalWrite(driver_EN_PIN, HIGH);
  }
  else
    digitalWrite(driver_EN_PIN, LOW);

  static uint16_t cnnt;
  cnnt++;
  if (cnnt >= 2)
  {
    cnnt = 0;
    Serial.print(E_total, 2); // mJ
    // Serial.print(",    ");
    // Serial.print(cart_position * 100, 2); // cm
    // Serial.print(",    ");
    // Serial.print(-(angle - PI) * 180 / PI, 2); // -dregee
    Serial.println();
  }
  while (millis() < prevt) //constant sampling frequncy
    ;
  prevt = millis() + dt;

  bt_state = (bt_state << 1) | digitalRead(user_button_pin);

  if ((bt_state & 0x03) == 2)
  {
    on_off = 1 - on_off;
  }
}

float Stabilzer(float cart_ref, float angle, float cart_position, boolean en_controller)
{

  static int counter, toggle, toggle_add = 1;
  static float C2_u, ref_2;

  ref_2 = -cart_ref;

  if (en_controller == 0)
  {
    //// reset intergal term
    error_sum = 0;
    sum_offset = 0;
  }

  counter++;
  if (counter >= 10)
  {
    float ckp = 0.1921;
    float cki = 0.0586;
    float ckd = 0.1465;

    float offset_temp = offset;

    offset = ref_2 + cart_position;
    offset_dot = (offset - offset_temp) * 1000 / dt / counter;
    sum_offset = sum_offset + offset / 1000 * dt * counter;

    C2_u = (offset * ckp) + (offset_dot * ckd) + (sum_offset * cki);
    counter = 0;
  }

  float error_temp = error;
  error = (C2_u + PI - angle);
  error_dot = (error - error_temp) * 1000 / dt;
  error_sum = error_sum + error * dt / 1000;

  float pkp = 10.04 * 5.2267;
  float pkd = 1 * 5.2267;
  float pki = 8 * 5.2267;

  float cart_ac = pkp * error + pkd * error_dot + error_sum * pki;

  return cart_ac;
}

float swing_up(float angle, float cart_position, float cart_velo)
{

  cart_ac_max = 6; // m/s^2

  // m/s^2   From control rule and maximize acc whem angle near to 0 and PI
  float cart_ac = -cart_ac_max * sgn(-angular_velo) * sgn(-cosf(angle)) * fabs(cosf(angle)); // fixed angular velocity dirr

  // start condition
  if ((angle > 357 * PI / 180 || angle < 3 * PI / 180) && angular_velo == 0)
    cart_ac = 1 - 2 * (millis() % 2);

  // Energu control
  if (E_total > 0.5)
    cart_ac = -cart_ac;
  else if (E_total >= 0)
    cart_ac = 0;

  // making the cart keeping stay started point, + acc25% direction to center
  const float acc_offset = 0.25;
  if (cart_position > 0.1)
  {
    if (cart_ac > 0)
      cart_ac *= (1.0f - acc_offset);
    else
      cart_ac *= (1.0f + acc_offset);
  }
  else if (cart_position < -0.1)
  {
    if (cart_ac < 0)
      cart_ac *= (1.0f - acc_offset);
    else
      cart_ac *= (1.0f + acc_offset);
  }

  // making the cart keeping low speed
  if (cart_ac > 0)
    cart_ac *= (1.0f + (0 - cart_velo) * 2.6);
  else
    cart_ac *= (1.0f + (0 + cart_velo) * 2.6);

  return cart_ac;
}