Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
// max velocity 1000 mm /s
// max acc 10 m /s^2
#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);
// 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(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()
{
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;
static int counter, toggle, toggle_add = 1;
static float C2_u, ref_2;
toggle += toggle_add;
if (toggle > 1000)
{
toggle_add = -toggle_add;
ref_2 = -0.08;
}
if (toggle < -1000)
{
toggle_add = -toggle_add;
ref_2 = 0.08;
}
// ref_2 = 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;
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)
{
//// 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(cart_position*100, 4); // cm
Serial.print(", ");
Serial.print(-(angle - PI) * 180 / PI, 4); // -dregee
Serial.println();
}
while (millis() < prevt) //constant sampling frequncy
;
prevt = millis() + dt;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment