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

.vscode/launch.json

@@ -43,7 +43,7 @@
             "interface": "swd",
             "svdFile": "STM32F407.svd",
             "rtos": "FreeRTOS",
-            "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用
+            //"preLaunchTask": "build task",//先运行Build任务,取消注释即可使用
             "liveWatch": {
                 "enabled": true,
                 "samplesPerSecond": 4

.vscode/tasks.json

@@ -24,7 +24,7 @@
         {
             "label": "download jlink", // 要使用此任务,需添加jlink的环境变量
             "type": "shell",
-            "command":"mingw32-make -j24 ; mingw32-make download_jlink", // "mingw32-make -j24 ; mingw32-make download_jlink"
+            "command":" download_jlink", // "mingw32-make -j24 ; mingw32-make download_jlink"
             "group": {
                 "kind": "build",
                 "isDefault": false,

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;

application/cmd/robot_cmd.c

@@ -83,11 +83,11 @@ static void CalcOffsetAngle()
     // @todo:相差一整圈时会出问题，待修复
     // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
     uint16_t yaw_chassis_align_ecd;
-    if(chassis_direction == CHASSIS_ALIGN)
+    if (chassis_direction == CHASSIS_ALIGN)
     {
         yaw_chassis_align_ecd = 2716;
     }
-    else if(chassis_direction == CHASSIS_SIDLE)
+    else if (chassis_direction == CHASSIS_SIDLE)
     {
         yaw_chassis_align_ecd = 765;
     }
@@ -96,7 +96,7 @@ static void CalcOffsetAngle()
     yaw_align_angle = yaw_chassis_align_ecd * ECD_ANGLE_COEF_DJI; // 从底盘获取的yaw电机对齐角度
     angle = gimbal_fetch_data.yaw_motor_single_round_angle;       // 从云台获取的当前yaw电机单圈角度
 
-    if (yaw_chassis_align_ecd > 4096)                             // 如果大于180度
+    if (yaw_chassis_align_ecd > 4096) // 如果大于180度
     {
         if (angle > yaw_align_angle)
             chassis_cmd_send.offset_angle = angle - yaw_align_angle;
@@ -106,7 +106,7 @@ static void CalcOffsetAngle()
             chassis_cmd_send.offset_angle = angle - yaw_align_angle + 360.0f;
     }
     else
-    {   // 小于180度
+    { // 小于180度
         if (angle > yaw_align_angle && angle <= 180.0f + yaw_align_angle)
             chassis_cmd_send.offset_angle = angle - yaw_align_angle;
         else if (angle > 180.0f + yaw_align_angle)
@@ -122,19 +122,21 @@ static void CalcOffsetAngle()
  */
 static void RemoteControlSet()
 {
+    memcpy(&rc_data->key_count, 0, sizeof(rc_data->key_count));
     gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
 
     // 云台参数,确定云台控制数据
     gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_;
     gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1;
+    chassis_cmd_send.rotate_w = 0.0f;
 
     // 摇杆控制的软件限位
     gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
 
     if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[下],视觉模式
     {
-        gimbal_cmd_send.yaw = ( vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw );
-        gimbal_cmd_send.pitch = ( vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch );
+        gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw);
+        gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch);
     }
 
     // 底盘参数
@@ -150,7 +152,17 @@ static void RemoteControlSet()
     // 右侧开关状态为[下],底盘旋转
     if (switch_is_down(rc_data[TEMP].rc.switch_right))
     {
-        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+        chassis_cmd_send.rotate_w = 4.0f;
+        chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+
+        if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
+        {
+            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+        }
+        if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
+        {
+            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE;
+        }
         chassis_cmd_send.vx = 0;
     }
     else
@@ -182,7 +194,7 @@ static void RemoteControlSet()
 
     // 发射参数
     shoot_cmd_send.bullet_speed = 30;
-    shoot_cmd_send.shoot_rate = 15;
+    shoot_cmd_send.shoot_rate = 10;
 }
 
 /**
@@ -193,15 +205,14 @@ static void MouseKeySet()
 {
     gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
 
-    gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f ; // 系数待测
+    gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f; // 系数待测
     gimbal_cmd_send.pitch += (float)(rc_data[TEMP].mouse.y + rc_data[TEMP].mouse.y) / 660.0f * 4.0f;
 
     gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
 
-    if(chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
+    if (chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
     {
-        chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s 
-                                - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d);
+        chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d);
     }
     else if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE)
     {
@@ -218,17 +229,14 @@ static void MouseKeySet()
     {
         chassis_direction = CHASSIS_SIDLE;
     }
-
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q 小陀螺
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2)
     {
     case 0:
+        break;
+    case 1:
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         break;
-    default:
-        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-        break;
     }
-
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F 摩擦轮
     {
     case 0:
@@ -239,7 +247,7 @@ static void MouseKeySet()
         break;
     }
 
-    if(shoot_cmd_send.friction_mode == FRICTION_ON)
+    if (shoot_cmd_send.friction_mode == FRICTION_ON)
     {
         if (rc_data[TEMP].mouse.press_l)
             shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
@@ -249,7 +257,7 @@ static void MouseKeySet()
     else
         shoot_cmd_send.load_mode = LOAD_STOP;
 
-    switch (rc_data[TEMP].key[KEY_PRESS].z)  // Z键刷新UI
+    switch (rc_data[TEMP].key[KEY_PRESS].z) // Z键刷新UI
     {
     case 0:
         chassis_cmd_send.ui_mode = UI_KEEP;
@@ -264,6 +272,15 @@ static void MouseKeySet()
         gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw);
         gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch);
     }
+    if (rc_data[TEMP].key[KEY_PRESS].q)
+    {
+        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+        chassis_cmd_send.rotate_w = 6.0f;
+    }
+    if (rc_data[TEMP].key[KEY_PRESS].r)
+    {
+        chassis_cmd_send.chassis_mode = CHASSIS_RESET;
+    }
 }
 
 /**
@@ -302,9 +319,7 @@ static void EmergencyHandler()
         switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C] % 2) // ctrl+c 进入急停
         {
         case 0:
-            robot_state = ROBOT_READY;
-            shoot_cmd_send.shoot_mode = SHOOT_ON;
-            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+
             break;
 
         default:

application/robot_def.h

@@ -18,8 +18,8 @@
 
 /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
 // #define ONE_BOARD // 单板控制整车
-#define CHASSIS_BOARD //底盘板
-// #define GIMBAL_BOARD  //云台板
+// #define CHASSIS_BOARD //底盘板
+#define GIMBAL_BOARD  //云台板
 
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
 // #define VISION_USE_UART // 使用串口发送视觉数据
@@ -91,6 +91,7 @@ typedef enum
     CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
     CHASSIS_RESET,             // 底盘重置,双腿缩回
     CHASSIS_FREE_DEBUG,        // 底盘单独调试模式
+    CHASSIS_ROTATE_REVERSE,
 } chassis_mode_e;
 
 

modules/super_cap/super_cap.c

@@ -35,7 +35,7 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
 void SuperCapSend(SuperCapInstance *instance, uint8_t *data)
 {
     memcpy(instance->can_ins->tx_buff, data, 8);
-    CANTransmit(instance->can_ins,1);
+    CANTransmit(instance->can_ins,0.5);
 }
 
 SuperCap_Msg_s SuperCapGet(SuperCapInstance *instance)