Created
June 24, 2020 19:07
-
-
Save kkmonster/eb042d2140664bc08fdc1f40dcc78cef to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// 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