xiaocheng/Er_sentry
1
1
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

待添加超电

Browse Source
This commit is contained in:
Xiaocheng 2026-03-15 20:17:44 +08:00
parent 9ebf54e0b2
commit bdb9de8b0b
7 changed files with 67 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
User/module/chassis.c
View File

@ -405,7 +405,7 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
// 跟随云台模式 // 跟随云台模式
c->move_vec.Vx =-c_cmd->y_l; c->move_vec.Vx =-c_cmd->y_l;
c->move_vec.Vy =-c_cmd->x_l; c->move_vec.Vy =-c_cmd->x_l;
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt); c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt); // c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
break; break;
@ -415,7 +415,7 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
{ {
c->move_vec.Vx = -c_cmd->Vx/1; c->move_vec.Vx = -c_cmd->Vx/1;
c->move_vec.Vy = c_cmd->Vy/1; c->move_vec.Vy = c_cmd->Vy/1;
c->move_vec.Vw =PID_Calc(&c->pid.daohang_chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt); c->move_vec.Vw =PID_Calc(&c->pid.daohang_chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
} }
// case CHASSIS_MODE_DAOHANG: // case CHASSIS_MODE_DAOHANG:
// // 导航模式 // // 导航模式

10
User/module/cmd.c
View File

@ -106,16 +106,20 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
} }
switch (ctx->input.rc.sw[1]) { switch (ctx->input.rc.sw[1]) {
case CMD_SW_UP: case CMD_SW_UP:
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RC; ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RC;
ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_REMOTE;
break; break;
case CMD_SW_MID: case CMD_SW_MID:
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_DAOHANG; ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_DAOHANG;
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_DAOHANG;
break; break;
case CMD_SW_DOWN: case CMD_SW_DOWN:
ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RELAX; ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RELAX;
ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_AI;
break; break;
default: default:
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
break; break;
} }

11
User/module/config.c
View File

@ -246,8 +246,8 @@ Config_Param_t config = {
}, },
.pid.daohang_6020_motor_angle={ .pid.daohang_6020_motor_angle={
.k = 1.0f, .k = 2.0f,
.p = 2.0f, .p = 2.5f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 1.0f, .i_limit = 1.0f,
@ -258,10 +258,10 @@ Config_Param_t config = {
.pid.daohang_4310_motor_angle={ .pid.daohang_4310_motor_angle={
.k = 2.0f, .k = 2.0f,
.p = 5.0f, .p = 5.0f,
.i = 0.0f, .i = 1.0f,
.d = 1.0f, .d = 0.2f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 2.0f, .out_limit = 3.0f,
.d_cutoff_freq = 20.0f, .d_cutoff_freq = 20.0f,
.range = M_2PI .range = M_2PI
}, },
@ -408,6 +408,7 @@ Config_Param_t config = {
}, },
}; };
////////////
/** /**
* @brief 获取机器人配置参数 * @brief 获取机器人配置参数

57
User/module/gimbal.c
View File

@ -65,8 +65,8 @@ static double poly(double x) {
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
if (g == NULL) if (g == NULL)
return -1; return -1;
if (mode == g->mode) if (mode == g->mode){
return GIMBAL_OK; return GIMBAL_OK;}
PID_Reset(&g->pid.yaw_4310_angle); PID_Reset(&g->pid.yaw_4310_angle);
PID_Reset(&g->pid.yaw_4310_omega); PID_Reset(&g->pid.yaw_4310_omega);
@ -194,7 +194,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
} }
gimbal->feedback.imu.gyro = imu->gyro; gimbal->feedback.imu.gyro = imu->gyro;
gimbal->feedback.imu.eulr = imu->eulr; gimbal->feedback.imu.eulr.yaw = imu->eulr.yaw;
gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换 gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换
gimbal->feedback.imu.quat = imu->quat; gimbal->feedback.imu.quat = imu->quat;
@ -312,7 +312,26 @@ if(g->param->travel.pitch_4310 > 0) // 有限位才处理
/* 控制相关逻辑 */ /* 控制相关逻辑 */
switch (g->mode) { switch (g_cmd->ctrl_mode) {
case GIMBAL_MODE_REMOTE:
g->setpoint.eulr.yaw = g->setpoint.eulr.yaw;
g->setpoint.eulr.pit = g->setpoint.eulr.pit;
break;
case GIMBAL_MODE_AI:
g->setpoint.eulr.yaw = g_cmd->set_yaw;
g->setpoint.eulr.pit = g_cmd->set_pitch;
g->setpoint.yaw_vel = g_cmd->yaw_vel;
g->setpoint.yaw_accl = g_cmd->yaw_accl;
g->setpoint.pit_vel = g_cmd->pit_vel;
g->setpoint.pit_accl = g_cmd->pit_accl;
break;
}
switch (g->mode)
{
case GIMBAL_MODE_RELAX: case GIMBAL_MODE_RELAX:
g->out.yaw_4310 = 0.0f; g->out.yaw_4310 = 0.0f;
g->out.pitch_4310 = 0.0f; g->out.pitch_4310 = 0.0f;
@ -322,19 +341,7 @@ switch (g->mode) {
/*绝对坐标 大地坐标系下云台控制*/ /*绝对坐标 大地坐标系下云台控制*/
/*6020小YAW控制*/ /*6020小YAW控制*/
// Gimbal_Control_mode(g, g_cmd); // Gimbal_Control_mode(g, g_cmd);
if(g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE){
g->setpoint.eulr.yaw = g->setpoint.eulr.yaw;
g->setpoint.eulr.pit = g->setpoint.eulr.pit;
}
else if (g_cmd->ctrl_mode == GIMBAL_MODE_AI) {
g->setpoint.eulr.yaw = g_cmd->set_yaw;
g->setpoint.eulr.pit = g_cmd->set_pitch;
g->setpoint.yaw_vel = g_cmd->yaw_vel;
g->setpoint.yaw_accl = g_cmd->yaw_accl;
g->setpoint.pit_vel = g_cmd->pit_vel;
g->setpoint.pit_accl = g_cmd->pit_accl;
}
yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw, yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw,0.0f,g->dt); g->feedback.imu.eulr.yaw,0.0f,g->dt);
@ -358,9 +365,6 @@ g->setpoint.pit_accl = g_cmd->pit_accl;
// g->out.pitch_4310 = PID_Calc(&g->pid.pitch_4310_omega,8*pitch_omega_set_point, // g->out.pitch_4310 = PID_Calc(&g->pid.pitch_4310_omega,8*pitch_omega_set_point,
// -g->feedback.imu.gyro.y,0.0f,g->dt)+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle); // -g->feedback.imu.gyro.y,0.0f,g->dt)+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
// g->out.pitch_4310 = pitch_omega_set_point+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle); // g->out.pitch_4310 = pitch_omega_set_point+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
break; break;
/*云台相对底盘控制，暂时没写，后面可以更进*/ /*云台相对底盘控制，暂时没写，后面可以更进*/
case GIMBAL_MODE_RELATIVE: case GIMBAL_MODE_RELATIVE:
@ -372,20 +376,23 @@ g->setpoint.pit_accl = g_cmd->pit_accl;
case GIMBAL_MODE_DAOHANG: case GIMBAL_MODE_DAOHANG:
/*导航模式，NUC直接给出目标角度，云台控制环跟随即可*/ /*导航模式，NUC直接给出目标角度，云台控制环跟随即可*/
g->setpoint.eulr.yaw = g->param->mech_zero.yaw_6020+g->param->travel.yaw_6020*0.5f; // 这里可以直接设置为NUC传来的目标角度 g->setpoint.eulr.yaw = g->param->mech_zero.yaw_6020+g->param->travel.yaw_6020*0.5f; // 这里可以直接设置为NUC传来的目标角度
g->setpoint.yaw_4310 = 1.44215584f; g->setpoint.yaw_4310 = 4.51735544f;
g->out.yaw_6020 = PID_Calc(&g->pid.daohang_6020_angle,g->setpoint.eulr.yaw, g->out.yaw_6020 = PID_Calc(&g->pid.daohang_6020_angle,g->setpoint.eulr.yaw,
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,0.0f,g->dt); g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,0.0f,g->dt);
g->out.yaw_4310 = PID_Calc(&g->pid.daohang_4310_angle,g->setpoint.yaw_4310, g->out.yaw_4310 = PID_Calc(&g->pid.daohang_4310_angle,g->setpoint.yaw_4310,
g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt); g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,0.0f,g->dt);
break;
}
/* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/ /* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/
// g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020); // g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020); // g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
g->out.pitch_4310 = LowPassFilter2p_Apply(&g->filter_out.pitch_4310, g->out.pitch_4310); // g->out.pitch_4310 = LowPassFilter2p_Apply(&g->filter_out.pitch_4310, g->out.pitch_4310);
g->out.yaw_4310 = LowPassFilter2p_Apply(&g->filter_out.yaw_4310, g->out.yaw_4310); // g->out.yaw_4310 = LowPassFilter2p_Apply(&g->filter_out.yaw_4310, g->out.yaw_4310);
}
return 0; return 0;
} }

2
User/module/gimbal.h
View File

@ -116,8 +116,6 @@ extern "C"
{ {
GIMBAL_MODE_REMOTE, GIMBAL_MODE_REMOTE,
GIMBAL_MODE_AI, GIMBAL_MODE_AI,
}GIMBAL_Ctrl_mode_t; }GIMBAL_Ctrl_mode_t;
typedef struct { typedef struct {

2
User/task/chassis.c
View File

@ -66,7 +66,7 @@ osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0);
Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0}; Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
Chassis_Control(&chassis, &safe_cmd,tick); Chassis_Control(&chassis, &safe_cmd,tick);
} }
Chassis_Setoutput(&chassis); // Chassis_Setoutput(&chassis);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }

38
User/task/super_cap.c
View File

@ -38,33 +38,33 @@ void Task_super_cap(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
SuperCap_Init(); // SuperCap_Init();
SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能 // SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能
SuperCap_CanTX.Charge = 0 ; //此标志位无效，超电的充放电是自动的 // SuperCap_CanTX.Charge = 0 ; //此标志位无效，超电的充放电是自动的
SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制 // SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制
SuperCap_CanTX.ChargePower = 1 ; //此参数无效，超电的充电功率随着底盘功率变化 // SuperCap_CanTX.ChargePower = 1 ; //此参数无效，超电的充电功率随着底盘功率变化
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
osMessageQueueGet(task_runtime.msgq.referee.cap, &SuperCap_CanTX.Powerlimit , NULL, 0); // osMessageQueueGet(task_runtime.msgq.referee.cap, &SuperCap_CanTX.Powerlimit , NULL, 0);
SuperCap_Update(); // SuperCap_Update();
CAN_TX_SuperCapData(&SuperCap_CanTX); // CAN_TX_SuperCapData(&SuperCap_CanTX);
/* 锁住RTOS内核防止控制过程中断，造成错误 */ /* 锁住RTOS内核防止控制过程中断，造成错误 */
osKernelLock(); // osKernelLock();
/* 根据裁判系统数据计算输出功率 */ /* 根据裁判系统数据计算输出功率 */
Cap_Control(&CAN_SuperCapRXData, &referee_cap, &power_limit); // Cap_Control(&CAN_SuperCapRXData, &referee_cap, &power_limit);
osKernelUnlock(); // osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.cap.for_chassis); // osMessageQueueReset(task_runtime.msgq.cap.for_chassis);
osMessageQueuePut(task_runtime.msgq.cap.for_chassis, &CAN_SuperCapRXData, 0, 0); // osMessageQueuePut(task_runtime.msgq.cap.for_chassis, &CAN_SuperCapRXData, 0, 0);
osMessageQueueReset(task_runtime.msgq.cap.power_limit); // osMessageQueueReset(task_runtime.msgq.cap.power_limit);
osMessageQueuePut(task_runtime.msgq.cap.power_limit, &power_limit, 0, 0); // osMessageQueuePut(task_runtime.msgq.cap.power_limit, &power_limit, 0, 0);
/* 超电UI */ // /* 超电UI */
Cap_DumpUI(&CAN_SuperCapRXData,&cap_ui); // Cap_DumpUI(&CAN_SuperCapRXData,&cap_ui);
osMessageQueueReset(task_runtime.msgq.ui.cap); // osMessageQueueReset(task_runtime.msgq.ui.cap);
osMessageQueuePut(task_runtime.msgq.ui.cap, &cap_ui, 0, 0); // osMessageQueuePut(task_runtime.msgq.ui.cap, &cap_ui, 0, 0);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */