改纤维

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -135,7 +135,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>CMSIS_AGDI</Key>
-          <Name>-X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
+          <Name>-X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -157,6 +157,22 @@
         <Bp>
           <Number>0</Number>
           <Type>0</Type>
+          <LineNumber>255</LineNumber>
+          <EnabledFlag>1</EnabledFlag>
+          <Address>134218668</Address>
+          <ByteObject>0</ByteObject>
+          <HtxType>0</HtxType>
+          <ManyObjects>0</ManyObjects>
+          <SizeOfObject>0</SizeOfObject>
+          <BreakByAccess>0</BreakByAccess>
+          <BreakIfRCount>1</BreakIfRCount>
+          <Filename>startup_stm32h723xx.s</Filename>
+          <ExecCommand></ExecCommand>
+          <Expression>\\CtrBoard_H7_ALL\startup_stm32h723xx.s\255</Expression>
+        </Bp>
+        <Bp>
+          <Number>1</Number>
+          <Type>0</Type>
           <LineNumber>0</LineNumber>
           <EnabledFlag>1</EnabledFlag>
           <Address>128</Address>
@@ -171,7 +187,7 @@
           <Expression>0x00000080</Expression>
         </Bp>
         <Bp>
-          <Number>1</Number>
+          <Number>2</Number>
           <Type>0</Type>
           <LineNumber>0</LineNumber>
           <EnabledFlag>1</EnabledFlag>
@@ -208,46 +224,6 @@
           <WinNumber>1</WinNumber>
           <ItemText>gimbal</ItemText>
         </Ww>
-        <Ww>
-          <count>4</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>GIMBAL_IMU_Update</ItemText>
-        </Ww>
-        <Ww>
-          <count>5</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>rx_msg</ItemText>
-        </Ww>
-        <Ww>
-          <count>6</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>count</ItemText>
-        </Ww>
-        <Ww>
-          <count>7</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>rx_msg</ItemText>
-        </Ww>
-        <Ww>
-          <count>8</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>can2_debug</ItemText>
-        </Ww>
-        <Ww>
-          <count>9</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>fdcan2_init_debug</ItemText>
-        </Ww>
-        <Ww>
-          <count>10</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>rx_debug</ItemText>
-        </Ww>
-        <Ww>
-          <count>11</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>cmd_for_gimbal</ItemText>
-        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -296,7 +272,7 @@
         <EnableFlashSeq>1</EnableFlashSeq>
         <EnableLog>0</EnableLog>
         <Protocol>2</Protocol>
-        <DbgClock>5000000</DbgClock>
+        <DbgClock>1000000</DbgClock>
       </DebugDescription>
     </TargetOption>
   </Target>

User/device/ai.c

@@ -0,0 +1,152 @@
+/*
+AI
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "ai.h"
+
+#include <string.h>
+
+#include "bsp\delay.h"
+#include "bsp\uart.h"
+#include "component\crc16.h"
+#include "component\crc8.h"
+#include "component\user_math.h"
+#include "component\filter.h"
+
+
+/* Private define ----------------------------------------------------------- */
+#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static volatile uint32_t drop_message = 0;
+
+static uint8_t rxbuf[AI_LEN_RX_BUFF];
+
+static bool inited = false;
+
+static osThreadId_t thread_alert;
+
+/* Private function  -------------------------------------------------------- */
+
+static void Ai_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
+static void Ai_IdleLineCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t AI_Init(AI_t *ai) {
+  UNUSED(ai);
+  ASSERT(ai);
+  if (inited) return DEVICE_ERR_INITED;
+  VERIFY((thread_alert = osThreadGetId()) != NULL);
+
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
+                            Ai_RxCpltCallback);
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
+                            Ai_IdleLineCallback);
+  
+  inited = true;
+  return 0;
+}
+
+int8_t AI_Restart(void) {
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
+  return DEVICE_OK;
+}
+
+int8_t AI_StartReceiving(AI_t *ai) {
+  UNUSED(ai);
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
+                           AI_LEN_RX_BUFF) == HAL_OK)
+    return DEVICE_OK;
+  return DEVICE_ERR;
+}
+
+bool AI_WaitDmaCplt(void) {
+  return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
+          SIGNAL_AI_RAW_REDY);
+}
+
+int8_t AI_ParseHost(AI_t *ai) {
+  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
+    goto error;
+  ai->ai_online = true;
+  memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
+  memset(rxbuf, 0, AI_LEN_RX_BUFF);
+  return DEVICE_OK;
+
+error:
+  drop_message++;
+  return DEVICE_ERR;
+}
+
+void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
+  cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
+  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,
+         sizeof(*quat));
+  ai->to_host.mcu.package.data.notice = 0;
+  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) {
+  (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) {
+  if (ref_update) {
+    if (HAL_UART_Transmit_DMA(
+            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

@@ -0,0 +1,69 @@
+/*
+  AI
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "component\ahrs.h"
+#include "component\cmd.h"
+#include "component\user_math.h"
+#include "component\filter.h"
+#include "device\device.h"
+#include "device\referee.h"
+#include "protocol.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct __packed {
+  uint8_t id;
+  Protocol_UpPackageReferee_t package;
+} AI_UpPackageReferee_t;
+
+typedef struct __packed {
+  uint8_t id;
+  Protocol_UpPackageMCU_t package;
+} AI_UpPackageMCU_t;
+
+typedef struct __packed {
+  osThreadId_t thread_alert;
+
+  Protocol_DownPackage_t from_host;
+
+  struct {
+    AI_UpPackageReferee_t ref;
+    AI_UpPackageMCU_t mcu;
+  } to_host;
+
+  CMD_AI_Status_t status;
+  bool ai_online;
+} AI_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t AI_Init(AI_t *ai);
+int8_t AI_Restart(void);
+
+int8_t AI_StartReceiving(AI_t *ai);
+bool AI_WaitDmaCplt(void);
+int8_t AI_ParseHost(AI_t *ai);
+int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
+int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
+int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
+int8_t AI_StartSend(AI_t *ai, bool option);
+void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
+#ifdef __cplusplus
+}
+#endif
+
+
+

User/module/balance_chassis.h

@@ -38,19 +38,10 @@ extern "C" {
 
 typedef enum {
   CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
-  CHASSIS_MODE_RECOVER,                 /* 复位模式 */
+  CHASSIS_MODE_RECOVER,          /* 复位模式 */
-  CHASSIS_MODE_WHELL_LEG_BALANCE,      /* 轮子+腿平衡模式，底盘自我平衡 */
+  CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
-  CHASSIS_MODE_JUMP,                   /* 跳跃模式，底盘跳跃 */
-  CHASSIS_MODE_ROTOR,                  /* 小陀螺模式，底盘高速旋转 */
 } Chassis_Mode_t;
 
-typedef enum {
-  CHASSIS_JUMP_STATE_READY,  /* 准备跳跃 */
-  CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */
-  CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */
-  CHASSIS_JUMP_STATE_END, /* 跳跃结束 */
-} Chassis_JumpState_t;
-
 typedef struct {
   Chassis_Mode_t mode;           /* 底盘模式 */
   MoveVector_t move_vec;         /* 底盘运动向量 */
@@ -126,13 +117,6 @@ typedef struct {
   VMC_t vmc_[2]; /* 两条腿的VMC */
   LQR_t lqr[2]; /* 两条腿的LQR控制器 */
 
-  int8_t state;
-  uint64_t jump_time;
-
-
-  float angle;
-  float height;
-  
   /* 机体状态估计 */
   struct {
     float position_x;     /* 机体x位置 */
@@ -151,8 +135,6 @@ typedef struct {
     bool is_reversed;     /* 是否使用反转的零点（180度零点），影响前后方向 */
   } yaw_control;
 
-  float wz_multi; /* 小陀螺模式旋转方向 */
-
   /* PID计算的目标值 */
   struct {
     AHRS_Eulr_t chassis;
@@ -169,8 +151,9 @@ typedef struct {
 
   /* 滤波器 */
   struct {
-    LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
+    LowPassFilter2p_t joint_out[4];  /* 关节输出滤波器 */
-    LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
+    LowPassFilter2p_t wheel_out[2];  /* 轮子输出滤波器 */
+    LowPassFilter2p_t velocity_est;  /* 速度估计滤波器 */
   } filter;
 
 } Chassis_t;

User/module/config.c

@@ -24,7 +24,7 @@ Config_RobotParam_t robot_config = {
     .gimbal_param = {
         .pid = {
             .yaw_omega = {
-                .k = 1.0f,
+                .k = 2.0f,
                 .p = 1.0f,
                 .i = 0.3f,
                 .d = 0.0f,
@@ -34,8 +34,8 @@ Config_RobotParam_t robot_config = {
                 .range = -1.0f,
             },
             .yaw_angle = {
-                .k = 8.0f,
+                .k = 7.0f,
-                .p = 3.0f,
+                .p = 3.5f,
                 .i = 0.0f,
                 .d = 0.0f,
                 .i_limit = 0.0f,
@@ -54,10 +54,10 @@ Config_RobotParam_t robot_config = {
                 .range = -1.0f,
             },
             .pit_angle = {
-                .k = 5.0f,
+                .k = 2.5f,
                 .p = 5.0f,
-                .i = 2.5f,
+                .i = 0.2f,
-                .d = 0.0f,
+                .d = 0.01f,
                 .i_limit = 0.0f,
                 .out_limit = 10.0f,
                 .d_cutoff_freq = -1.0f,
@@ -66,11 +66,11 @@ Config_RobotParam_t robot_config = {
         },
         .mech_zero = {
             .yaw = 0.0f,
-            .pit = 2.12f,
+            .pit = 2.2f,
         },
         .travel = {
             .yaw = -1.0f,
-            .pit = 0.9f,
+            .pit = 0.85f,
         },
         .low_pass_cutoff_freq = {
             .out = -1.0f,
@@ -180,7 +180,7 @@ Config_RobotParam_t robot_config = {
 
     .chassis_param = {
         .yaw={
-            .k=0.0f,
+            .k=1.0f,
             .p=1.0f,
             .i=0.0f,
             .d=0.3f,
@@ -297,7 +297,7 @@ Config_RobotParam_t robot_config = {
             },
         },
 
-        .mech_zero_yaw = 4.34085676f, /* 250.5度，机械零点 */
+        .mech_zero_yaw = 1.78040409f, /* 机械零点 */
 
                 .vmc_param = {
             { // 左腿
@@ -317,18 +317,18 @@ Config_RobotParam_t robot_config = {
         },
 
 .lqr_gains = {
-    .k11_coeff = { -2.111003343424443e+02f, 2.534552823281283e+02f, -1.288428090302336e+02f, -4.837794661314790e-01f }, // theta
+    .k11_coeff = { -2.120324305163214e+02f, 2.384281822810979e+02f, -1.162210131498081e+02f, -2.405740963481636e+00f }, // theta
-    .k12_coeff = { 9.842131763802227e+00f, -7.572331320496771e+00f, -6.747008033464407e+00f, 2.794374462854655e-02f },  // d_theta
+    .k12_coeff = { 4.320836680770054e-01f, 1.360781729068518e-01f, -8.343612068216379e+00f, -8.600090910123033e-02f },  // d_theta
-    .k13_coeff = { -7.026854551050478e+01f, 7.582881862804882e+01f, -2.920474536554779e+01f, 1.123343465922494e+00f },  // x
+    .k13_coeff = { -4.493046202256553e+01f, 4.577344792125822e+01f, -1.629835599958664e+01f, -1.007247166173255e+00f },  // x
-    .k14_coeff = { -7.955300118741869e+01f, 8.683186601398823e+01f, -3.503519879783562e+01f, 1.204200953017506e+00f },  // d_x
+    .k14_coeff = { -4.823335755850488e+01f, 4.987409533322209e+01f, -1.917162943665977e+01f, -1.473642463713354e+00f },  // d_x
-    .k15_coeff = { 2.214748921482655e+01f, -5.719424594843836e+00f, -8.215498046486028e+00f, 3.357225411090161e+00f }, // phi
+    .k15_coeff = { -4.920796990864941e+01f, 6.450325939254844e+01f, -3.268284723443183e+01f, 7.841340265493823e+00f }, // phi
-    .k16_coeff = { -2.588732118811617e-01f, 3.189187555191166e+00f, -3.235989186139448e+00f, 9.848256812196189e-01f },  // d_phi
+    .k16_coeff = { -1.264530042590822e+01f, 1.663296191858249e+01f, -8.467446023207946e+00f, 2.096442008644146e+00f },  // d_phi
-    .k21_coeff = { 3.783375900877637e+02f, -3.033633007638497e+02f, 4.903946574528503e+01f, 1.550109363173939e+01f }, // theta
+    .k21_coeff = { 1.636475235954215e+02f, -1.128937920212271e+02f, 4.887401528348596e+00f, 1.459120525493287e+01f }, // theta
-    .k22_coeff = { 2.878430656512739e+01f, -2.993892895824752e+01f, 1.019545567095573e+01f, 1.119137437173934e+00f },  // d_theta
+    .k22_coeff = { 9.939826270288583e+00f, -8.353709902293666e+00f, 1.640639416293288e+00f, 1.492185865897709e+00f },  // d_theta
-    .k23_coeff = { 2.518975783541210e+01f, 2.088553619504979e+00f, -1.929185285401291e+01f, 8.558202445583612e+00f },  // x
+    .k23_coeff = { -4.968599085108445e+01f, 6.646556717472484e+01f, -3.397333983104631e+01f, 7.847958130301292e+00f },  // x
-    .k24_coeff = { 2.519335578848752e+01f, 5.878465085842500e+00f, -2.359290011046876e+01f, 1.050067818606354e+01f },  // d_x
+    .k24_coeff = { -7.188031995054928e+01f, 9.082377283123918e+01f, -4.412648091833633e+01f, 9.959213854333724e+00f },  // d_x
-    .k25_coeff = { 1.146427691025439e+02f, -1.303101485018965e+02f, 5.234994297700838e+01f, 1.889931627651257e+00f }, // phi
+    .k25_coeff = { 2.360507220981238e+02f, -2.398095392324340e+02f, 8.536061841837561e+01f, 2.469595316477948e+00f }, // phi
-    .k26_coeff = { 3.214791991578748e+01f, -3.467989029865478e+01f, 1.318125503363052e+01f, -3.471228203459202e-01f },  // d_phi
+    .k26_coeff = { 6.296425580760564e+01f, -6.412220242164098e+01f, 2.293354052056732e+01f, 4.870876539985159e-01f },  // d_phi
 },
         .theta = 0.0f,
         .x = 0.0f,

User/module/gimbal.c

@@ -33,6 +33,9 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
   if (mode == g->mode)
     return GIMBAL_OK;
 
+  // 检查是否relax->absolute
+  int relax_to_absolute = (g->mode == GIMBAL_MODE_RELAX && mode == GIMBAL_MODE_ABSOLUTE);
+
   PID_Reset(&g->pid.yaw_angle);
   PID_Reset(&g->pid.yaw_omega);
   PID_Reset(&g->pid.pit_angle);
@@ -43,14 +46,13 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
   MOTOR_DM_Enable(&(g->param->yaw_motor));
 
   AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
-  // if (g->mode == GIMBAL_MODE_RELAX) {
+
-  //   if (mode == GIMBAL_MODE_ABSOLUTE) {
+  if (relax_to_absolute) {
-  //     g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+    // pitch回到限位中点
-  //   } else if (mode == GIMBAL_MODE_RELATIVE) {
+    g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
-  //     g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+  } else {
-  //   }
+    g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
-  // }
+  }
-  g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
   g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
 
   g->mode = mode;
@@ -253,7 +255,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
 void Gimbal_Output(Gimbal_t *g){
     MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
     MOTOR_MIT_Output_t output = {0};
-    output.torque = g->out.yaw * 5.0f; // 乘以减速比
+    output.torque = g->out.yaw * 5.0f;
     output.kd = 0.3f;
     MOTOR_RM_Ctrl(&g->param->pit_motor);
     MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);

User/module/shoot.c

@@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
 	
     memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
     memset(&s->output,0,sizeof(s->output));
-    s->target_variable.target_rpm=6000.0f;  /* 归一化目标转速 */
+    s->target_variable.target_rpm=4000.0f;  /* 归一化目标转速 */
 
 	return 0;
 }

User/task/ctrl_gimbal.c

@@ -52,8 +52,8 @@ void Task_ctrl_gimbal(void *argument) {
 
     Gimbal_UpdateFeedback(&gimbal);
 
-    // osMessageQueueReset(task_runtime.msgq.chassis_yaw);
+    osMessageQueueReset(task_runtime.msgq.chassis.yaw); // 重置消息队列，防止阻塞
-    // osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
+    osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
     
     Gimbal_Control(&gimbal, &gimbal_cmd);
 

User/task/init.c

@@ -34,7 +34,7 @@ void Task_Init(void *argument) {
   /* 创建任务线程 */
   task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
   task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit);
-  // task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
+  task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
   task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
   task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
   task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);

utils/Simulation-master/balance/series_legs/get_k_length.m

@@ -17,7 +17,7 @@ function K = get_k_length(leg_length)
     R1=0.068;                         %驱动轮半径
     L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
     LM1=leg_length/2;                 %摆杆重心到其转轴距离
-    l1=-0.03;                          %机体质心距离转轴距离
+    l1=0.01;                          %机体质心距离转轴距离
     mw1=0.60;                         %驱动轮质量
     mp1=1.8;                         %杆质量
     M1=12.0;                          %机体质量
@@ -48,7 +48,7 @@ function K = get_k_length(leg_length)
     B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
     B=double(B);
     
-    Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    Q=diag([1500 100 2500 1500 8000 500]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
     R=[250 0;0 50];                %T Tp
     
     K=lqr(A,B,Q,R);