inner_loop_PID_pendulum.ino

#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 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 = 1;

#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;

void IRAM_ATTR limit_sw_lock(void)
{
  portENTER_CRITICAL_ISR(&sync);

  ledcSetup(channel, driver_frequncy, 0);
  limit_locking = 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;
}

void setup()
{
  delay(100);

  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(0, 2, true);
  timerAlarmWrite(timer, UINT32_MAX, true);
  timerAlarmEnable(timer);

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

  pinMode(user_button_pin, INPUT_PULLUP);

  ledcSetup(channel, freq, resolution);
  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()
{

  float angle = count_to_rad(encoder.getCount());

  while (angle < 0)
    angle += 2 * PI;
  while (angle >= 2 * PI)
    angle -= 2 * PI;

  float error_temp = error;

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

  Serial.println(angle / PI * 180);

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

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

  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 minimum speed
  if (fabs(cart_velo) > 0.001f)
  {
    driver_frequncy = fabs(cart_velo) * microstep * 5 * 1000.0f;

    if (driver_frequncy < 1)
      driver_frequncy = 1;

    if (limit_locking == 0)
      ledcSetup(channel, driver_frequncy, 2);
    else
    {
      if (digitalRead(limit_A) && digitalRead(limit_B))
        limit_locking = 0;
    }
    //// Control Direction
    if (cart_velo > 0)
      digitalWrite(driver_dir_pin, HIGH);
    else
      digitalWrite(driver_dir_pin, LOW);
    ledcWrite(channel, 1);
  }
  else
    ledcWrite(channel, 0);

  //// User button reset
  if (digitalRead(user_button_pin) == 1)
  {
    cart_ac = 0;
    cart_velo = 0;
    cart_position = 0;
    limit_locking = 0;
    error_sum = 0;
    sum_offset = 0;
    digitalWrite(driver_EN_PIN, HIGH); //disable driver
  }
  else
    digitalWrite(driver_EN_PIN, LOW); //enable driver

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