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