deepseek/wrobot
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修复lqr_u计算bug

Browse Source
This commit is contained in:
myperworld committed 2025-10-07 23:02:52 +08:00
1 parent 2affa22351
commit c059348614
1 file changed
+23 -33
Download Patch File Download Diff File Expand all files Collapse all files
v1.1/Shibo-main-TOMMY/src/main.cpp
+23 -33
View File
@@ -433,7 +433,7 @@ void setup()
motor2.init(); motor2.init();
motor2.initFOC(); motor2.initFOC();
resetToDefault(); // 重置到默认参数状态 // resetToDefault(); // 重置到默认参数状态
// FastLED初始化 // FastLED初始化
FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS); FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
@@ -448,7 +448,7 @@ void loop()
{ {
// 处理LED闪烁 // 处理LED闪烁
handleLEDBlink(); // handleLEDBlink();
// 检测小车状态变化 坐 -> 站 // 检测小车状态变化 坐 -> 站
if (robot_enabled && !last_robot_enabled) { if (robot_enabled && !last_robot_enabled) {
@@ -483,28 +483,16 @@ void loop()
} }
// 大仰角到底，轮子就不要转了 // 大仰角到底，轮子就不要转了
if (abs(LQR_angle) > 70.0f) { // if (abs(LQR_angle) > 70.0f) {
motor1.target = 0; // motor1.target = 0;
motor2.target = 0; // motor2.target = 0;
} // }
// 达到平衡角度后，重置重置计数器 // 达到平衡角度后，重置重置计数器
if( abs(LQR_angle) < 25.0f){ if( abs(LQR_angle) < 25.0f){
resetToDefaultCounter = 0; resetToDefaultCounter = 0;
} }
// target目标电压 0~8V, 0为不转, 8为最大转速
// Serial.print("LQR_angle:"); // 屁股向后倒 -33~-34度,舵机2047的时候-42度，头向前到底 75度，完全垂直88度
// Serial.print(LQR_angle);
// Serial.print(",loopCounter:");
// Serial.print(loopCounter);
// Serial.print(",resetToDefaultCounter:");
// Serial.print(resetToDefaultCounter);
// Serial.print(",motor1.target:");
// Serial.print(motor1.target);
// Serial.print(",motor2.target:");
// Serial.print(motor2.target);
// 向前跳跃 // 向前跳跃
if(forward_jump_flag){ if(forward_jump_flag){
if(forward_jump_flag == 1){ if(forward_jump_flag == 1){
@@ -636,8 +624,6 @@ void lqr_balance_loop()
unsigned long current_time = millis(); unsigned long current_time = millis();
if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) { if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
robot_speed_diff = LQR_speed - last_lqr_speed; robot_speed_diff = LQR_speed - last_lqr_speed;
// Serial.print("LQR_angle:");
// Serial.print(LQR_angle); ; // 就等于LQR_speed，离地速度绝对值大于15左右，在地面只有5
// Serial.print("，robot_speed_last:"); // Serial.print("，robot_speed_last:");
// Serial.print(last_lqr_speed); // Serial.print(last_lqr_speed);
// Serial.print("，robot_speed_diff:"); // Serial.print("，robot_speed_diff:");
@@ -664,6 +650,7 @@ void lqr_balance_loop()
// Serial.print(robot_speed_diff); // Serial.print(robot_speed_diff);
// Serial.print(",jump_flag:"); // Serial.print(",jump_flag:");
// Serial.println(jump_flag); // 120左右 // Serial.println(jump_flag); // 120左右
if( jump_flag ){ // 落地点作为新的位移零点 if( jump_flag ){ // 落地点作为新的位移零点
distance_zeropoint = LQR_distance; distance_zeropoint = LQR_distance;
pid_lqr_u.error_prev = 0; pid_lqr_u.error_prev = 0;
@@ -677,21 +664,23 @@ void lqr_balance_loop()
// 判断摇杆是否没有前后左右运动指令 // 判断摇杆是否没有前后左右运动指令
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
{ {
wrobot_move_stop_flag = 1; // wrobot_move_stop_flag = 1;
distance_zeropoint = LQR_distance; // 位移零点重置
pid_lqr_u.error_prev = 0; // 输出积分清零 测试
} }
// 没有摇杆指令，小车微微移动，需要重置位移零点，让小车自己回来 // 没有摇杆指令，小车微微移动，需要重置位移零点，让小车自己回来
if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) // if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
{ // {
distance_zeropoint = LQR_distance; // 位移零点重置 // distance_zeropoint = LQR_distance; // 位移零点重置
// pid_lqr_u.error_prev = 0; // 输出积分清零 如果有这个会前后漂移 // pid_lqr_u.error_prev = 0; // 输出积分清零 测试
wrobot_move_stop_flag = 0; // wrobot_move_stop_flag = 0;
} // }
// 实时重置位移零点和积分情形 // 实时重置位移零点和积分情形
// if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15)
// || jump_flag > 0 || wrobot.joyy != 0 || sitting_down ) // || jump_flag > 0 || wrobot.joyy != 0 || sitting_down )
if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15)
|| wrobot.joyy != 0 || sitting_down ) || wrobot.joyy != 0 || sitting_down )
{ {
distance_zeropoint = LQR_distance; distance_zeropoint = LQR_distance;
pid_lqr_u.error_prev = 0; // 输出积分清零 pid_lqr_u.error_prev = 0; // 输出积分清零
@@ -700,11 +689,10 @@ void lqr_balance_loop()
distance_control = pid_distance(LQR_distance - distance_zeropoint); distance_control = pid_distance(LQR_distance - distance_zeropoint);
// 计算 LQR_u // 计算 LQR_u
// 轮部离地情况下 LQR_u=角度控制量+角速度控制量 // 必须是在跳跃时，LQR_u= 角度控制量 + 角速度控制量
// if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 100)) if ( (jump_flag > 0 && abs(robot_speed_diff) > 10) || (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
{ {
LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略 LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略
} }
else // 当轮部未离地时，LQR_u：4个参数 else // 当轮部未离地时，LQR_u：4个参数
{ {
@@ -966,6 +954,7 @@ void resetToDefault() {
angle_zeropoint = original_angle_zeropoint; angle_zeropoint = original_angle_zeropoint;
LQR_speed = 0; LQR_speed = 0;
LQR_distance = 0; LQR_distance = 0;
last_lqr_speed = 0;
speed_control = 0; speed_control = 0;
distance_control = 0; distance_control = 0;
// 需要将位移零点设置为当前位置 // 需要将位移零点设置为当前位置
@@ -979,10 +968,11 @@ void resetToDefault() {
YAW_angle = 0; YAW_angle = 0;
YAW_output = 0; YAW_output = 0;
last_lqr_speed = 0;
jump_flag = 0; jump_flag = 0;
leg_position_add = 0; leg_position_add = 0;
wrobot_move_stop_flag = 0;
} }
// 添加LED闪烁处理函数 // 添加LED闪烁处理函数