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

Compare commits

base: robofish:main
Branches Tags
robofish:main
robofish:xm
..
compare: robofish:xm
Branches Tags
robofish:main
robofish:xm

No commits in common. "main" and "xm" have entirely different histories.
main ... xm

3 changed files with 15 additions and 26 deletions
Show Stats Expand all files Collapse all files

20
User/module/gimbal.c
View File

@ -45,7 +45,6 @@ Gimbal_Output(&gimbal);
/*
*拟合函数，用于对pitch的重力补偿
*/
static double poly(double x) {
return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984;
@ -399,10 +398,10 @@ switch (g->mode)
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
// g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
//g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt);
g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
// g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
/*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/
pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt);
@ -464,7 +463,7 @@ void Gimbal_Output(Gimbal_t *g)
// output_yaw_4310.kd=0.1f;
MOTOR_MIT_Output_t output_pitch_4310 = {0};
output_pitch_4310.torque = g->out.pitch_4310 * 1.0f+1.5;//1.5是重力补偿
output_pitch_4310.torque = g->out.pitch_4310 * 1.0f;
output_pitch_4310.kd=0.2f;
MOTOR_RM_Ctrl(&g->param->yaw_6020_motor);
@ -482,14 +481,7 @@ void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui) {
ui->mode = g->mode;
}
/* @brief 导出云台UI数据
*
* @param g 云台结构体
* @param ui UI结构体
*/
void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui) {
ui->mode = g->mode;
}

6
User/module/gimbal.h
View File

@ -16,7 +16,7 @@ extern "C"
#define GIMBAL_ERR_NULL -2
#define GIMBAL_ERR_MODE (-3)
#define GIMBAL_ERR_TYPE (-4)
#define AI_GIMBAL_SEARCH (1u << 1) // bit1: 云台搜索模式
@ -201,10 +201,6 @@ typedef struct {
} Gimbal_t;
typedef struct {
Gimbal_Mode_t mode;
} Gimbal_RefereeUI_t;
/* Exported functions prototypes -------------------------------------------- */
/**

15
User/task/gimbal.c
View File

@ -22,7 +22,7 @@ Gimbal_t gimbal;
AI_result_t ai_gimbal_result_cmd; /*ai发送自瞄数据和 导航数据*/
Gimbal_CMD_t final_gimbal_cmd; //最终命令
Gimbal_RefereeUI_t gimbal_ui;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -46,7 +46,8 @@ void Task_gimbal(void *argument) {
/* USER CODE BEGIN */
osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0);
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_result_cmd,NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_result_cmd,NULL, 0);
/* ai指令 */
// if(osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd, &ai_gimbal_result_cmd, NULL, 0)==osOK){
// if(ai_gimbal_result_cmd.mode==0){
@ -58,14 +59,16 @@ void Task_gimbal(void *argument) {
// final_gimbal_cmd.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
// }
// }
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
if (ai_gimbal_result_cmd.mode == 0) {
/* AI无目标 */
cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
/* 烧苗模式标志位判断 */
if (ai_gimbal_result_cmd.reserved & AI_GIMBAL_SEARCH ) {
if (cmd_gimbal.scan_enable) {
cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN; /* 有扫描：回退到扫描模式 */
} else {
cmd_gimbal.ctrl_mode = GIMBAL_MODE_REMOTE; /* 无扫描：保持当前姿态不动 */
@ -85,8 +88,6 @@ if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) {
Gimbal_UpdateFeedback(&gimbal);
osMessageQueueReset(task_runtime.msgq.gimbal.yaw4310);
/* 底盘跟随统一使用大YAW反馈，避免跟随锁到小YAW */
osMessageQueuePut(task_runtime.msgq.gimbal.yaw4310, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0);
@ -98,7 +99,7 @@ osMessageQueuePut(task_runtime.msgq.gimbal.ai.feedback, &gimbal.feedback, 0, 0);
Gimbal_Control(&gimbal, &cmd_gimbal);
Gimbal_Output(&gimbal);
Gimbal_DumpUI(&gimbal,&gimbal_ui);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */