diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 4bfc4a7..2d1b665 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -60,7 +60,7 @@ float chassis_power = 0; static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 float P_cmdall = 0; -static uint8_t last_chassis_power_limit=0; +static uint8_t input=0; void ChassisInit() { @@ -119,7 +119,14 @@ void ChassisInit() .can_handle = &hcan1, .tx_id = 0x210, .rx_id = 0x211, - }}; + }, + .buffer_pid_config = { + .Kp = 0.5f, + .Ki = 0, + .Kd = 0, + .MaxOut = 500, + } + }; cap = SuperCapInit(&cap_conf); // 超级电容初始化 SuperCapSetPower(cap,70); // 超级电容限制功率 @@ -218,11 +225,16 @@ static void LimitChassisOutput() static void SuperCapSetUpdate() { - if(last_chassis_power_limit != chassis_cmd_recv.chassis_power_limit) - { - SuperCapSetPower(cap,chassis_cmd_recv.chassis_power_limit); // 超级电容限制功率 - last_chassis_power_limit = chassis_cmd_recv.chassis_power_limit; - } +// PIDCalculate(&cap->buffer_pid, chassis_cmd_recv.chassis_power_buffer,30); +// input = chassis_cmd_recv.chassis_power_limit - cap->buffer_pid.Output; +// SuperCapSetPower(cap,input); + + +// if(last_chassis_power_limit != chassis_cmd_recv.chassis_power_limit) +// { +// SuperCapSetPower(cap,chassis_cmd_recv.chassis_power_limit); // 超级电容限制功率 +// last_chassis_power_limit = chassis_cmd_recv.chassis_power_limit; +// } } /** * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算 diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index ae2be56..cbb3646 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -64,6 +64,9 @@ static referee_info_t* referee_data; // 用于获取裁判系统的数据 static referee_info_vt_t* referee_vt_data; static uint8_t Pitch_Limit_Flag=1; +float PitchMotorAngle; +float DeltaPitchAngle; + void RobotCMDInit() { rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 @@ -157,9 +160,8 @@ static void pitch_limit() // if(!Pitch_Limit_Flag) // { -// float Delta_total_angle;//pitch电机自水平位置转过的角度 -// Delta_total_angle = gimbal_fetch_data.pitch_motor_total_angle-Horizontal_Pitch_total_angle; -// PitchMotorAngle = -0.0019f * Delta_total_angle - 0.4059;//电机旋转导致的云台相对底盘角度 +// +// PitchMotorAngle = -0.0019f * gimbal_fetch_data.pitch_motor_total_angle - 0.4059;//电机旋转导致的云台相对底盘角度 // DeltaPitchAngle = gimbal_fetch_data.gimbal_imu_data.Pitch - PitchMotorAngle;//陀螺仪和电机角的误差 // gimbal_cmd_send.pitch = LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE-DeltaPitchAngle, PITCH_MAX_ANGLE-DeltaPitchAngle); // } diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c index a75d507..f726bbc 100644 --- a/modules/super_cap/super_cap.c +++ b/modules/super_cap/super_cap.c @@ -24,6 +24,7 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config) supercap_config->can_config.can_module_callback = SuperCapRxCallback; super_cap_instance->can_ins = CANRegister(&supercap_config->can_config); + PIDInit(&super_cap_instance->buffer_pid, &supercap_config->buffer_pid_config); return super_cap_instance; } diff --git a/modules/super_cap/super_cap.h b/modules/super_cap/super_cap.h index 171c851..ed1736b 100644 --- a/modules/super_cap/super_cap.h +++ b/modules/super_cap/super_cap.h @@ -2,6 +2,7 @@ #define SUPER_CAP_H #include "bsp_can.h" +#include "controller.h" #pragma pack(1) typedef struct @@ -18,12 +19,14 @@ typedef struct { CANInstance *can_ins; // CAN实例 SuperCap_Msg_s cap_msg; // 超级电容信息 + PIDInstance buffer_pid; //缓冲功率闭环控制参数 } SuperCapInstance; /* 超级电容初始化配置 */ typedef struct { CAN_Init_Config_s can_config; + PID_Init_Config_s buffer_pid_config; } SuperCap_Init_Config_s; /**