修复lqr_u计算bug
This commit is contained in:
myperworld committed
1 parent
2affa22351
commit
c059348614
1 file changed
+24
-34
@@ -433,7 +433,7 @@ void setup()
|
||||
motor2.init();
|
||||
motor2.initFOC();
|
||||
|
||||
resetToDefault(); // 重置到默认参数状态
|
||||
// resetToDefault(); // 重置到默认参数状态
|
||||
|
||||
// FastLED初始化
|
||||
FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
|
||||
@@ -448,7 +448,7 @@ void loop()
|
||||
{
|
||||
|
||||
// 处理LED闪烁
|
||||
handleLEDBlink();
|
||||
// handleLEDBlink();
|
||||
|
||||
// 检测小车状态变化 坐 -> 站
|
||||
if (robot_enabled && !last_robot_enabled) {
|
||||
@@ -483,28 +483,16 @@ void loop()
|
||||
}
|
||||
|
||||
// 大仰角到底，轮子就不要转了
|
||||
if (abs(LQR_angle) > 70.0f) {
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
// if (abs(LQR_angle) > 70.0f) {
|
||||
// motor1.target = 0;
|
||||
// motor2.target = 0;
|
||||
// }
|
||||
|
||||
// 达到平衡角度后，重置重置计数器
|
||||
if( abs(LQR_angle) < 25.0f){
|
||||
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 == 1){
|
||||
@@ -629,15 +617,13 @@ void lqr_balance_loop()
|
||||
}else{
|
||||
speed_target_coeff = 0.1;
|
||||
}
|
||||
|
||||
|
||||
speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v
|
||||
|
||||
// 检测轮子差速，判断轮子是否离地
|
||||
unsigned long current_time = millis();
|
||||
if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
|
||||
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(last_lqr_speed);
|
||||
// Serial.print("，robot_speed_diff:");
|
||||
@@ -664,6 +650,7 @@ void lqr_balance_loop()
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 120左右
|
||||
|
||||
if( jump_flag ){ // 落地点作为新的位移零点
|
||||
distance_zeropoint = LQR_distance;
|
||||
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))
|
||||
{
|
||||
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))
|
||||
{
|
||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||
// pid_lqr_u.error_prev = 0; // 输出积分清零 如果有这个会前后漂移
|
||||
wrobot_move_stop_flag = 0;
|
||||
}
|
||||
// if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
|
||||
// {
|
||||
// distance_zeropoint = LQR_distance; // 位移零点重置
|
||||
// pid_lqr_u.error_prev = 0; // 输出积分清零 测试
|
||||
// wrobot_move_stop_flag = 0;
|
||||
// }
|
||||
|
||||
// 实时重置位移零点和积分情形
|
||||
// if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15)
|
||||
// || jump_flag > 0 || wrobot.joyy != 0 || sitting_down )
|
||||
if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15)
|
||||
|| wrobot.joyy != 0 || sitting_down )
|
||||
|| wrobot.joyy != 0 || sitting_down )
|
||||
{
|
||||
distance_zeropoint = LQR_distance;
|
||||
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||
@@ -700,11 +689,10 @@ void lqr_balance_loop()
|
||||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||
|
||||
// 计算 LQR_u
|
||||
// 轮部离地情况下 LQR_u=角度控制量+角速度控制量
|
||||
// if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 100))
|
||||
if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
|
||||
// 必须是在跳跃时，LQR_u= 角度控制量 + 角速度控制量
|
||||
if ( (jump_flag > 0 && 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个参数
|
||||
{
|
||||
@@ -966,6 +954,7 @@ void resetToDefault() {
|
||||
angle_zeropoint = original_angle_zeropoint;
|
||||
LQR_speed = 0;
|
||||
LQR_distance = 0;
|
||||
last_lqr_speed = 0;
|
||||
speed_control = 0;
|
||||
distance_control = 0;
|
||||
// 需要将位移零点设置为当前位置
|
||||
@@ -979,10 +968,11 @@ void resetToDefault() {
|
||||
YAW_angle = 0;
|
||||
YAW_output = 0;
|
||||
|
||||
last_lqr_speed = 0;
|
||||
jump_flag = 0;
|
||||
leg_position_add = 0;
|
||||
|
||||
wrobot_move_stop_flag = 0;
|
||||
|
||||
}
|
||||
|
||||
// 添加LED闪烁处理函数
|
||||
|
||||
Reference in new issue
Block a user