robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化云台

Browse Source
This commit is contained in:
Robofish 2026-01-11 23:22:12 +08:00
parent 6228cafada
commit 8c44c6a274
16 changed files with 6553 additions and 6133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -82,6 +82,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/module/balance_chassis.c
User/module/config.c
User/module/shoot.c
User/module/gimbal.c
# User/task sources
User/task/atti_esit.c

79
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -153,7 +153,24 @@
<Name>-U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(5BA02477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
<Breakpoint>
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>56</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>134292314</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename>..\User\task\ctrl_gimbal.c</Filename>
<ExecCommand></ExecCommand>
<Expression>\\CtrBoard_H7_ALL\../User/task/ctrl_gimbal.c\56</Expression>
</Bp>
</Breakpoint>
<WatchWindow1>
<Ww>
<count>0</count>
@ -170,6 +187,16 @@
<WinNumber>1</WinNumber>
<ItemText>dr16</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>gimbal</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>GIMBAL_IMU_Update</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
@ -1257,6 +1284,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\device\gimbal_imu.c</PathWithFileName>
<FilenameWithoutPath>gimbal_imu.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -1267,7 +1306,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1279,7 +1318,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1291,7 +1330,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1301,6 +1340,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\module\gimbal.c</PathWithFileName>
<FilenameWithoutPath>gimbal.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -1311,7 +1362,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1323,7 +1374,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1335,7 +1386,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1347,7 +1398,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1359,7 +1410,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1371,7 +1422,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1383,7 +1434,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>93</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1395,7 +1446,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>94</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1407,7 +1458,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>95</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1419,7 +1470,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>94</FileNumber>
<FileNumber>96</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

10
MDK-ARM/CtrBoard-H7_ALL.uvprojx
View File

@ -824,6 +824,11 @@
<FileType>1</FileType>
<FilePath>..\User\device\motor_rm.c</FilePath>
</File>
<File>
<FileName>gimbal_imu.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\gimbal_imu.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -844,6 +849,11 @@
<FileType>1</FileType>
<FilePath>..\User\module\shoot.c</FilePath>
</File>
<File>
<FileName>gimbal.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\gimbal.c</FilePath>
</File>
</Files>
</Group>
<Group>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

12360
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

2
User/bsp/fdcan.c
View File

@ -27,7 +27,7 @@
#define FDCAN2_FILTER_CONFIG_TABLE(X) \
X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \
X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0)
#define FDCAN2_GLOBAL_FILTER FDCAN_REJECT, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
#define FDCAN2_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
#endif
#ifdef FDCAN3_EN
#define FDCAN3_FILTER_CONFIG_TABLE(X) \

46
User/device/gimbal_imu.c
View File

@ -8,6 +8,7 @@
#include <string.h>
#include "bsp/can.h"
#include "bsp/time.h"
/* Private function prototypes ---------------------------------------------- */
@ -34,9 +35,6 @@ static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_t *gimbal_imu, uint32_t id, const
return;
}
/* 更新在线状态 */
gimbal_imu->header.online = true;
/* 判断是哪个ID的数据 */
if (id == gimbal_imu->param.accl_id) {
/* 解析加速度计数据 */
@ -89,10 +87,15 @@ static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_t *gimbal_imu, uint32_t id, const
/**
* @brief 初始化Gimbal IMU设备
* @param gimbal_imu Gimbal IMU结构体指针
* @param param 参数配置
* @return 初始化结果
* - DEVICE_OK: 成功
* - DEVICE_ERR_NULL: 参数为空
*/
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
if (gimbal_imu == NULL || param == NULL) {
return -1;
return DEVICE_ERR_NULL;
}
/* 清空数据 */
@ -110,35 +113,56 @@ int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param
BSP_CAN_RegisterId(param->can, param->eulr_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
BSP_CAN_RegisterId(param->can, param->quat_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
return 0;
return DEVICE_OK;
}
/**
* @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
* @param gimbal_imu Gimbal IMU结构体指针
* @return 更新结果
* - DEVICE_OK: 成功
* - DEVICE_ERR_NULL: 参数为空
*/
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu) {
if (gimbal_imu == NULL) {
return -1;
return DEVICE_ERR_NULL;
}
BSP_CAN_Message_t msg;
bool received = false;
/* 读取所有可用的CAN消息 */
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
received = true;
}
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
received = true;
}
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
received = true;
}
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
received = true;
}
return 0;
/* 更新在线状态和时间戳 */
if (received) {
gimbal_imu->header.online = true;
gimbal_imu->header.last_online_time = BSP_TIME_Get();
} else {
/* 超时检测 - 100ms */
uint64_t now_time = BSP_TIME_Get();
if (now_time - gimbal_imu->header.last_online_time > 100000) { /* 100ms超时，单位微秒 */
gimbal_imu->header.online = false;
}
}
return DEVICE_OK;
}

13
User/device/gimbal_imu.h
View File

@ -65,14 +65,21 @@ typedef struct {
/**
* @brief 初始化Gimbal IMU设备
*
* @param gimbal_imu Gimbal IMU结构体指针
* @param param 参数配置
* @return int8_t 0:成功 -1:失败
* @return 初始化结果
* - DEVICE_OK: 成功
* - DEVICE_ERR_NULL: 参数为空
*/
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param);
/**
* @brief 更新Gimbal IMU设备状态
* @param gimbal_imu Gimbal IMU结构体指针
* @return 更新结果
* - DEVICE_OK: 成功
* - DEVICE_ERR_NULL: 参数为空
*/
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu);

86
User/module/config.c
View File

@ -20,9 +20,91 @@
*/
Config_RobotParam_t robot_config = {
/* USER CODE BEGIN robot_config */
.gimbal_param = {
.pid = {
.yaw_omega = {
.k = 1.0f,
.p = 1.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.yaw_angle = {
.k = 8.0f,
.p = 3.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
.pit_omega = {
.k = 0.25f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.pit_angle = {
.k = 5.0f,
.p = 5.0f,
.i = 2.5f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
},
.mech_zero = {
.yaw = 0.0f,
.pit = 1.77f,
},
.travel = {
.yaw = -1.0f,
.pit = 0.8f,
},
.low_pass_cutoff_freq = {
.out = -1.0f,
.gyro = 1000.0f,
},
.pit_motor ={
.can = BSP_CAN_1,
.id = 0x206,
.gear = false,
.module = MOTOR_GM6020,
.reverse = true,
},
.yaw_motor = {
.can = BSP_CAN_1,
.can_id = 0x1,
.master_id = 0x11,
.module = MOTOR_DM_J4310,
.reverse = false,
},
.imu = {
.can = BSP_CAN_2,
.accl_id = 0x64, // 100
.gyro_id = 0x65, // 101
.eulr_id = 0x66, // 102
.quat_id = 0x67 // 103
},
},
.shoot_param = {
.trig_step_angle=M_2PI/8,
.shot_delay_time=0.5f,
.shot_delay_time=1.0f,
.shot_burst_num=20,
.fric_motor_param[0] = {
.can = BSP_CAN_1,
@ -47,7 +129,7 @@ Config_RobotParam_t robot_config = {
},
.fric_follow = {
.k=1.0f,
.p=1.8f,
.p=1.5f,
.i=0.0f,
.d=0.0f,
.i_limit=0.0f,

2
User/module/config.h
View File

@ -12,6 +12,7 @@ extern "C" {
#include <stdbool.h>
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
/**
* @brief 机器人参数配置结构体
* @note 在此添加您的配置参数
@ -20,6 +21,7 @@ typedef struct {
/* USER CODE BEGIN Config_RobotParam */
Shoot_Params_t shoot_param;
Chassis_Params_t chassis_param;
Gimbal_Params_t gimbal_param;
/* USER CODE END Config_RobotParam */
} Config_RobotParam_t;

31
User/module/gimbal.c
View File

@ -9,6 +9,7 @@
#include <math.h>
#include "component/filter.h"
#include "component/pid.h"
#include "device/gimbal_imu.h"
#include "device/motor_dm.h"
#include "device/motor_rm.h"
@ -72,9 +73,12 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
if (g == NULL)
return -1;
g->param = param;
g->param = (Gimbal_Params_t *)param;
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
/* 先初始化CAN */
BSP_CAN_Init();
/* 初始化云台电机控制PID和LPF */
PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq,
&(g->param->pid.yaw_angle));
@ -93,12 +97,15 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
g->limit.yaw.min = g->param->mech_zero.yaw;
g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
g->limit.pit.min = g->param->mech_zero.pit;
BSP_CAN_Init();
MOTOR_RM_Register(&(g->param->pit_motor));
MOTOR_DM_Register(&(g->param->yaw_motor));
MOTOR_DM_Enable(&(g->param->yaw_motor));
/* 最后初始化IMU，确保CAN和电机都已经设置好 */
GIMBAL_IMU_Init(&g->imu, &(g->param->imu));
return 0;
}
@ -136,7 +143,14 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal){
if (gimbal == NULL) {
return -1;
}
GIMBAL_IMU_Update(&gimbal->imu);
/* 将IMU数据复制到feedback中供控制使用 */
gimbal->feedback.imu.eulr = gimbal->imu.data.eulr;
gimbal->feedback.imu.gyro = gimbal->imu.data.gyro;
gimbal->feedback.imu.accl = gimbal->imu.data.accl;
return 0;
}
/**
@ -208,6 +222,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
break;
case GIMBAL_MODE_ABSOLUTE:
case GIMBAL_MODE_RELATIVE:
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
@ -220,13 +235,13 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
break;
/* 输出滤波 */
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
return 0;
}
/* 输出滤波 */
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
return 0;
}
/**

7
User/module/gimbal.h
View File

@ -76,7 +76,11 @@ typedef struct {
/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
typedef struct {
GIMBAL_IMU_t imu;
struct {
AHRS_Eulr_t eulr; /* 云台姿态欧拉角 */
AHRS_Gyro_t gyro; /* 云台陀螺仪数据 */
AHRS_Accl_t accl; /* 云台加速度计数据 */
}imu;
struct {
MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
@ -100,6 +104,7 @@ typedef struct {
/* 模块通用 */
Gimbal_Mode_t mode; /* 云台模式 */
GIMBAL_IMU_t imu; /* 云台IMU数据 */
/* PID计算的目标值 */
struct {

2
User/module/shoot.c
View File

@ -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=5000.0f; /* 归一化目标转速 */
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */
return 0;
}

37
User/task/ctrl_gimbal.c
View File

@ -6,9 +6,10 @@
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "component/ahrs.h"
#include "module/gimbal.h"
#include "module/config.h"
#include "bsp/can.h"
#include "device/motor.h"
#include "device/motor_dm.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -16,13 +17,9 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
MOTOR_DM_Param_t motor_dm_yaw = {
.can = BSP_CAN_1,
.can_id = 0x1,
.master_id = 0x11,
.module = MOTOR_DM_J4310,
.reverse = false,
};
Gimbal_t gimbal;
Gimbal_CMD_t gimbal_cmd;
BSP_FDCAN_StdDataFrame_t can_frame;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -38,14 +35,30 @@ void Task_ctrl_gimbal(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BSP_CAN_Init();
MOTOR_DM_Register(&motor_dm_yaw);
Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
MOTOR_DM_Enable(&motor_dm_yaw);
can_frame.id = 0x200;
can_frame.dlc = 8;
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
Gimbal_UpdateIMU(&gimbal);
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
Gimbal_UpdateFeedback(&gimbal);
// osMessageQueueReset(task_runtime.msgq.chassis_yaw);
osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
Gimbal_Control(&gimbal, &gimbal_cmd);
Gimbal_Output(&gimbal);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

4
User/task/user_task.c
View File

@ -22,12 +22,12 @@ const osThreadAttr_t attr_atti_esit = {
const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
.stack_size = 256 * 8,
};
const osThreadAttr_t attr_ctrl_gimbal = {
.name = "ctrl_gimbal",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
.stack_size = 256 * 8,
};
const osThreadAttr_t attr_monitor = {
.name = "monitor",

BIN
utils/Simulation-master/balance/.DS_Store vendored
View File

Binary file not shown.