zcy
/
foc
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

master
慕炎 2022-01-03 05:46:58 +00:00 committed by Gitee
parent 0f4ffde5fb
commit a149cb4556
No known key found for this signature in database
GPG Key ID: 173E9B9CA92EEF8F
1 changed files with 25 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
arduino/Betas/RGB_V1.1.1/main/main.ino
View File

@ -217,17 +217,12 @@ 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) { void Debug_Log_func(String debuglog, bool debug_control=debug_log_control) {
uint32_t loop_time_end; if (debug_control) {
if (debug_log_control) { uint32_t tmp_loop_time_begin = millis();
if (debug_begin) { sprintf(Debug_Log[debug_times], "%s\r\nBegin time:%d\tEnd time:%d\tProcessed in %d ms\tFreeHeap:%d\r\n%s", Debug_Log[debug_times], loop_time_begin, tmp_loop_time_begin, (tmp_loop_time_begin - loop_time_begin), ESP.getFreeHeap(), debuglog.c_str());
loop_time_begin = millis(); loop_time_begin = tmp_loop_time_begin;
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++; debug_times++;
}
} }
} }
@ -242,8 +237,7 @@ void onPacketCallBack(AsyncUDPPacket packet)
} }
// instantiate the commander // instantiate the commander
void setup() { void setup() {
Debug_Log_func("Before setup"); Debug_Log_func("Before setup",1);
Debug_Log_func("setup", 1);
Serial.begin(115200); Serial.begin(115200);
pinMode(ACTIVE_PIN, OUTPUT); pinMode(ACTIVE_PIN, OUTPUT);
@ -494,14 +488,14 @@ void setup() {
Serial.println("System is ready"); Serial.println("System is ready");
Serial.println("-----------------------------------------------"); Serial.println("-----------------------------------------------");
Debug_Log_func("setup"); Debug_Log_func("setup",1);
} }
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); Debug_Log_func("loop");
ArduinoOTA.handle(); ArduinoOTA.handle();
motor.loopFOC(); motor.loopFOC();
@ -541,7 +535,7 @@ void loop() {
{ {
target_velocity = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity()); target_velocity = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity());
if (abs(target_velocity) > 140) if (abs(target_velocity) > 140)
//target_velocity = _sign(target_velocity) * 140; target_velocity = _sign(target_velocity) * 140;
motor.controller = MotionControlType::velocity; motor.controller = MotionControlType::velocity;
motor.move(target_velocity); motor.move(target_velocity);
@ -608,8 +602,6 @@ 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 ");
@ -634,8 +626,10 @@ void loop() {
FastLED.show(); FastLED.show();
} }
Debug_Log_func("loop"); TenthSecondsSinceStartTask();
} }
/* mpu6050加速度转换为角度 /* mpu6050加速度转换为角度
acc2rotation(ax, ay) acc2rotation(ax, ay)
acc2rotation(az, ay) */ acc2rotation(az, ay) */
@ -926,16 +920,17 @@ String ProcessUpdate() //页面更新
ReturnString += v_p_2; ReturnString += v_p_2;
ReturnString += ","; ReturnString += ",";
if (debug_log_control) { if (debug_log_control) {
for (int i = 0; i < debug_times; i++) { Debug_Log_func("debug print begin",1);
if (String(Debug_Log[i]) != "") { int i = 0;
//Serial.println(Debug_Log[i]); while(strlen(Debug_Log[debug_times-1]) != 0){
ReturnString += Debug_Log[i]; ReturnString += Debug_Log[i];
memset( Debug_Log[debug_times], 0, sizeof(Debug_Log[debug_times]) ); memset( Debug_Log[i], 0, strlen(Debug_Log[i]) );
} i++;
if (i == debug_times - 1)
debug_times = 0;
} }
debug_times = 0;
Debug_Log_func("debug print end",1);
} }
//Serial.println(ReturnString); //Serial.println(ReturnString);
return ReturnString; return ReturnString;
} }
@ -1004,10 +999,12 @@ void PocessControl(int DeviceType, int DeviceIndex, int Operation, float Operati
else if (Operation % SysIndex == 1) else if (Operation % SysIndex == 1)
log_control = 1; log_control = 1;
} else if (DeviceIndex == 6) { //DEBUG输出控制 } else if (DeviceIndex == 6) { //DEBUG输出控制
if (Operation % SysIndex == 0) if (Operation % SysIndex == 0){
debug_log_control = 0; debug_log_control = 0;
else if (Operation % SysIndex == 1) }else if (Operation % SysIndex == 1){
Debug_Log_func("DEBUG OUT", 1);
debug_log_control = 1; debug_log_control = 1;
}
} }
} }