balance init

application/chassis/balance.c

@@ -0,0 +1,225 @@
+// app
+#include "balance.h"
+#include "robot_def.h"
+#include "general_def.h"
+#include "ins_task.h"
+#include "HT04.h"
+#include "LK9025.h"
+#include "controller.h"
+#include "can_comm.h"
+#include "super_cap.h"
+#include "user_lib.h"
+#include "remote_control.h"
+#include "referee_task.h"
+#include "stdint.h"
+#include "arm_math.h" // 需要用到较多三角函数
+#include "bsp_dwt.h"
+#include "bsp_log.h"
+
+
+static uint32_t balance_dwt_cnt;
+static float del_t;
+
+/* 底盘拥有的模块实例 */
+static attitude_t *imu_data;
+static RC_ctrl_t *rc_data; // 调试用
+static Referee_Interactive_info_t my_ui;
+static referee_info_t *referee_data;
+static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // syh
+static Chassis_Upload_Data_s chassis_feed;
+static CANCommInstance *ci;
+static SuperCapInstance *cap; // syh
+// 四个关节电机和两个驱动轮电机
+static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
+static LKMotorInstance *l_driven, *r_driven, *driven[2];
+// 两个腿的参数,0为左腿,1为右腿
+static LinkNPodParam l_side, r_side; // syh  phi5
+static ChassisParam chassis;
+// 综合运动补偿的PID控制器
+static PIDInstance steer_p_pid, steer_v_pid;   // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
+static PIDInstance anti_crash_pid, phi5_pid;   // 抗劈叉,将输出以相反的方向叠加到左右腿的上
+static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
+static PIDInstance legdot_pid_l, legdot_pid_r;
+static PIDInstance roll_compensate_pid, rolldot_pid; // roll轴补偿,用于保持机体水平
+
+static Robot_Status_e chassis_status;
+
+void BalanceInit()
+{
+    referee_data = UITaskInit(&huart6, &my_ui);
+    rc_data = RemoteControlInit(&huart3);
+    CANComm_Init_Config_s commconf = {
+        .can_config = {
+            .can_handle = &hcan1,
+            .tx_id = 0x40,
+            .rx_id = 0x41},
+        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
+        .send_data_len = sizeof(Chassis_Upload_Data_s)};
+    ci = CANCommInit(&commconf);
+    imu_data = INS_Init();
+
+    SuperCap_Init_Config_s cap_conf = {
+        .can_config = {
+            .can_handle = &hcan1,
+            .tx_id = 0x302, // todo 电容id
+            .rx_id = 0x301}};
+    cap = SuperCapInit(&cap_conf);
+
+    Motor_Init_Config_s joint_conf = {
+        // 写一个,剩下的修改方向和id即可
+        .can_init_config = {
+            .can_handle = &hcan1},
+        .controller_param_init_config = {
+            .angle_PID = {
+                .Kp = 0.3,
+                .Kd = 0.1,
+                .Ki = 0,
+                .DeadBand = 0.0001,
+                .Improve = PID_DerivativeFilter,
+                .MaxOut = 4,
+                .Derivative_LPF_RC = 0.05,
+            }, // 仅用于复位腿
+        },
+        .controller_setting_init_config = {
+            .close_loop_type = ANGLE_LOOP,
+            .outer_loop_type = OPEN_LOOP,
+            .motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
+            .angle_feedback_source = MOTOR_FEED,
+            .speed_feedback_source = MOTOR_FEED,
+        },
+        .motor_type = HT04};
+    joint_conf.can_init_config.tx_id = 4;
+    joint_conf.can_init_config.rx_id = 14;
+    joint[LF] = lf = HTMotorInit(&joint_conf);
+    joint_conf.can_init_config.tx_id = 3;
+    joint_conf.can_init_config.rx_id = 13;
+    joint[LB] = lb = HTMotorInit(&joint_conf);
+    joint_conf.can_init_config.tx_id = 2;
+    joint_conf.can_init_config.rx_id = 12;
+    joint[RF] = rf = HTMotorInit(&joint_conf);
+    joint_conf.can_init_config.tx_id = 1;
+    joint_conf.can_init_config.rx_id = 11;
+    joint[RB] = rb = HTMotorInit(&joint_conf);
+
+    Motor_Init_Config_s driven_conf = {
+        // 写一个,剩下的修改方向和id即可
+        .can_init_config.can_handle = &hcan2,
+        .controller_setting_init_config = {
+            .angle_feedback_source = MOTOR_FEED,
+            .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = OPEN_LOOP,
+            .close_loop_type = OPEN_LOOP,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+        },
+        .motor_type = LK9025,
+    };
+    driven_conf.can_init_config.tx_id = 1;
+    driven[LD] = l_driven = LKMotorInit(&driven_conf);
+    driven_conf.can_init_config.tx_id = 2;
+    driven[RD] = r_driven = LKMotorInit(&driven_conf);
+
+    PID_Init_Config_s steer_p_pid_conf = {
+        .Kp = 2,
+        .Kd = 1,
+        .Ki = 0.0f,
+        .MaxOut = 4,
+        .DeadBand = 0.01f,
+        .Improve = PID_DerivativeFilter,
+        .Derivative_LPF_RC = 0.05,
+    };
+    PIDInit(&steer_p_pid, &steer_p_pid_conf);
+    PID_Init_Config_s steer_v_pid_conf = {
+        .Kp = 2,
+        .Kd = 0.0f,
+        .Ki = 0.0f,
+        .MaxOut = 100,
+        .DeadBand = 0.0f,
+        .Improve = PID_DerivativeFilter | PID_Integral_Limit,
+        .Derivative_LPF_RC = 0.05,
+        .IntegralLimit = 2,
+    };
+    PIDInit(&steer_v_pid, &steer_v_pid_conf);
+
+    PID_Init_Config_s anti_crash_pid_conf = {
+        .Kp = 8,
+        .Kd = 2.5,
+        .Ki = 0.4,
+        .MaxOut = 45,
+        .DeadBand = 0.01f,
+        .Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit,
+        .Derivative_LPF_RC = 0.05,
+        .CoefA = 0.05,
+        .CoefB = 0.05,
+        .IntegralLimit = 2,
+    };
+    PIDInit(&anti_crash_pid, &anti_crash_pid_conf);
+
+    PID_Init_Config_s leg_length_pid_conf = {
+        .Kp = 450,
+        .Kd = 150,
+        .Ki = 5,
+        .MaxOut = 60,
+        .DeadBand = 0.0001f,
+        .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
+        .CoefA = 0.01,
+        .CoefB = 0.02,
+        .Derivative_LPF_RC = 0.08,
+    };
+    PIDInit(&leglen_pid_l, &leg_length_pid_conf);
+    PIDInit(&leglen_pid_r, &leg_length_pid_conf);
+
+    PID_Init_Config_s roll_compensate_pid_conf = {
+        .Kp = 0.0008f,
+        .Kd = 0.00065f,
+        .Ki = 0.0f,
+        .MaxOut = 0.04,
+        .DeadBand = 0.005f,
+        .Improve = PID_DerivativeFilter,
+        .Derivative_LPF_RC = 0.05,
+    };
+    PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);
+
+    l_side.target_len = r_side.target_len = 0.23;
+    chassis.vel_cov = 1000; // 初始化速度协方差
+    chassis_status = ROBOT_READY;
+    DWT_GetDeltaT(&balance_dwt_cnt);
+}
+
+static void EnableAllMotor() /* 打开所有电机 */
+{
+    for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机
+        HTMotorEnable(joint[i]);
+    for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机
+        LKMotorEnable(driven[i]);
+}
+
+/* 切换底盘遥控器控制和云台双板控制 */
+static void ControlSwitch()
+{ // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
+    if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline())
+    {
+        if (rc_data->rc.rocker_l1 < -600)
+        {
+            chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
+            chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
+        }
+        else // 设定值覆盖双板
+        {
+            chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;                        // 自由转动&前后
+            chassis_cmd_recv.vx = 0.02 * (float)rc_data[TEMP].rc.rocker_r1;           // speed x, unit m/s
+            chassis_cmd_recv.offset_angle = 0.001 * (float)rc_data[TEMP].rc.rocker_r_; // rotate? follow.
+            chassis_cmd_recv.delta_leglen = -0.0000015f * (float)rc_data[TEMP].rc.dial;
+        }
+    }
+    else if (CANCommIsOnline(ci) && !switch_is_down(rc_data->rc.switch_right))
+        chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(ci); // 获取云台板指令
+    else
+        chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
+}
+
+
+void BalanceTask()
+{
+ 
+    
+}

application/chassis/balance.h

@@ -0,0 +1,90 @@
+#pragma once
+
+// 底盘参数
+#define CALF_LEN 0.245f           // 小腿
+#define THIGH_LEN 0.14f           // 大腿
+#define JOINT_DISTANCE 0.108f     // 关节间距
+#define WHEEL_RADIUS 0.078f       // 轮子半径
+#define LIMIT_LINK_RAD 0.15149458 // 初始限位角度,见ParamAssemble
+#define WHEEL_DISTANCE 0.48f      // 轮子间距
+#define BALANCE_GRAVITY_BIAS 0
+#define ROLL_GRAVITY_BIAS 0.03f
+#define MAX_ACC_REF 0.7f
+#define MAX_DIST_TRACK 0.1f
+#define MAX_VEL_TRACK 0.5f
+
+#define CENTER_IMU_R 0.13f // IMU距离中心的距离
+#define CENTER_IMU_W 0.11f
+#define CENTER_IMU_L 0.074f
+#define CENTER_IMU_H 0.060f
+#define CENTER_IMU_THETA 0.9768f
+
+#define VEL_PROCESS_NOISE 25  // 速度过程噪声
+#define VEL_MEASURE_NOISE 800 // 速度测量噪声
+// 同时估计加速度和速度时对加速度的噪声
+// 更好的方法是设置为动态,当有冲击时/加加速度大时更相信轮速
+#define ACC_PROCESS_NOISE 2000 // 加速度过程噪声
+#define ACC_MEASURE_NOISE 0.01 // 加速度测量噪声
+
+// 用于循环枚举的宏,方便访问关节电机和驱动轮电机
+#define JOINT_CNT 4u
+#define LF 0u
+#define LB 1u
+#define RF 2u
+#define RB 3u
+
+#define DRIVEN_CNT 2u
+#define LD 0u
+#define RD 1u
+
+typedef struct
+{
+    // joint
+    float phi1_w, phi4_w, phi2_w, phi5_w; // phi2_w used for calc real wheel speed
+    float T_back, T_front;
+    // link angle, phi1-ph5, phi5 is pod angle
+    float phi1, phi2, phi3, phi4, phi5;
+    // wheel
+    float w_ecd;      // 电机编码器速度
+    float wheel_dist; // 单侧轮子的位移
+    float wheel_w;    // 单侧轮子的速度
+    float body_v;     // 髋关节速度
+    float T_wheel;
+    // pod
+    float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一
+    float leg_len, legd;
+    float height, height_v;
+    float F_leg, T_hip;
+    float target_len;
+
+    float coord[6]; // xb yb xc yc xd yd
+
+    float wheel_out[7];
+    float hip_out[7];
+} LinkNPodParam;
+
+typedef struct
+{
+    float vel, target_v;        // 底盘速度
+    float vel_m;                // 底盘速度测量值
+    float vel_predict;          // 底盘速度预测值
+    float vel_cov;              // 速度方差
+    float acc, acc_m, acc_last; // 水平方向加速度,用于计算速度预测值
+
+    float dist, target_dist;   // 底盘位移距离
+    float yaw, wz, target_yaw; // yaw角度和底盘角速度
+    float pitch, pitch_w;      // 底盘俯仰角度和角速度
+    float roll, roll_w;        // 底盘横滚角度和角速度
+} ChassisParam;
+
+/**
+ * @brief 平衡底盘初始化
+ *
+ */
+void BalanceInit();
+
+/**
+ * @brief 平衡底盘任务
+ *
+ */
+void BalanceTask();

application/chassis/balance.md

@@ -0,0 +1,32 @@
+# balance
+
+可以继续解耦,将VMC独立成模块.
+
+目前默认使用平衡底盘时为双板.
+
+## 工作流程
+
+1. 获取控制信息和状态信息，组装到linkparam
+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有更好的表现
+
+## 优化环节
+
+为了控制系统有更好的效果，对工程上的细节有一些微小的优化如下：
+
+1. 腿长控制实际上对机体高度计算腿长闭环，使得云台能够保持恒定高度
+2. 静止时开启位置反馈，若有速度输入则不使用位置反馈，从而避免底盘打滑的情况
+3. 没有转向输入时使用轮式里程计计算位置x，有转向输入时使用imu的二重积分，从而避免平衡控制器和转向控制器冲突

application/chassis/chassis.c

@@ -1,257 +0,0 @@
-/**
- * @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 "dji_motor.h"
-#include "super_cap.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 CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
-#include "can_comm.h"
-#include "ins_task.h"
-static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
-attitude_t *Chassis_IMU_data;
-#endif // CHASSIS_BOARD
-#ifdef ONE_BOARD
-static Publisher_t *chassis_pub;                    // 用于发布底盘的数据
-static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控制命令
-#endif                                              // !ONE_BOARD
-static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
-static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
-
-static referee_info_t* referee_data; // 用于获取裁判系统的数据
-static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
-
-static SuperCapInstance *cap;                                       // 超级电容
-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 = {
-            .speed_PID = {
-                .Kp = 10, // 4.5
-                .Ki = 0,  // 0
-                .Kd = 0,  // 0
-                .IntegralLimit = 3000,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .MaxOut = 12000,
-            },
-            .current_PID = {
-                .Kp = 0.5, // 0.4
-                .Ki = 0,   // 0
-                .Kd = 0,
-                .IntegralLimit = 3000,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .MaxOut = 15000,
-            },
-        },
-        .controller_setting_init_config = {
-            .angle_feedback_source = MOTOR_FEED,
-            .speed_feedback_source = MOTOR_FEED,
-            .outer_loop_type = SPEED_LOOP,
-            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
-        },
-        .motor_type = M3508,
-    };
-    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
-    chassis_motor_config.can_init_config.tx_id = 1;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-    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_REVERSE;
-    motor_rf = DJIMotorInit(&chassis_motor_config);
-
-    chassis_motor_config.can_init_config.tx_id = 4;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-    motor_lb = DJIMotorInit(&chassis_motor_config);
-
-    chassis_motor_config.can_init_config.tx_id = 3;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-    motor_rb = DJIMotorInit(&chassis_motor_config);
-
-    referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
-
-    SuperCap_Init_Config_s cap_conf = {
-        .can_config = {
-            .can_handle = &hcan2,
-            .tx_id = 0x302, // 超级电容默认接收id
-            .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
-        }};
-    cap = SuperCapInit(&cap_conf); // 超级电容初始化
-
-    // 发布订阅初始化,如果为双板,则需要can comm来传递消息
-#ifdef CHASSIS_BOARD
-    Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
-
-    CANComm_Init_Config_s comm_conf = {
-        .can_config = {
-            .can_handle = &hcan2,
-            .tx_id = 0x311,
-            .rx_id = 0x312,
-        },
-        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
-        .send_data_len = sizeof(Chassis_Upload_Data_s),
-    };
-    chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化
-#endif                                          // CHASSIS_BOARD
-
-#ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息
-    chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
-    chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
-#endif // ONE_BOARD
-}
-
-#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
-#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
-#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
-#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
-/**
- * @brief 计算每个轮毂电机的输出,正运动学解算
- *        用宏进行预替换减小开销,运动解算具体过程参考教程
- */
-static void MecanumCalculate()
-{
-    vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
-    vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
-    vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
-    vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
-}
-
-/**
- * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
- *
- */
-static void LimitChassisOutput()
-{
-    // 功率限制待添加
-    // referee_data->PowerHeatData.chassis_power;
-    // referee_data->PowerHeatData.chassis_power_buffer;
-
-    // 完成功率限制后进行电机参考输入设定
-    DJIMotorSetRef(motor_lf, vt_lf);
-    DJIMotorSetRef(motor_rf, vt_rf);
-    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_lf);
-        DJIMotorStop(motor_rf);
-        DJIMotorStop(motor_lb);
-        DJIMotorStop(motor_rb);
-    }
-    else
-    { // 正常工作
-        DJIMotorEnable(motor_lf);
-        DJIMotorEnable(motor_rf);
-        DJIMotorEnable(motor_lb);
-        DJIMotorEnable(motor_rb);
-    }
-
-    // 根据控制模式设定旋转速度
-    switch (chassis_cmd_recv.chassis_mode)
-    {
-    case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
-        chassis_cmd_recv.wz = 0;
-        break;
-    case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
-        chassis_cmd_recv.wz = -1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
-        break;
-    case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-        chassis_cmd_recv.wz = 4000;
-        break;
-    default:
-        break;
-    }
-
-    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
-    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
-    static float sin_theta, cos_theta;
-    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-    chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
-    chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
-
-    // 根据控制模式进行正运动学解算,计算底盘输出
-    MecanumCalculate();
-
-    // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
-    LimitChassisOutput();
-
-    // 根据电机的反馈速度和IMU(如果有)计算真实速度
-    EstimateSpeed();
-
-    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
-    // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
-    // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
-    // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
-    // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
-    // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
-
-    // 推送反馈消息
-#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
-}

application/chassis/chassis.h

@@ -1,16 +0,0 @@
-#ifndef CHASSIS_H
-#define CHASSIS_H
-
-/**
- * @brief 底盘应用初始化,请在开启rtos之前调用(目前会被RobotInit()调用)
- * 
- */
-void ChassisInit();
-
-/**
- * @brief 底盘应用任务,放入实时系统以一定频率运行
- * 
- */
-void ChassisTask();
-
-#endif // CHASSIS_H

application/chassis/chassis.md

@@ -1,24 +0,0 @@
-# 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