装超电后为预检录准备的版本

application/chassis/balance.c

@@ -53,12 +53,21 @@ static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据
 
 static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
 
+static SuperCapInstance *cap; // 超级电容
+static uint16_t DataSend2Cap[4] = {0, 0, 0, 0};
+
 void BalanceInit()
 {
     rc_data = RemoteControlInit(&huart3);
     Chassis_IMU_data = INS_Init();
     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); // ww超级电容初始化
     CANComm_Init_Config_s comm_conf = {
         .can_config = {
             .can_handle = &hcan2,
@@ -120,6 +129,7 @@ void BalanceInit()
             .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
         },
         .motor_type = LK9025,
+
     };
     driven_conf.can_init_config.tx_id = 1;
     driven[LD] = l_driven = LKMotorInit(&driven_conf);
@@ -229,8 +239,9 @@ static uint8_t DrivenMotorIsLost()
 /* 切换底盘遥控器控制和云台双板控制 */
 static void ControlSwitch()
 {
-    // 根据裁判系统底盘输出电压设定底盘状态
     float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001;
+
+    // 根据裁判系统底盘输出电压设定底盘状态
     if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost())
     {
         chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
@@ -259,6 +270,10 @@ static void ControlSwitch()
     //     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
     // }
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
+    // if (abs(l_side.theta) > (30.0f * DEGREE_2_RAD) || abs(r_side.theta) > (30.0f * DEGREE_2_RAD))
+    // {
+    //     chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
+    // }
 }
 
 /* 腿缩回复位,只允许驱动轮电机移动 */
@@ -344,12 +359,12 @@ static void WokingStateSet()
     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.12, 0.25);
-    VAL_LIMIT(r_side.target_len, 0.12, 0.25);
+    VAL_LIMIT(l_side.target_len, 0.12, 0.30);
+    VAL_LIMIT(r_side.target_len, 0.12, 0.30);
 
     // 加速度限幅,防止键盘控制摔倒
     chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
-
+    VAL_LIMIT(r_side.target_len, 0.002, 2.0);
     // 角度输入
     if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
     {
@@ -397,15 +412,19 @@ static void ParamAssemble()
 
 static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
 {
-    if(chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG || 
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG ||
         chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随
     {
         float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
         PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
     }
-    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE)   // 小陀螺
+    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺
     {
-        PIDCalculate(&steer_v_pid, chassis.wz, 6);
+        PIDCalculate(&steer_v_pid, chassis.wz, chassis_cmd_recv.rotate_w);
+    }
+    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE_REVERSE)
+    {
+        PIDCalculate(&steer_v_pid, chassis.wz, -chassis_cmd_recv.rotate_w);
     }
     l_side.T_wheel -= steer_v_pid.Output;
     r_side.T_wheel += steer_v_pid.Output;
@@ -444,8 +463,16 @@ static void WattLimitSet() /* 设定运动模态的输出 */
 // 裁判系统,双板通信,电容功率控制等
 static void CommNPower()
 {
+    static uint8_t supercap_send_cnt = 0;
     // CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
-
+    supercap_send_cnt++;
+    if (supercap_send_cnt % 5 == 0)
+    {
+        DataSend2Cap[0] = referee_data->PowerHeatData.buffer_energy; // 200hz发送
+        DataSend2Cap[1] = referee_data->GameRobotState.chassis_power_limit;
+        SuperCapSend(cap, (uint8_t *)&DataSend2Cap);
+        supercap_send_cnt = 0;
+    }
     /* 更新ui数据 */
     ui_data.direction = chassis_cmd_recv.direction;
     ui_data.friction_mode = chassis_cmd_recv.friction_mode;
@@ -459,7 +486,7 @@ static void CommNPower()
 void BalanceTask()
 {
     del_t = DWT_GetDeltaT(&balance_dwt_cnt);
-    
+
     BuzzerOn();
     // 切换遥控器控制or云台板控制
     ControlSwitch();

application/chassis/balance.h

@@ -10,7 +10,7 @@
 #define LIMIT_LINK_RAD 0.220039368      // 初始限位角度,见ParamAssemble
 #define BALANCE_GRAVITY_BIAS 0
 #define ROLL_GRAVITY_BIAS 0
-#define MAX_ACC_REF 0.9f
+#define MAX_ACC_REF 1.2f
 
 // 驱动轮质量
 #define WHEEL_MASS 0.58f

application/chassis/speed_estimation.h

@@ -61,7 +61,7 @@ void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS
     VAL_LIMIT(cp->vel_cov, 0.01, 100);       // 协方差限幅
 
     // 速度和位置分离，有速度输入时不进行位置闭环
-    if(abs(cp->target_v) < 0.001)
+    if(abs(cp->target_v) < 0.005)
     {
         cp->target_dist = 0;
         cp->dist += cp->vel * delta_t;