From c9bba0bea208c23a97a647eda76e726396ef30a0 Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Sat, 27 Jan 2024 19:38:00 +0800
Subject: [PATCH] =?UTF-8?q?LQR=E8=AE=A1=E7=AE=97=E8=A7=A3=E8=80=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c  | 163 +++++++++++++++++++++++++++++++++
 application/chassis/linkNleg.h |   2 +-
 application/chassis/lqr_calc.h |  32 +++++++
 modules/motor/LKmotor/LK9025.h |   2 +-
 4 files changed, 197 insertions(+), 2 deletions(-)
 create mode 100644 application/chassis/lqr_calc.h

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 8e3ffb4..dadc42a 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -17,6 +17,7 @@
 #include "bsp_log.h"
 #include "linkNleg.h"
 #include "speed_estimation.h"
+#include "lqr_calc.h"
 
 
 static uint32_t balance_dwt_cnt;
@@ -231,6 +232,168 @@ static void ControlSwitch()
 }
 
 
+/* 腿缩回复位,只允许驱动轮电机移动 */
+static void ResetChassis()
+{
+    EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
+
+    // 复位时清空距离和腿长积累量,保证顺利站起
+    chassis.dist = chassis.target_dist = 0;
+    l_side.target_len = r_side.target_len = 0.24;
+    // 撞墙时前后移动保证能重新站立,执行速度输入
+    LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
+    LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
+
+    // 若关节完成复位,进入ready态
+    if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.02 &&
+        abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.02 &&
+        abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.02 &&
+        abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.02)
+    {
+        chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
+    }
+    else if (abs(lf->measure.total_angle) <= 0.02 &&
+             abs(lb->measure.total_angle) <= 0.02 &&
+             abs(rf->measure.total_angle) <= 0.02 &&
+             abs(rb->measure.total_angle) <= 0.02)
+    {                                 // 双阈值保证关节能够复位而不会进入死区
+        chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
+        for (uint8_t i = 0; i < JOINT_CNT; i++)
+            HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
+        return;                                    // 退出函数不再执行关节指令
+    }
+    else
+        chassis_status = ROBOT_STOP;
+
+    // 还在复位中,关节改为位置环,执行复位
+    for (uint8_t i = 0; i < JOINT_CNT; i++)
+    {
+        HTMotorOuterLoop(joint[i], ANGLE_LOOP);
+        HTMotorSetRef(joint[i], 0);
+    }
+}
+
+
+/* 工作状态设定 */
+static void WokingStateSet()
+{
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式
+    {
+        ResetChassis();
+        return;
+    }
+    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
+    {
+        for (uint8_t i = 0; i < JOINT_CNT; i++)
+            HTMotorStop(joint[i]);
+        for (uint8_t i = 0; i < DRIVEN_CNT; i++)
+            LKMotorStop(driven[i]);
+        return; // 关闭所有电机,发送的指令为零
+    }
+
+    // 运动模式
+    EnableAllMotor();
+    // 设置目标速度/腿长/距离
+    l_side.target_len += chassis_cmd_recv.delta_leglen;
+    r_side.target_len += chassis_cmd_recv.delta_leglen;
+    VAL_LIMIT(l_side.target_len, 0.13, 0.3); // 腿长限幅
+    VAL_LIMIT(r_side.target_len, 0.13, 0.3);
+    // 加速度限幅,防止键盘控制摔倒
+    if (abs(chassis_cmd_recv.vx - chassis.target_v) / del_t < MAX_ACC_REF)
+        chassis.target_v = chassis_cmd_recv.vx;
+    else
+        chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
+    // 模型距离参考输入
+    chassis.target_dist += chassis.target_v * del_t;
+    chassis.target_yaw = chassis_cmd_recv.offset_angle; // 云台和底盘对齐时电机编码器的单圈反馈角度
+}
+
+
+/**
+ * @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
+ *
+ * @note HT04电机上电的编码器位置为零(校准过),请看Link2Pod()的note,以及HT04.c中的电机解码部分
+ * @note 海泰04电机顺时针旋转为正; LK9025电机逆时针旋转为正,此处皆需要转换为模型中给定的正方向
+ *
+ */
+static void ParamAssemble()
+{   
+    // 机体参数,视为平面刚体
+    chassis.pitch = (-imu_data->Pitch + BALANCE_GRAVITY_BIAS) * DEGREE_2_RAD;
+    chassis.pitch_w = -imu_data->Gyro[0];
+    chassis.yaw = imu_data->YawTotalAngle * DEGREE_2_RAD;
+    chassis.wz = imu_data->Gyro[2];
+    chassis.roll = imu_data->Roll * DEGREE_2_RAD + ROLL_GRAVITY_BIAS;
+    chassis.roll_w = imu_data->Gyro[1];
+
+    // HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正
+    l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_angle;
+    l_side.phi4 = -lf->measure.total_angle - LIMIT_LINK_RAD;
+    l_side.phi1_w = -lb->measure.speed_rads;
+    l_side.phi4_w = -lf->measure.speed_rads;
+    l_side.w_ecd = l_driven->measure.speed_rads;
+    r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_angle;
+    r_side.phi4 = rf->measure.total_angle - LIMIT_LINK_RAD;
+    r_side.phi1_w = rb->measure.speed_rads;
+    r_side.phi4_w = rf->measure.speed_rads;
+    r_side.w_ecd = -r_driven->measure.speed_rads;
+}
+
+
+/* 腿部控制:抗劈叉; 轮子控制:转向 */
+static void SynthesizeMotion() 
+{
+    // 跟随云台yaw
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW ||
+        chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG) // 角度环
+    {
+        float p_ref = PIDCalculate(&steer_p_pid, chassis_cmd_recv.offset_angle, 0);
+        PIDCalculate(&steer_v_pid, chassis.wz, p_ref); // 双环
+    }
+    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 速度环
+        PIDCalculate(&steer_v_pid, chassis.wz, 4);
+
+    l_side.T_wheel -= steer_v_pid.Output;
+    r_side.T_wheel += steer_v_pid.Output;
+
+    // 抗劈叉
+    volatile static float swerving_speed_ff, ff_coef = 3;
+    swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
+    PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
+    l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
+    r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
+}
+
+
+/* 腿长控制和Roll补偿 */
+static void LegControl() 
+{
+    PIDCalculate(&roll_compensate_pid, chassis.roll, 0);
+    l_side.target_len -= roll_compensate_pid.Output;
+    r_side.target_len += roll_compensate_pid.Output;
+
+    static float gravity_comp = 0;
+    static float roll_extra_comp_p = 0;
+    float roll_comp = roll_extra_comp_p * chassis.roll;
+    l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp + roll_comp;
+    r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp - roll_comp;
+
+    // @todo: 还需要加和roll的纯Kp项
+}
+
+
+/* 设定运动模态的输出 */
+static void WattLimitSet() 
+{
+    HTMotorSetRef(lf, 0.285f * -l_side.T_front); // 根据扭矩常数计算得到的系数
+    HTMotorSetRef(lb, 0.285f * -l_side.T_back);
+    HTMotorSetRef(rf, 0.285f * r_side.T_front);
+    HTMotorSetRef(rb, 0.285f * r_side.T_back);
+    LKMotorSetRef(l_driven, 274.348 * l_side.T_wheel);
+    LKMotorSetRef(r_driven, 274.348 * -r_side.T_wheel);
+}
+
+
 void BalanceTask()
 {
     del_t = DWT_GetDeltaT(&balance_dwt_cnt);
diff --git a/application/chassis/linkNleg.h b/application/chassis/linkNleg.h
index f1ac1de..409cbbc 100644
--- a/application/chassis/linkNleg.h
+++ b/application/chassis/linkNleg.h
@@ -13,7 +13,7 @@ void VMCProject(LinkNPodParam *p)
     float phi52 = p->phi5 - p->phi2;
     float F_m_L = p->F_leg * p->leg_len;
     p->T_back = (THIGH_LEN * msin(phi12) * (F_m_L * msin(phi53) + p->T_hip * mcos(phi53))) / (p->leg_len * msin(phi32));
-    p->T_front = (CALF_LEN * msin(phi34) * (F_m_L * msin(phi52) + p->T_hip * mcos(phi52))) / (p->leg_len * msin(phi32));
+    p->T_front = (THIGH_LEN * msin(phi34) * (F_m_L * msin(phi52) + p->T_hip * mcos(phi52))) / (p->leg_len * msin(phi32));
 }
 
 /**
diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h
new file mode 100644
index 0000000..b2ca080
--- /dev/null
+++ b/application/chassis/lqr_calc.h
@@ -0,0 +1,32 @@
+#include "balance.h"
+#include "stdint.h"
+#include "arm_math.h"
+
+/**
+ * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
+ * @note 得到的腿部力矩输出还要经过综合运动控制系统补偿后映射为两个关节电机输出
+ *
+ */
+static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
+{
+    static float k[12][3] = {};
+    float T[2] = {0}; // 0 T_wheel  1 T_hip
+    float l = p->leg_len;
+    float lsqr = l * l;
+
+    // float dist_limit = abs(chassis->target_dist - chassis->dist) > MAX_DIST_TRACK ? sign(chassis->target_dist - chassis->dist) * MAX_DIST_TRACK : (chassis->target_dist - chassis->dist); // todo设置值
+    // float vel_limit = abs(chassis->target_v - chassis->vel) > MAX_VEL_TRACK ? sign(chassis->target_v - chassis->vel) * MAX_VEL_TRACK : (chassis->target_v - chassis->vel);
+
+    for (uint8_t i = 0; i < 2; ++i)
+    {
+        uint8_t j = i * 6;
+        T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
+               (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w +
+               (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
+               (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
+               (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
+               (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w;
+    }
+    p->T_wheel = T[0];
+    p->T_hip = T[1];
+}
\ No newline at end of file
diff --git a/modules/motor/LKmotor/LK9025.h b/modules/motor/LKmotor/LK9025.h
index 0bf077c..a880dd6 100644
--- a/modules/motor/LKmotor/LK9025.h
+++ b/modules/motor/LKmotor/LK9025.h
@@ -15,7 +15,7 @@
 #define SPEED_SMOOTH_COEF 0.85f
 #define REDUCTION_RATIO_DRIVEN 1
 #define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
-#define CURRENT_TORQUE_COEF_LK 0.00512f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
+#define CURRENT_TORQUE_COEF_LK 0.003645f // // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
 
 typedef struct // 9025
 {