第一次提交

2026-05-29 17:36:34 +08:00
commit 5b3333125d
1318 changed files with 722705 additions and 0 deletions
@@ -0,0 +1,224 @@
+/**
+ * @file chassis.c
+ * @author NeoZeng neozng1@hnu.edu.cn
+ * @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出
+ *        注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为x正方向;横向运动的右侧为y正方向
+ *
+ * @version 0.1
+ * @date 2022-12-04
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "chassis.h"
+#include "robot_def.h"
+#include "super_cap.h"
+#include "dji_motor.h"
+#include "message_center.h"
+#include "referee_task.h"
+
+#include "general_def.h"
+#include "bsp_dwt.h"
+#include "referee_UI.h"
+#include "arm_math.h"
+
+
+
+/* 根据robot_def.h中的macro自动计算的参数 */
+#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
+#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
+#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
+
+#ifdef ONE_BOARD
+static Publisher_t *chassis_pub;  // 用于发布底盘的数据
+static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令
+#endif
+static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
+static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
+
+static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
+
+
+/* 用于自旋变速策略的时间变量 */
+// static float t;
+
+/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
+static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
+static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
+
+void ChassisInit()
+{
+    // 四个轮子的参数一样,改tx_id和反转标志位即可
+    Motor_Init_Config_s chassis_motor_config = {
+        .can_init_config.can_handle = &hcan1,
+        .controller_param_init_config = {
+            .angle_PID = {
+                .Kp = 50,
+                .Ki = 150,
+                .Kd = 0.3,
+                .IntegralLimit = 1000,
+                .Derivative_LPF_RC = 0.008,
+                .Output_LPF_RC = 0.005,
+                .CoefA= 10.0f,
+                .CoefB = 3.0f,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement |PID_ChangingIntegrationRate | PID_DerivativeFilter  | PID_OutputFilter,
+                .MaxOut = 1000,
+            },
+            .speed_PID = {
+                .Kp = 12,
+                .Ki = 0,
+                .Kd = 0.08,
+                .IntegralLimit = 16000,
+                .Derivative_LPF_RC = 0.008,
+                .Output_LPF_RC = 0.001,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_OutputFilter,
+                .MaxOut = 16000,
+            },
+
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = MOTOR_FEED,
+            .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type =  ANGLE_LOOP, // 设置为开环，电机设定值由下面的功率控制设定，不走普通的pid
+            .close_loop_type = SPEED_LOOP | ANGLE_LOOP, // 角度环和速度环
+        },
+        .motor_type = GM6020_NEW,
+    };
+    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
+    // 使用功率控制的电机需要使用PowerControlInit()函数初始化,因为电机的控制方式不同
+    // chassis_motor_config.can_init_config.tx_id = 1;
+    // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
+    // motor_lf = DJIMotorInit(&chassis_motor_config);
+
+    // chassis_motor_config.can_init_config.tx_id = 2;
+    // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
+    // motor_rf = DJIMotorInit(&chassis_motor_config);
+
+    chassis_motor_config.can_init_config.tx_id = 1;
+    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
+    // motor_lb = DJIMotorInit(&chassis_motor_config);
+    Motor_Init_Config_s pitch_motor_config = {
+        .can_init_config.can_handle = &hcan1,
+        .can_init_config.tx_id = 2, // 俯仰电机的ID
+        .controller_param_init_config = {
+            .angle_PID = {
+                .Kp = 30,
+                .Ki = 80,
+                .Kd = 0.2,
+                .IntegralLimit = 1000,
+                .Derivative_LPF_RC = 0.008,
+                .Output_LPF_RC = 0.005,
+                .CoefA= 20.0f,
+                .CoefB = 10.0f,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement |PID_ChangingIntegrationRate | PID_DerivativeFilter  | PID_OutputFilter,
+                .MaxOut = 1000,
+            },
+            .speed_PID = {
+                .Kp = 8,
+                .Ki = 0,
+                .Kd = 0.02,
+                .IntegralLimit = 16000,
+                .Derivative_LPF_RC = 0.008,
+                .Output_LPF_RC = 0.001,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_OutputFilter,
+                .MaxOut = 16000,
+            },
+
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = MOTOR_FEED,
+            .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type =  ANGLE_LOOP, // 设置为开环，电机设定值由下面的功率控制设定，不走普通的pid
+            .close_loop_type = SPEED_LOOP | ANGLE_LOOP, // 角度环和速度环
+        },
+        .motor_type = GM6020_NEW,
+    };
+    // motor_rb = DJIMotorInit(&pitch_motor_config);
+
+    chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
+    chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
+}
+
+#define LF_CENTER 1
+#define RF_CENTER 1
+#define LB_CENTER 1
+#define RB_CENTER 1
+/**
+ * @brief 计算每个轮毂电机的输出,正运动学解算
+ *        用宏进行预替换减小开销,运动解算具体过程参考教程
+ */
+static void MecanumCalculate()
+{
+    vt_lb = chassis_vx;
+    vt_rb = chassis_cmd_recv.wz;
+}
+
+/**
+ * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
+ *
+ */
+static void LimitChassisOutput()
+{
+    // 完成功率限制后进行电机参考输入设定
+    DJIMotorSetRef(motor_lb, vt_lb);
+    DJIMotorSetRef(motor_rb, vt_rb);
+}
+
+/**
+ * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
+ *        对于双板的情况,考虑增加来自底盘板IMU的数据
+ *
+ */
+static void EstimateSpeed()
+{
+    // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有)
+    // chassis_feedback_data.vx vy wz =
+    //  ...
+}
+
+/* 机器人底盘控制核心任务 */
+void ChassisTask()
+{
+    // 后续增加没收到消息的处理(双板的情况)
+    // 获取新的控制信息
+#ifdef ONE_BOARD
+    SubGetMessage(chassis_sub, &chassis_cmd_recv);
+#endif
+#ifdef CHASSIS_BOARD
+    chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
+#endif // CHASSIS_BOARD
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
+    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
+        DJIMotorStop(motor_lb);
+        DJIMotorStop(motor_rb);
+    }
+    else
+    { // 正常工作
+        
+        DJIMotorEnable(motor_rb);
+        DJIMotorEnable(motor_lb);
+    }
+
+    // 根据控制模式设定旋转速度
+    switch (chassis_cmd_recv.chassis_mode)
+    {
+    default:
+        break;
+    }
+    chassis_vx = chassis_cmd_recv.vx;
+
+    // 根据控制模式进行正运动学解算,计算底盘输出
+    MecanumCalculate();
+
+    // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
+    LimitChassisOutput();
+
+    // 推送反馈消息
+#ifdef ONE_BOARD
+    PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
+#endif
+#ifdef CHASSIS_BOARD
+    CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
+#endif // CHASSIS_BOARD
+}
@@ -0,0 +1,16 @@
+#ifndef CHASSIS_H
+#define CHASSIS_H
+
+/**
+ * @brief 底盘应用初始化,请在开启rtos之前调用(目前会被RobotInit()调用)
+ * 
+ */
+void ChassisInit();
+
+/**
+ * @brief 底盘应用任务,放入实时系统以一定频率运行
+ * 
+ */
+void ChassisTask();
+
+#endif // CHASSIS_H
@@ -0,0 +1,24 @@
+# chassis
+
+
+@Todo 使用条件编译，选择麦轮(全向轮),舵轮,平衡底盘
+## 工作流程
+
+首先进行初始化，`ChasissInit()`会被`RobotInit()`调用，进行裁判系统、底盘电机的初始化。如果为双板模式，则还会初始化IMU，并且将消息订阅者和发布者的初始化改为`CANComm`的初始化。
+
+操作系统启动后，工作顺序为：
+
+1. 从cmd模块获取数据（如果双板则从CANComm获取）
+2. 判断当前控制数据的模式，如果为停止则停止所有电机
+3. 根据控制数据，计算底盘的旋转速度
+4. 根据控制数据中yaw电机的编码器值`angle_offset`，将控制数据映射到底盘坐标系下
+5. 进行麦克纳姆轮的运动学解算，得到每个电机的设定值
+6. 获取裁判系统的数据，并根据底盘功率限制对输出进行限幅
+7. 由电机的反馈数据和IMU（如果有），计算底盘当前的真实运动速度
+8. 设置底盘反馈数据，包括运动速度和裁判系统数据
+9. 将反馈数据推送到消息中心（如果双板则通过CANComm发送）
+
+
+### 后续支持平衡底盘
+
+新增一个app balance_chassis