修复LK电机id计算错误，构建平衡底盘框架，增加通用通信模块，增加平衡底盘条件编译兼容，删除lqr

application/balance_chassis/balance.c

@@ -0,0 +1,229 @@
+#include "balance.h"
+#include "HT04.h"
+#include "LK9025.h"
+#include "bmi088.h"
+#include "referee.h"
+#include "super_cap.h"
+#include "controller.h"
+#include "can_comm.h"
+#include "stdint.h"
+#include "robot_def.h"
+#include "general_def.h"
+#include "arm_math.h" // 需要用到较多三角函数
+
+/* 底盘拥有的模块实例 */
+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 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; // 左右驱动电机力矩
+
+/* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/
+
+/**
+ * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
+ *        由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算
+ *
+ */
+static void CalcLQR()
+{
+}
+
+/**
+ * @brief 将LQR的输出映射到关节和驱动电机的输出
+ *
+ */
+static void VMCProject()
+{
+}
+
+/**
+ * @brief 腿部角度控制:转向和抗劈叉
+ *
+ */
+static PIDInstance swerving_pid;
+static PIDInstance anti_crash_pid;
+
+static void SynthesizeMotion()
+{
+}
+
+/**
+ * @brief 腿长控制:长度 + roll轴补偿(保持机体水平),用PD模拟弹簧的传递函数
+ *
+ */
+static PIDInstance leg_length_pid;
+static PIDInstance roll_compensate_pid;
+
+static void LegControl()
+{
+}
+
+/**
+ * @brief 离地监测和?跳跃控制
+ *
+ */
+static void FlyDetect()
+{
+}
+
+/**
+ * @brief 功率限制
+ *
+ */
+static void WattLimit()
+{
+}
+
+void BalanceInit()
+{
+    BMI088_Init_Config_s imu_config = {
+        // IMU初始化
+        .spi_acc_config = {
+            .GPIOx = GPIOC,
+            .cs_pin = GPIO_PIN_4,
+            .spi_handle = &hspi1,
+        },
+        .spi_gyro_config = {
+            .GPIOx = GPIOC,
+            .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,
+            .Ki = 0.0f,
+            .MaxOut = 0.0f,
+            .DeadBand = 0.0f,
+        },
+        .heat_pwm_config = {
+            .channel = TIM_CHANNEL_1,
+            .htim = &htim1,
+        },
+        .cali_mode = BMI088_CALIBRATE_ONLINE_MODE,
+        .work_mode = BMI088_BLOCK_PERIODIC_MODE,
+    };
+    imu = BMI088Register(&imu_config);
+
+    SuperCap_Init_Config_s cap_conf = {
+        // 超级电容初始化
+        .can_config.can_handle = &hcan1,
+        .can_config.rx_id = 0x311,
+        .can_config.tx_id = 0x312,
+    };
+    super_cap = SuperCapInit(&cap_conf);
+
+    // ↓↓↓---------------关节电机初始化----------------↓↓↓
+
+    Motor_Init_Config_s joint_conf = {
+        // 写一个,剩下的修改方向和id即可
+
+    };
+    lf = HTMotorInit(&joint_conf);
+
+    rf = HTMotorInit(&joint_conf);
+
+    lb = HTMotorInit(&joint_conf);
+
+    rb = HTMotorInit(&joint_conf);
+
+    // ↓↓↓---------------驱动电机初始化----------------↓↓↓
+
+    Motor_Init_Config_s driven_conf = {
+        // 写一个,剩下的修改方向和id即可
+
+    };
+    l_driven = LKMotorInit(&driven_conf);
+
+    r_driven = LKMotorInit(&driven_conf);
+
+    CANComm_Init_Config_s chassis_comm_conf = {
+        // 底盘板和云台板通信
+        .can_config = {
+            .can_handle = &hcan1,
+            .rx_id = 0x201,
+            .tx_id = 0x200,
+        },
+        .send_data_len = sizeof(Chassis_Upload_Data_s),
+        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
+    };
+    chassis_comm = CANCommInit(&chassis_comm_conf);
+
+    referee_data = RefereeInit(&huart6); // 裁判系统串口
+
+    // ↓↓↓---------------综合运动控制----------------↓↓↓
+    PID_Init_Config_s swerving_pid_conf = {
+        .Kp = 0.0f,
+        .Kd = 0.0f,
+        .Ki = 0.0f,
+        .MaxOut = 0.0f,
+        .DeadBand = 0.0f,
+        .Improve = PID_IMPROVE_NONE,
+    };
+    PIDInit(&swerving_pid, &swerving_pid_conf);
+
+    PID_Init_Config_s anti_crash_pid_conf = {
+        .Kp = 0.0f,
+        .Kd = 0.0f,
+        .Ki = 0.0f,
+        .MaxOut = 0.0f,
+        .DeadBand = 0.0f,
+        .Improve = PID_IMPROVE_NONE,
+    };
+    PIDInit(&swerving_pid, &swerving_pid_conf);
+
+    PID_Init_Config_s leg_length_pid_conf = {
+        .Kp = 0.0f,
+        .Kd = 0.0f,
+        .Ki = 0.0f,
+        .MaxOut = 0.0f,
+        .DeadBand = 0.0f,
+        .Improve = PID_IMPROVE_NONE,
+    };
+    PIDInit(&leg_length_pid, &leg_length_pid_conf);
+
+    PID_Init_Config_s roll_compensate_pid_conf = {
+        .Kp = 0.0f,
+        .Kd = 0.0f,
+        .Ki = 0.0f,
+        .MaxOut = 0.0f,
+        .DeadBand = 0.0f,
+        .Improve = PID_IMPROVE_NONE,
+    };
+    PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);
+}
+
+void BalanceTask()
+{
+}

application/balance_chassis/balance.h

@@ -0,0 +1,17 @@
+#pragma once
+
+/**
+ * @brief 平衡底盘初始化
+ * 
+ */
+void BalanceInit();
+
+/**
+ * @brief 平衡底盘任务
+ * 
+ */
+void BalanceTask();
+
+
+
+

application/balance_chassis/balance.md

@@ -0,0 +1,24 @@
+# balance
+
+可以继续解耦,将VMC独立成模块.
+
+目前默认使用平衡底盘时为双板.
+
+## 工作流程
+
+1. 获取控制信息
+2. 根据控制模式将控制指令转化为实际的参考输入
+3. 使用lqr得出的反馈增益,计算二阶倒立摆模型的控制输出;需要根据当前腿长查gain table,或预先拟合K=f(Leg)的函数
+4. 计算二阶倒立摆$[L0 phi0]$和轮腿[phi1 phi4]间的雅可比,根据VMC将lqr的输出[F Tp]映射成[T1 T2] ; 驱动轮不需要映射
+5. 进行综合运动补偿,即转向控制和抗劈叉
+6. 进行腿长控制计算,即长度控制和roll轴水平控制
+7. 进行离地检测判断是否要让腿保持垂直,后续再加入跳跃功能
+8. 根据裁判系统和超级电容的功率信息进行输出限幅
+9. 设置反馈信息,包括裁判系统的数据,并通过电机反馈和IMU数据计算底盘实际运动状态等
+10. 推送反馈信息
+
+电机初始化为电流环即可,注意基于模型的控制需要正确设定单位
+
+如果功率可能超限,需要判定降低功率输出后受影响最小的执行单元,并给予其较大的功率输出衰减(一般不会超功率)
+
+另外, 选择平衡底盘有枪口冷却增益, 注意将这一部分改变反馈给cmd, 以使得shoot有更好的表现

application/balance_chassis/gain_table.h

@@ -0,0 +1,24 @@
+/* 平衡底盘lqr反馈增益和腿长的关系表,可以选择查找精度和插值 */
+
+#pragma once
+#include "stdint.h"
+#include "arm_math.h"
+#include "math.h"
+
+#define GAIN_TABLE_SIZE 100 // 增益表大小
+
+// K 2x6,6个状态变量2个输出(Tp关节电机和T驱动轮电机)
+static float leglen2gain [GAIN_TABLE_SIZE][2][6] = {};
+
+static interpolation_flag = 0; // 插值方式:1 线性插值 0 关闭插值
+
+void EnalbeInterpolation(void)
+{
+    interpolation_flag = 1;
+}
+
+/* 默认关闭插值,向下取整 */
+float LookUpKgain(float leg_length)
+{
+
+}

application/chassis/chassis.c

@@ -82,7 +82,7 @@ void ChassisInit()
         },
         .motor_type = M3508,
     };
-
+    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
     chassis_motor_config.can_init_config.tx_id = 4;
     chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
     motor_lf = DJIMotorInit(&chassis_motor_config);

application/grab/grab.md

@@ -1 +1 @@
-工程机器人夹爪
+工程机器人夹爪  

application/lift/lift.md

@@ -1 +1,3 @@
-抬升/横移机构
+抬升/横移机构
+
+气缸和电磁阀似乎没有必要构建模块,但是也可以组合微动开关和编码器+继电器等传感器构成模块,降低lift的复杂度,完成解耦.

application/robot_def.h

@@ -16,11 +16,11 @@
 #include "master_process.h"
 #include "stdint.h"
 
-
 /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
 #define ONE_BOARD // 单板控制整车
 // #define CHASSIS_BOARD //底盘板
 // #define GIMBAL_BOARD  //云台板
+// #define BALANCE_BOARD //启用平衡底盘,则默认双板且当前板位底盘,目前不支持!请勿使用!
 
 // @todo: 增加机器人类型定义,后续是否要兼容所有机器人?(只兼容步兵英雄哨兵似乎就够了)
 // 通过该宏,你可以直接将所有机器人的参数保存在一处,然后每次只需要修改这个宏就可以替换所有参数
@@ -179,6 +179,7 @@ typedef struct
  *
  */
 
+/* @todo : 对于平衡底盘,需要新增控制模式和控制数据 */
 typedef struct
 {
 #ifdef CHASSIS_BOARD
@@ -198,6 +199,7 @@ typedef struct
 
 } Chassis_Upload_Data_s;
 
+/* @todo : 对于平衡底盘,需要不同的反馈数据 */
 typedef struct
 {
     attitude_t gimbal_imu_data;