Inverted Pendulum EP.4 : Actuation

……มาถึงคลิปชุดการเรียนรู้ Inverted Pendulum(ลูกตุ้มกลับหัว) ตอนที่ 4 กันแล้ว จากเมื่อตอนก่อนหน้าเราได้เขียนโปรแกรมวัดค่าและแสดงผลพลังงานภายใน Pendulum ในตอนนี้เราจะทำการเขียนโปรแกรมควบคุมการเคลื่อนที่ของ Cart ให้พลังงานภายใน Pendulum เพิ่มขึ้น เพื่อแกว่ง Pendulum ขึ้นไปยังจุดสูงสุดกันครับ….

ตัวอย่าง Code old version

#include <ESP32Encoder.h>
#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 = 25;
const int encoderB_pin = 26;

const int encoder_CPR = 1600;
const int user_button_pin = 5;

const int driver_dir_pin = 17;
const int driver_step_pin = 16;
const int driver_EN_PIN = 4;

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;

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

#define EN_PIN 4    //
#define DIR_PIN 17  //
#define STEP_PIN 16 //
#define CS_PIN 23   //
#define SCK_PIN 19  //
#define SDI_PIN 12  //
#define SDO_PIN 14  //

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

#include <TMC2130Stepper.h>
TMC2130Stepper driver = TMC2130Stepper(EN_PIN, DIR_PIN, STEP_PIN, CS_PIN, SDI_PIN, SDO_PIN, SCK_PIN);

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;

void IRAM_ATTR read_angle(void)
{
  static volatile 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 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 = -PI * 0.00125f / 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_angle(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;
}

int microstep = 256;

void setup()
{

  delay(100);
  pinMode(CS_PIN, OUTPUT);
  digitalWrite(CS_PIN, HIGH);
  delay(100);

  driver.begin();          // Initiate pins and registeries
  driver.rms_current(600); // Set stepper current to 600mA. The command is the same as command TMC2130.setCurrent(600, 0.11, 0.5);
  driver.stealthChop(1);   // Enable extremely quiet stepping
  driver.stealth_autoscale(1);
  driver.microsteps(microstep);

  // 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);

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

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

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

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

  ledcSetup(channel, freq, resolution);
  ledcAttachPin(driver_step_pin, channel);
  ledcWrite(channel, 2);

  Serial.begin(115200);
}

uint32_t prevt = 0;

void loop()
{
  // Ploting pendulun's internal energy
  portENTER_CRITICAL_ISR(&timerMux);

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

  /////////////////////////////////////////////////////////////////////////////
  // 0.2 m/s^2  ~  ถึง 0-100 km/hr in 139 s
  // 0.4 m/s^2  ~  ถึง 0-100 km/hr in 70  s
  // 0.8 m/s^2  ~  ถึง 0-100 km/hr in 35  s

  cart_ac_max = 0.8; // m/s^2

  cart_ac = -cart_ac_max * sgn(angular_velo) * sgn(-cosf(theta)); // m/s^2   From control rule

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

  cart_position = cart_position + cart_velo * (float)dt * 0.001; // m
  cart_velo = cart_velo + cart_ac * (float)dt * 0.001;           // m/s

  // Control minimum speed
  if (fabs(cart_velo) > 0)
  {

    driver_frequncy = fabs(cart_velo) * microstep * 5 * 1000.0f;
    ledcSetup(channel, driver_frequncy, 1);
    //// 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;
    digitalWrite(driver_EN_PIN, HIGH);
  }
  else
    digitalWrite(driver_EN_PIN, LOW);

  /////////////////////////////////////////////////////////////////////////////

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

  Serial.print(10 * cart_ac);
  Serial.print(",    ");
  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;
}

ตัวอย่าง Code New version สำหรับชุดคิท

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

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 = 2;

#define EN_PIN 4    //
#define DIR_PIN 17  //
#define STEP_PIN 16 //
#define CS_PIN 23   //
#define SCK_PIN 19  //
#define SDI_PIN 12  //
#define SDO_PIN 14  //

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

void IRAM_ATTR read_angle(void)
{
  static volatile 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 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);
}

void IRAM_ATTR limit_sw_lock(void) // limit locking interrupt function
{
  ledcSetup(channel, driver_frequncy, 0);
  limit_locking = 1;
}

float count_to_angle(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;
}

int microstep = 64; //fixed value

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); // reverse encoder direction
  encoder.setCount(0);

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

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

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

  attachInterrupt(digitalPinToInterrupt(limit_A), limit_sw_lock, FALLING); // add stopping limit switch
  attachInterrupt(digitalPinToInterrupt(limit_B), limit_sw_lock, FALLING);

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

  pinMode(user_button_pin, INPUT_PULLUP);

  ledcSetup(channel, freq, resolution);
  ledcAttachPin(driver_step_pin, channel);
  ledcWrite(channel, 2);

  Serial.begin(115200);
}

uint32_t prevt = 0;

void loop()
{
  // Ploting pendulun's internal energy
  portENTER_CRITICAL_ISR(&timerMux);

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

  /////////////////////////////////////////////////////////////////////////////
  // 0.2 m/s^2  ~  ถึง 0-100 km/hr in 139 s
  // 0.4 m/s^2  ~  ถึง 0-100 km/hr in 70  s
  // 0.8 m/s^2  ~  ถึง 0-100 km/hr in 35  s

  cart_ac_max = 0.8; // m/s^2

  cart_ac = -cart_ac_max * sgn(angular_velo) * sgn(-cosf(theta)); // m/s^2   From control rule

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

  cart_position = cart_position + cart_velo * (float)dt * 0.001; // m
  cart_velo = cart_velo + cart_ac * (float)dt * 0.001;           // 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;

    digitalWrite(driver_EN_PIN, HIGH);
  }
  else
    digitalWrite(driver_EN_PIN, LOW);

  /////////////////////////////////////////////////////////////////////////////

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

  Serial.print(10 * cart_ac);
  Serial.print(",    ");
  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;
}

สนใจชุดคิทชุดนี้นะสามารถเข้าไปดูรายละเอียดได้ที่ https://www.aiiotshop.com/product/62/control-system-linear-inverted-pendulum-experiment-system