增加了文档,修改通信协议,更新了平衡底盘的VMC映射和五连杆解算,修复了双板通信数据结构步对齐的错误

application/balance_chassis/balance.c

@@ -1,6 +1,5 @@
 // app
 #include "balance.h"
-#include "vmc_project.h"
 #include "gain_table.h"
 #include "robot_def.h"
 #include "general_def.h"
@@ -12,77 +11,156 @@
 #include "super_cap.h"
 #include "controller.h"
 #include "can_comm.h"
+#include "user_lib.h"
 // standard
 #include "stdint.h"
 #include "arm_math.h" // 需要用到较多三角函数
 
+#include "bsp_dwt.h"
+static uint32_t balance_dwt_cnt;
+static float balance_dt;
+
 /* 底盘拥有的模块实例 */
-static BMI088Instance *imu;
+static attitude_t *imu_data;
+// static BMI088Instance *imu;
 static SuperCapInstance *super_cap;
 static referee_info_t *referee_data; // 裁判系统的数据会被不断更新
-
+// 电机
 static HTMotorInstance *lf;
 static HTMotorInstance *rf;
 static HTMotorInstance *lb;
 static HTMotorInstance *rb;
 static LKMotorInstance *l_driven;
 static LKMotorInstance *r_driven;
-
-static CANCommInstance *chassis_comm; // 底盘板和云台板通信
+// 底盘板和云台板通信
+static CANCommInstance *chassis_comm; // 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率并降低阻塞
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
 static Chassis_Upload_Data_s chassis_feed_send;
-// 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率
-
-/* 方便函数间无开销传递参数的中间变量 */
-// 若将下面的封装函数取缔,则可以将这些变量放入BalanceTask函数体内.
-// static ...
-static float leg_len_l, leg_len_r;     // 左右腿长(虚拟)
-static float leg_angle_l, leg_angle_r; // 左右腿角度(虚拟)
-// 倒立摆的虚拟力和虚拟力矩
-static float F_virtual_left, T_virtual_left, F_virtual_right, T_virtual_right;
-// 左前,左后,右前,右后关节力矩
-static float T_joint_lf, T_joint_lr, T_joint_rf, T_joint_rb;
-static float T_leg_left, T_leg_right; // 左右驱动电机力矩
+// 两个腿的参数,0为左腿,1为右腿
+static LinkNPodParam left_side, right_side;
+static PIDInstance swerving_pid;        // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
+static PIDInstance anti_crash_pid;      // 抗劈叉,将输出以相反的方向叠加到左右腿的上
+static PIDInstance leg_length_pid;      // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
+static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
 
 /* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/
 
+/**
+ * @brief 将电机和imu的数据组装为LinkNPodParam结构体
+ *
+ */
+static void ParamAssemble()
+{
+}
+
+/**
+ * @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
+ *
+ * @param p 5连杆和腿的参数
+ */
+static void Link2Pod(LinkNPodParam *p)
+{ // 拟将功能封装到vmc_project.h中
+    float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2;
+    xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(p->phi4);
+    yD = THIGH_LEN * arm_sin_f32(p->phi4);
+    xB = 0 + THIGH_LEN * arm_cos_f32(p->phi1);
+    yB = THIGH_LEN * arm_sin_f32(p->phi1);
+    BD = powf(xD - xB, 2) + powf(yD - yB, 2);
+    A0 = 2 * CALF_LEN * (xD - xB);
+    B0 = 2 * CALF_LEN * (yD - yB);
+    p->phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
+    xC = xB + CALF_LEN * arm_cos_f32(p->phi2);
+    yC = yB + CALF_LEN * arm_sin_f32(p->phi2);
+#ifdef ANGLE_DIFF_VMC
+    float p5t = atan2f(yC, xC - JOINT_DISTANCE / 2); // 避免重复计算
+    float plt = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
+    p->phi5_w = (p5t - p->phi5) / balance_dt;
+    p->pod_len_w = (plt - p->pod_len) / balance_dt;
+    p->phi5 = p5t;
+    p->pod_len = plt;
+#endif
+    p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2);
+    p->pod_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
+    p->phi3 = atan2(yC - yD, xC - xD); // 稍后用于计算VMC
+
+#ifdef VELOCITY_DIFF_VMC
+    float phi1_pred = p->phi1 + p->phi1_w * balance_dt; // 预测下一时刻的关节角度(利用关节角速度)
+    float phi4_pred = p->phi4 + p->phi4_w * balance_dt;
+    // 重新计算腿长和腿角度
+    xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(phi4_pred);
+    yD = THIGH_LEN * arm_sin_f32(phi4_pred);
+    xB = 0 + THIGH_LEN * arm_cos_f32(phi1_pred);
+    yB = THIGH_LEN * arm_sin_f32(phi1_pred);
+    BD = powf(xD - xB, 2) + powf(yD - yB, 2);
+    A0 = 2 * CALF_LEN * (xD - xB);
+    B0 = 2 * CALF_LEN * (yD - yB);
+    phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
+    xC = xB + CALF_LEN * arm_cos_f32(phi2);
+    yC = yB + CALF_LEN * arm_sin_f32(phi2);
+    // 差分计算腿长变化率和腿角速度
+    p->pod_w = (atan2f(yC, xC - JOINT_DISTANCE / 2) - p->phi5) / balance_dt;
+    p->pod_v = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->pod_len) / balance_dt;
+#endif // VELOCITY_DIFF_VMC
+}
+
 /**
  * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
  *        由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算
+ * @note 计算得到的腿部力矩输出还要再综合运动控制系统补偿后映射为两个关节电机
+ *       而轮子的输出则只经过转向PID的反馈增益计算
  *
  */
-static void CalcLQR()
-{ 
+static void CalcLQR(LinkNPodParam *p)
+{
 }
 
 /**
- * @brief 将LQR的输出映射到关节和驱动电机的输出
- *
+ * @brief 腿部控制:抗劈叉; 轮子控制:转向
+ * @todo 确定方向
  */
-static void VMCProject()
-{ // 拟将功能封装到vmc_project.h中
-}
-
-/**
- * @brief 腿部角度控制:转向和抗劈叉
- *
- */
-static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
-static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
-
 static void SynthesizeMotion()
 {
+    PIDCalculate(&anti_crash_pid, left_side.phi5 - right_side.phi5, 0);
+    left_side.T_pod += anti_crash_pid.Output;
+    right_side.T_pod -= anti_crash_pid.Output;
+
+    // PIDCalculate(&swerving_pid, imu_data->Yaw, 0); // 对速度闭环还是使用角度增量闭环?
+    left_side.T_wheel -= swerving_pid.Output;
+    right_side.T_wheel += swerving_pid.Output;
 }
 
 /**
- * @brief 腿长控制:长度 + roll轴补偿(保持机体水平),用PD模拟弹簧的传递函数
+ * @brief 腿部控制:长度.用PD模拟弹簧的传递函数
  *
  */
-static PIDInstance leg_length_pid; // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
-static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
-
-static void LegControl()
+static void LegControl(LinkNPodParam *p, float target_length)
 {
+    p->F_pod += PIDCalculate(&leg_length_pid, p->pod_len, target_length);
+}
+
+/**
+ * @brief roll轴补偿(保持机体水平)
+ *
+ */
+static void RollCompensate()
+{
+    PIDCalculate(&roll_compensate_pid, imu_data->Roll, 0);
+    left_side.F_pod += roll_compensate_pid.Output;
+    right_side.F_pod -= roll_compensate_pid.Output;
+}
+
+/**
+ * @brief 将前面计算的T和F映射为关节电机输出
+ *
+ */
+static void VMCProject(LinkNPodParam *p)
+{
+    float s23 = arm_sin_f32(p->phi2 - p->phi3);
+    float phi25 = p->phi2 - p->phi5;
+    float phi35 = p->phi3 - p->phi5;
+    float F_m_L = p->F_pod * p->pod_len;
+    p->T_back = -(THIGH_LEN * arm_sin_f32(p->phi1 - p->phi2) * (p->T_pod * arm_cos_f32(phi35) - F_m_L * arm_sin_f32(phi35))) / (p->pod_len * s23);
+    p->T_front = -(THIGH_LEN * arm_sin_f32(p->phi3 - p->phi4) * (p->T_pod * arm_cos_f32(phi25) - F_m_L * arm_sin_f32(phi25))) / (p->pod_len * s23);
 }
 
 /**
@@ -90,6 +168,7 @@ static void LegControl()
  *        通过模型解算地面的支持力完成离地检测
  *
  */
+static uint8_t air_flag;
 static void FlyDetect()
 {
 }
@@ -98,14 +177,13 @@ static void FlyDetect()
  * @brief 功率限制,一般不需要
  *
  */
-static void WattLimit()
+static void WattLimit(LinkNPodParam *p)
 {
 }
 
 void BalanceInit()
-{
+{ // IMU初始化
     BMI088_Init_Config_s imu_config = {
-        // IMU初始化
         .spi_acc_config = {
             .GPIOx = GPIOC,
             .cs_pin = GPIO_PIN_4,
@@ -116,16 +194,6 @@ void BalanceInit()
             .cs_pin = GPIO_PIN_4,
             .spi_handle = &hspi1,
         },
-        .acc_int_config = {
-            .exti_mode = EXTI_TRIGGER_FALLING,
-            .GPIO_Pin = GPIO_PIN_10,
-            .GPIOx = GPIOA,
-        },
-        .gyro_int_config = {
-            .exti_mode = EXTI_TRIGGER_FALLING,
-            .GPIO_Pin = GPIO_PIN_11,
-            .GPIOx = GPIOA,
-        },
         .heat_pid_config = {
             .Kp = 0.0f,
             .Kd = 0.0f,
@@ -141,6 +209,7 @@ void BalanceInit()
         .work_mode = BMI088_BLOCK_PERIODIC_MODE,
     };
     // imu = BMI088Register(&imu_config);
+    imu_data = INS_Init();
 
     SuperCap_Init_Config_s cap_conf = {
         // 超级电容初始化
@@ -165,7 +234,6 @@ void BalanceInit()
     rb = HTMotorInit(&joint_conf);
 
     // ↓↓↓---------------驱动电机初始化----------------↓↓↓
-
     Motor_Init_Config_s driven_conf = {
         // 写一个,剩下的修改方向和id即可
         .can_init_config.can_handle = &hcan1,
@@ -184,12 +252,11 @@ void BalanceInit()
             .close_loop_type = CURRENT_LOOP,
         },
         .motor_type = LK9025,
-
     };
-    driven_conf.can_init_config.tx_id=1;
+    driven_conf.can_init_config.tx_id = 1;
     l_driven = LKMotorInit(&driven_conf);
 
-    driven_conf.can_init_config.tx_id=2;
+    driven_conf.can_init_config.tx_id = 2;
     r_driven = LKMotorInit(&driven_conf);
 
     CANComm_Init_Config_s chassis_comm_conf = {
@@ -204,7 +271,7 @@ void BalanceInit()
     };
     chassis_comm = CANCommInit(&chassis_comm_conf);
 
-    referee_data = RefereeInit(&huart6); // 裁判系统串口
+    // referee_data = RefereeInit(&huart6); // 裁判系统串口
 
     // ↓↓↓---------------综合运动控制----------------↓↓↓
     PID_Init_Config_s swerving_pid_conf = {
@@ -251,4 +318,32 @@ void BalanceInit()
 /* balanceTask可能需要以更高频率运行,以提高线性化的精确程度 */
 void BalanceTask()
 {
+    chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chassis_comm);
+
+    ParamAssemble(); // 参数组装,将电机和IMU的参数组装到一起
+    // 将五连杆映射成单杆
+    Link2Pod(&left_side);
+    Link2Pod(&right_side);
+    // 根据单杆计算处的角度和杆长,计算反馈增益
+    CalcLQR(&left_side);
+    CalcLQR(&right_side);
+    // 腿长控制
+    LegControl(&left_side, 0);
+    LegControl(&right_side, 0);
+
+    SynthesizeMotion(); // 综合运动控制,转向+抗劈叉
+
+    RollCompensate(); // 俯仰角补偿,保持机体水平
+    // VMC映射成关节输出
+    VMCProject(&left_side);
+    VMCProject(&right_side);
+
+    FlyDetect(); // 滞空检测
+    // 电机输出限幅
+    WattLimit(&left_side);
+    WattLimit(&right_side);
+
+    // code to go here... 裁判系统,UI,多机通信
+    
+    CANCommSend(chassis_comm, (uint8_t*)&chassis_feed_send);
 }

application/balance_chassis/balance.h

@@ -1,17 +1,44 @@
 #pragma once
 
+// 底盘参数
+#define CALF_LEN 0.25f        // 小腿
+#define THIGH_LEN 0.15f       // 大腿
+#define JOINT_DISTANCE 0.108f // 关节间距
+
+#define VELOCITY_DIFF_VMC //通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
+// #define ANGLE_DIFF_VMC //直接保存上一次的值,通过差分计算腿长变化率和腿角速度
+
+
+typedef struct
+{
+    // joint
+    float phi1_w, phi4_w;
+    float T_back, T_front;
+
+    // link angle,phi1-ph5, phi5 is pod angle
+    float phi1, phi2, phi3, phi4, phi5;
+
+    // wheel
+    float wheel_angle;
+    float wheel_w;
+    float T_wheel;
+
+    // pod
+    float pod_len;
+    float pod_w;
+    float pod_v;
+    float F_pod;
+    float T_pod;
+} LinkNPodParam;
+
 /**
  * @brief 平衡底盘初始化
- * 
+ *
  */
 void BalanceInit();
 
 /**
  * @brief 平衡底盘任务
- * 
+ *
  */
 void BalanceTask();
-
-
-
-

application/balance_chassis/vmc_project.h

@@ -1,14 +0,0 @@
-#ifndef VMC_PROJECT_H
-#define VMC_PROJECT_H 
-
-#include "stm32f407xx.h"
-#include "arm_math.h"
-#include "math.h"
-#include "general_def.h"
-
-// 将五连杆和直杆的vmc映射放在此处,方便修改和调试,balance.c不会太长
-#endif // !VMC_PROJECT_H
-
-
-
-

application/gimbal/gimbal.c

@@ -136,7 +136,7 @@ void GimbalInit()
     gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
     gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
 }
-int aaaaaaa;
+
 /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
 void GimbalTask()
 {
@@ -178,10 +178,7 @@ void GimbalTask()
     default:
         break;
     }
-// if(yaw_motor->motor_measure.total_angle>120)
-// {
-//     aaaaaaa++;
-// }
+
     // 在合适的地方添加pitch重力补偿前馈力矩
     // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
     // ...

application/robot.c

@@ -10,7 +10,6 @@
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
 #include "chassis.h"
-#include "referee.h"
 #endif
 
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
@@ -19,6 +18,11 @@
 #include "robot_cmd.h"
 #endif
 
+#ifdef BALANCE_BAORD
+#include "balance.h"
+#endif // BALANCE_BOARD
+
+
 void RobotInit()
 {  
     // 关闭中断,防止在初始化过程中发生中断
@@ -35,9 +39,13 @@ void RobotInit()
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
-    // Referee_Interactive_init();
     ChassisInit();
 #endif
+
+#ifdef BALANCE_BAORD
+    BalanceInit();
+#endif // BALANCE_BA
+
     // 初始化完成,开启中断
     __enable_irq();
 }
@@ -52,6 +60,9 @@ void RobotTask()
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
     ChassisTask();
-    // Referee_Interactive_task();
 #endif
+
+#ifdef BALANCE_BAORD
+    BalanceTask();
+#endif // BALANCE_BA
 }

application/robot_def.h

@@ -131,11 +131,11 @@ typedef enum
     LOAD_BURSTFIRE, // 连发
 } loader_mode_e;
 
-// 功率限制,从裁判系统获取
+// 功率限制,从裁判系统获取,是否有必要保留?
 typedef struct
 { // 功率控制
     float chassis_power_mx;
-} Chassis_Power_Data_s;
+} Chassis_Power_Data_s; 
 
 /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
 /**
@@ -188,7 +188,9 @@ typedef struct
 /* @todo : 对于平衡底盘,需要新增控制模式和控制数据 */
 typedef struct
 {
-    attitude_t chassis_imu_data;
+#if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要)
+    // attitude_t chassis_imu_data;
+#endif
     // 后续增加底盘的真实速度
     // float real_vx;
     // float real_vy;
@@ -197,9 +199,7 @@ typedef struct
     uint8_t rest_heat;           // 剩余枪口热量
     Bullet_Speed_e bullet_speed; // 弹速限制
     Enemy_Color_e enemy_color;   // 0 for blue, 1 for red
-
-    // 是否需要剩余电量?(电容)
-
+    
 } Chassis_Upload_Data_s;
 
 /* @todo : 对于平衡底盘,需要不同的反馈数据 */