添加ai

Core/Inc/FreeRTOSConfig.h

@@ -187,7 +187,7 @@ standard names. */
 
 /* USER CODE BEGIN Defines */
 /* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
-#define configAPPLICATION_ALLOCATED_HEAP         1
+// #define configAPPLICATION_ALLOCATED_HEAP         1
 /* USER CODE END Defines */
 
 #endif /* FREERTOS_CONFIG_H */

User/device/ai.c

@@ -89,66 +89,18 @@ error:
   return DEVICE_ERR;
 }
 
-// void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
+// int8_t AI_PackMCU(AI_t *ai, const AI_Protucol_UpDataMCU_t *data){
-//   cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
+//   if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
-//   cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
-//   cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
-//   cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
-//   cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
-//   cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
-// }
-
-// int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
-//   if (ai == NULL) return DEVICE_ERR_NULL;
-//   if (cmd_host == NULL) return DEVICE_ERR_NULL;
-//   ai->ai_online = false;
-//   memset(&(ai->from_host), 0, sizeof(ai->from_host));
-//   memset(cmd_host, 0, sizeof(*cmd_host));
-//   return 0;
-// }
-
-// int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
 //   ai->to_host.mcu.id = AI_ID_MCU;
-//   memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
+//   ai->to_host.mcu.package = *data;
-//          sizeof(*quat));
+//   ai->to_host.mcu.crc16 =
-//   ai->to_host.mcu.package.data.notice = 0;
+//       CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2);
-//   if (ai->status == AI_STATUS_AUTOAIM)
-//     ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
-//   else if (ai->status == AI_STATUS_HITSWITCH)
-//     ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
-//   else if (ai->status == AI_STATUS_AUTOMATIC)
-//     ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
-
-//   ai->to_host.mcu.package.crc16 = CRC16_Calc(
-//       (const uint8_t *)&(ai->to_host.mcu.package),
-//       sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
 //   return DEVICE_OK;
 // }
 
-// int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
+int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
-//   (void)ref;
-//   ai->to_host.ref.id = AI_ID_REF;
-//   ai->to_host.ref.package.crc16 = CRC16_Calc(
-//       (const uint8_t *)&(ai->to_host.ref.package),
-//       sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
-//   return DEVICE_OK;
-// }
 
-// int8_t AI_StartSend(AI_t *ai, bool ref_update) {
+int8_t AI_HandleOffline(AI_t *ai);
-//   if (ref_update) {
+
-//     if (HAL_UART_Transmit_DMA(
+int8_t AI_StartSend(AI_t *ai, bool ref_online);
-//             BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
-//             sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
-//       return DEVICE_OK;
-//     else
-//       return DEVICE_ERR;
-//   } else {
-//     if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
-//                               (uint8_t *)&(ai->to_host.mcu),
-//                               sizeof(ai->to_host.mcu)) == HAL_OK)
-//       return DEVICE_OK;
-//     else
-//       return DEVICE_ERR;
-//   }
-// }
 

User/device/ai.h

@@ -20,6 +20,9 @@ extern "C" {
 
 /* Exported constants ------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
+#define AI_ID_MCU (0xC4)
+#define AI_ID_REF (0xA8)
+#define AI_ID_AI (0xA1)
 /* Exported types ----------------------------------------------------------- */
 
 /* 电控 -> 视觉 MCU数据结构体*/

User/module/balance_chassis.c

@@ -284,6 +284,8 @@ void Chassis_Output(Chassis_t *c) {
       LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
   Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
                                    c->output.wheel[1]);
+  // Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,
+  //                                  0.0f); // 暂时不让轮子动
 }
 
 int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
@@ -385,7 +387,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                 PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
                          c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
   // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
-  Delta_Tp[1] = -Delta_Tp[0];
+  Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
+                PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
+                         c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
 
   // 左腿控制
   {
@@ -398,12 +402,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     virtual_force[0] =
         (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
         pid_output + 60.0f + roll_compensation_force;
-    // virtual_force[0] =
+
-    //     (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
-    //     pid_output + 60.0f;
     // VMC逆解算（包含摆角补偿）
     if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
                          c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
+    // if (VMC_InverseSolve(&c->vmc_[0], 0.0f,
+    //                      Delta_Tp[0]) == 0) {
       VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
                           &c->output.joint[1]);
     } else {
@@ -424,13 +428,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     virtual_force[1] =
         (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
         pid_output + 65.0f - roll_compensation_force;
-    // virtual_force[1] =
-    //     (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
-    //     pid_output + 65.0f;
 
     // VMC逆解算（包含摆角补偿）
     if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
                          c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
+    // if (VMC_InverseSolve(&c->vmc_[1], 0.0f,
+    //                      Delta_Tp[1]) == 0) {
       VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
                           &c->output.joint[2]);
     } else {

User/module/config.c

@@ -100,7 +100,7 @@ Config_RobotParam_t robot_config = {
             .yaw_omega = {
                 .k = 1.0f,
                 .p = 1.0f,
-                .i = 0.5f,
+                .i = 0.3f,
                 .d = 0.0f,
                 .i_limit = 1.0f,
                 .out_limit = 1.0f,
@@ -108,7 +108,7 @@ Config_RobotParam_t robot_config = {
                 .range = -1.0f,
             },
             .yaw_angle = {
-                .k = 10.0f,
+                .k = 8.0f,
                 .p = 3.0f,
                 .i = 0.0f,
                 .d = 0.0f,
@@ -200,8 +200,8 @@ Config_RobotParam_t robot_config = {
             .range = -1.0f,
         },
         .leg_theta={
-            .k=1.0f,
+            .k=5.0f,
-            .p=5.0f,           /* 摆角比例系数 */
+            .p=2.0f,           /* 摆角比例系数 */
             .i=0.0f,           /* 摆角积分系数 */
             .d=0.0f,           /* 摆角微分系数 */
             .i_limit=0.0f,     /* 积分限幅 */