diff --git a/User/module/chassis.c b/User/module/chassis.c index 4e6d452..14c6f38 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -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.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); 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.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: // // 导航模式 diff --git a/User/module/cmd.c b/User/module/cmd.c index 6f61c84..acd3bee 100644 --- a/User/module/cmd.c +++ b/User/module/cmd.c @@ -106,16 +106,20 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) { } switch (ctx->input.rc.sw[1]) { 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; 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; case CMD_SW_DOWN: ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RELAX; + ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_AI; break; default: - + ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX; + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; break; } diff --git a/User/module/config.c b/User/module/config.c index a3b0aff..147e756 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -246,8 +246,8 @@ Config_Param_t config = { }, .pid.daohang_6020_motor_angle={ - .k = 1.0f, - .p = 2.0f, + .k = 2.0f, + .p = 2.5f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, @@ -258,10 +258,10 @@ Config_Param_t config = { .pid.daohang_4310_motor_angle={ .k = 2.0f, .p = 5.0f, - .i = 0.0f, - .d = 1.0f, + .i = 1.0f, + .d = 0.2f, .i_limit = 0.0f, - .out_limit = 2.0f, + .out_limit = 3.0f, .d_cutoff_freq = 20.0f, .range = M_2PI }, @@ -408,6 +408,7 @@ Config_Param_t config = { }, }; +//////////// /** * @brief 获取机器人配置参数 diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 4e6ee7a..6ad29d7 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -65,8 +65,8 @@ static double poly(double x) { static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { if (g == NULL) return -1; - if (mode == g->mode) - return GIMBAL_OK; + if (mode == g->mode){ + return GIMBAL_OK;} PID_Reset(&g->pid.yaw_4310_angle); 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.eulr = imu->eulr; + gimbal->feedback.imu.eulr.yaw = imu->eulr.yaw; gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换 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: g->out.yaw_4310 = 0.0f; g->out.pitch_4310 = 0.0f; @@ -322,19 +341,7 @@ switch (g->mode) { /*绝对坐标 大地坐标系下云台控制*/ /*6020小YAW控制*/ // 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, 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->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); - - - break; /*云台相对底盘控制，暂时没写，后面可以更进*/ case GIMBAL_MODE_RELATIVE: @@ -372,20 +376,23 @@ g->setpoint.pit_accl = g_cmd->pit_accl; case GIMBAL_MODE_DAOHANG: /*导航模式，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->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->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个人觉得不是很需要滤波，可以自己加*/ // 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.yaw_4310 = LowPassFilter2p_Apply(&g->filter_out.yaw_4310, g->out.yaw_4310); + // 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.yaw_4310 = LowPassFilter2p_Apply(&g->filter_out.yaw_4310, g->out.yaw_4310); -} + + return 0; } diff --git a/User/module/gimbal.h b/User/module/gimbal.h index b853eff..2cfc6b9 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -116,8 +116,6 @@ extern "C" { GIMBAL_MODE_REMOTE, GIMBAL_MODE_AI, - - }GIMBAL_Ctrl_mode_t; typedef struct { diff --git a/User/task/chassis.c b/User/task/chassis.c index ca321f0..26394cd 100644 --- a/User/task/chassis.c +++ b/User/task/chassis.c @@ -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_Control(&chassis, &safe_cmd,tick); } - Chassis_Setoutput(&chassis); + // Chassis_Setoutput(&chassis); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/super_cap.c b/User/task/super_cap.c index 1ea281b..64505f3 100644 --- a/User/task/super_cap.c +++ b/User/task/super_cap.c @@ -38,33 +38,33 @@ void Task_super_cap(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - SuperCap_Init(); - SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能 - SuperCap_CanTX.Charge = 0 ; //此标志位无效，超电的充放电是自动的 - SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制 - SuperCap_CanTX.ChargePower = 1 ; //此参数无效，超电的充电功率随着底盘功率变化 + // SuperCap_Init(); + // SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能 + // SuperCap_CanTX.Charge = 0 ; //此标志位无效，超电的充放电是自动的 + // SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制 + // SuperCap_CanTX.ChargePower = 1 ; //此参数无效，超电的充电功率随着底盘功率变化 /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - osMessageQueueGet(task_runtime.msgq.referee.cap, &SuperCap_CanTX.Powerlimit , NULL, 0); - SuperCap_Update(); - CAN_TX_SuperCapData(&SuperCap_CanTX); + // osMessageQueueGet(task_runtime.msgq.referee.cap, &SuperCap_CanTX.Powerlimit , NULL, 0); + // SuperCap_Update(); + // CAN_TX_SuperCapData(&SuperCap_CanTX); /* 锁住RTOS内核防止控制过程中断，造成错误 */ - osKernelLock(); + // osKernelLock(); /* 根据裁判系统数据计算输出功率 */ - Cap_Control(&CAN_SuperCapRXData, &referee_cap, &power_limit); - osKernelUnlock(); + // Cap_Control(&CAN_SuperCapRXData, &referee_cap, &power_limit); + // osKernelUnlock(); - osMessageQueueReset(task_runtime.msgq.cap.for_chassis); - osMessageQueuePut(task_runtime.msgq.cap.for_chassis, &CAN_SuperCapRXData, 0, 0); - osMessageQueueReset(task_runtime.msgq.cap.power_limit); - osMessageQueuePut(task_runtime.msgq.cap.power_limit, &power_limit, 0, 0); - /* 超电UI */ - Cap_DumpUI(&CAN_SuperCapRXData,&cap_ui); - osMessageQueueReset(task_runtime.msgq.ui.cap); - osMessageQueuePut(task_runtime.msgq.ui.cap, &cap_ui, 0, 0); + // osMessageQueueReset(task_runtime.msgq.cap.for_chassis); + // osMessageQueuePut(task_runtime.msgq.cap.for_chassis, &CAN_SuperCapRXData, 0, 0); + // osMessageQueueReset(task_runtime.msgq.cap.power_limit); + // osMessageQueuePut(task_runtime.msgq.cap.power_limit, &power_limit, 0, 0); + // /* 超电UI */ + // Cap_DumpUI(&CAN_SuperCapRXData,&cap_ui); + // osMessageQueueReset(task_runtime.msgq.ui.cap); + // osMessageQueuePut(task_runtime.msgq.ui.cap, &cap_ui, 0, 0); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */