update arduino/Betas/RGB_V1.1.1/main/main.ino.

增加debug功能,方便webserver端查看调试信息
master
慕炎 2022-01-02 16:12:16 +00:00 committed by Gitee
parent b97158bc08
commit d99e32ca0d
No known key found for this signature in database
GPG Key ID: 173E9B9CA92EEF8F
1 changed files with 71 additions and 21 deletions

View File

@ -1,7 +1,9 @@
/** /**
* RGB HW:Ver 1.5 FW:Ver 1.1.1 RGB HW:Ver 1.5 FW:Ver 1.1.1
EDA https://oshwhub.com/muyan2020/zi-ping-heng-di-lai-luo-san-jiao_10-10-ban-ben_copy
RGB https://gitee.com/muyan3000/RGBFOC 基于45°(https://gitee.com/coll45/foc/)程序修改 RGB https://gitee.com/muyan3000/RGBFOC 基于45°(https://gitee.com/coll45/foc/)程序修改
arduino-FOChttps://gitee.com/ream_d/Deng-s-foc-controller并安装Kalman。 arduino-FOChttps://gitee.com/ream_d/Deng-s-foc-controller并安装Kalman。
FOC32, 33, 25, 22 22enable FOC32, 33, 25, 22 22enable
AS5600 SDA-23 SCL-5 MPU6050 SDA-19 SCL-18 AS5600 SDA-23 SCL-5 MPU6050 SDA-19 SCL-18
FLAG_V1使0LQR使K1K2LQR使K3K4 FLAG_V1使0LQR使K1K2LQR使K3K4
@ -63,8 +65,10 @@ const char* username = "admin"; //web用户名
const char* userpassword = "12345678"; //web用户密码 const char* userpassword = "12345678"; //web用户密码
const char* ServerName = "ESP32-LELO-RGB"; const char* ServerName = "ESP32-LELO-RGB";
char DateTimeStr[20] = "1970-01-01 00:00:00"; char DateTimeStr[20] = "1970-01-01 00:00:00";
char Debug_Log[8192] = ""; char Debug_Log[255][255];
bool log_control = 1; uint32_t loop_time_begin = millis();
int debug_times;
bool log_control = 0, debug_log_control = 0;
WebServer ESP32Server(80); WebServer ESP32Server(80);
@ -213,6 +217,20 @@ void do_K43(char* cmd) {
comm.scalar(&LQR_K4_3, cmd); comm.scalar(&LQR_K4_3, cmd);
} }
void Debug_Log_func(String debuglog, bool debug_begin = 0) {
uint32_t loop_time_end;
if (debug_log_control) {
if (debug_begin) {
loop_time_begin = millis();
sprintf(Debug_Log[debug_times], "%s Begin time:%d", debuglog.c_str(), loop_time_begin);
} else {
loop_time_end = millis();
sprintf(Debug_Log[debug_times], "%s\t%s End time:%d\tProcessed in %d ms\tFreeHeap:%d\r\n", Debug_Log[debug_times], debuglog.c_str(), loop_time_end, (loop_time_end - loop_time_begin), ESP.getFreeHeap());
debug_times++;
}
}
}
void onPacketCallBack(AsyncUDPPacket packet) void onPacketCallBack(AsyncUDPPacket packet)
{ {
char* da; char* da;
@ -224,6 +242,8 @@ void onPacketCallBack(AsyncUDPPacket packet)
} }
// instantiate the commander // instantiate the commander
void setup() { void setup() {
Debug_Log_func("Before setup");
Debug_Log_func("setup", 1);
Serial.begin(115200); Serial.begin(115200);
pinMode(ACTIVE_PIN, OUTPUT); pinMode(ACTIVE_PIN, OUTPUT);
@ -244,6 +264,7 @@ void setup() {
Serial.println(ESP.getMaxAllocHeap()); Serial.println(ESP.getMaxAllocHeap());
//获取芯片可用内存 //获取芯片可用内存
printf("esp_get_free_heap_size : %d \n", esp_get_free_heap_size()); printf("esp_get_free_heap_size : %d \n", esp_get_free_heap_size());
printf("getFreeHeap : %d \n", ESP.getFreeHeap());
//获取从未使用过的最小内存 //获取从未使用过的最小内存
printf("esp_get_minimum_free_heap_size : %d \n", esp_get_minimum_free_heap_size()); printf("esp_get_minimum_free_heap_size : %d \n", esp_get_minimum_free_heap_size());
@ -435,10 +456,13 @@ void setup() {
motor.PID_velocity.I = v_i_1; motor.PID_velocity.I = v_i_1;
//最大电机限制电压 //最大电机限制电压
motor.voltage_limit = 12; motor.voltage_limit = 6;
//速度低通滤波时间常数 //速度低通滤波时间常数
motor.LPF_velocity.Tf = 0.02; motor.LPF_velocity.Tf = 0.01f;
// angle P controller
motor.P_angle.P = 20;
//设置最大速度限制 //设置最大速度限制
motor.velocity_limit = 40; motor.velocity_limit = 40;
@ -469,17 +493,17 @@ void setup() {
Serial.println("System is ready"); Serial.println("System is ready");
Serial.println("-----------------------------------------------"); Serial.println("-----------------------------------------------");
Debug_Log_func("setup");
} }
char buf[255]; char buf[255];
long loop_count = 0; long loop_count = 0;
double last_pitch; double last_pitch;
void loop() { void loop() {
Debug_Log_func("loop", 1);
ArduinoOTA.handle(); ArduinoOTA.handle();
motor.loopFOC(); motor.loopFOC();
TenthSecondsSinceStartTask();
ESP32Server.handleClient();
delay(2);//allow the cpu to switch to other tasks
while (i2cRead(0x3B, i2cData, 14)); while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
@ -584,6 +608,8 @@ void loop() {
touchAttach(3, T4); touchAttach(3, T4);
TenthSecondsSinceStartTask();
//单击事件处理 //单击事件处理
if (touch_touched[1]) { if (touch_touched[1]) {
//Serial.print("\nLight1 touched "); //Serial.print("\nLight1 touched ");
@ -608,6 +634,7 @@ void loop() {
FastLED.show(); FastLED.show();
} }
Debug_Log_func("loop");
} }
/* mpu6050加速度转换为角度 /* mpu6050加速度转换为角度
acc2rotation(ax, ay) acc2rotation(ax, ay)
@ -656,18 +683,18 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
last_unstable_time = millis(); last_unstable_time = millis();
if (stable) //如果是稳态进入非稳态则调整为目标角度 if (stable) //如果是稳态进入非稳态则调整为目标角度
{ {
target_angle = EEPROM.readFloat(0); target_angle = EEPROM.readFloat(0) - p_angle;
stable = 0; stable = 0;
} }
} }
if ((millis() - last_unstable_time) > 500 && !stable) //非稳态进入稳态超过500ms检测更新目标角为目标角+摆角,假设进入稳态 if ((millis() - last_unstable_time) > 500 && !stable) //非稳态进入稳态超过500ms检测更新目标角为目标角+摆角,假设进入稳态
{ {
target_angle -= _sign(target_velocity) * 0.2; target_angle -= _sign(target_velocity) * 0.4;
stable = 1; stable = 1;
} }
if ((millis() - last_stable_time) > 2500 && stable) { //稳态超过2000ms检测更新目标角 if ((millis() - last_stable_time) > 2500 && stable) { //稳态超过2000ms检测更新目标角
if (abs(target_velocity) > 2.5 && abs(target_velocity) < 15) { //稳态速度偏大校正 if (abs(target_velocity) > 3 && abs(target_velocity) < 10) { //稳态速度偏大校正
last_stable_time = millis(); last_stable_time = millis();
target_angle -= _sign(target_velocity) * 0.2; target_angle -= _sign(target_velocity) * 0.2;
} }
@ -707,6 +734,8 @@ unsigned long LastMillis = 0;
void TenthSecondsSinceStartTask() //100ms void TenthSecondsSinceStartTask() //100ms
{ {
unsigned long CurrentMillis = millis(); unsigned long CurrentMillis = millis();
ESP32Server.handleClient();
delay(1);//allow the cpu to switch to other tasks
if (abs(int(CurrentMillis - LastMillis)) > 100) if (abs(int(CurrentMillis - LastMillis)) > 100)
{ {
LastMillis = CurrentMillis; LastMillis = CurrentMillis;
@ -856,6 +885,10 @@ String ProcessUpdate() //页面更新
ReturnString += ","; ReturnString += ",";
ReturnString += TimeString(millis() / 1000); ReturnString += TimeString(millis() / 1000);
ReturnString += ",";
ReturnString += log_control;
ReturnString += ",";
ReturnString += debug_log_control;
ReturnString += ","; ReturnString += ",";
ReturnString += bat_voltage; ReturnString += bat_voltage;
if (log_control) { if (log_control) {
@ -891,6 +924,18 @@ String ProcessUpdate() //页面更新
ReturnString += v_i_2; ReturnString += v_i_2;
ReturnString += ","; ReturnString += ",";
ReturnString += v_p_2; ReturnString += v_p_2;
ReturnString += ",";
if (debug_log_control) {
for (int i = 0; i < debug_times; i++) {
if (String(Debug_Log[i]) != "") {
//Serial.println(Debug_Log[i]);
ReturnString += Debug_Log[i];
memset( Debug_Log[debug_times], 0, sizeof(Debug_Log[debug_times]) );
}
if (i == debug_times - 1)
debug_times = 0;
}
}
//Serial.println(ReturnString); //Serial.println(ReturnString);
return ReturnString; return ReturnString;
} }
@ -953,11 +998,16 @@ void PocessControl(int DeviceType, int DeviceIndex, int Operation, float Operati
printf("Reboot..."); printf("Reboot...");
esp_restart(); esp_restart();
} }
} else if (DeviceIndex == 5) { } else if (DeviceIndex == 5) { //参数记录输出控制
if (Operation % SysIndex == 0) if (Operation % SysIndex == 0)
log_control = 0; log_control = 0;
else if (Operation % SysIndex == 1) else if (Operation % SysIndex == 1)
log_control = 1; log_control = 1;
} else if (DeviceIndex == 6) { //DEBUG输出控制
if (Operation % SysIndex == 0)
debug_log_control = 0;
else if (Operation % SysIndex == 1)
debug_log_control = 1;
} }
} }
@ -966,7 +1016,7 @@ void PocessControl(int DeviceType, int DeviceIndex, int Operation, float Operati
if (Operation == 0) if (Operation == 0)
{ {
sprintf(do_commd, "%.2f", Operation2); sprintf(do_commd, "%.2f", Operation2);
Serial.println(do_commd); //Serial.println(do_commd);
if (DeviceIndex == 0) //期望角度TA if (DeviceIndex == 0) //期望角度TA
{ {
do_TA(do_commd); do_TA(do_commd);
@ -1061,7 +1111,7 @@ void handleNotFound()
Operation = 0; Operation = 0;
} }
printf("DeviceType:%d DeviceIndex:%d Operation:%d Operation2:%.2f\n", DeviceType, DeviceIndex, Operation, Operation2 ); //printf("DeviceType:%d DeviceIndex:%d Operation:%d Operation2:%.2f\n", DeviceType, DeviceIndex, Operation, Operation2 );
PocessControl(DeviceType, DeviceIndex, Operation, Operation2); PocessControl(DeviceType, DeviceIndex, Operation, Operation2);
} }