diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 7201427..09b1cd1 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -341,7 +341,7 @@ static void EmergencyHandler() { robot_state = ROBOT_STOP; gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - gimbal_cmd_send.yaw = 0; + gimbal_cmd_send.yaw = -gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;;//0 gimbal_cmd_send.pitch = 0; chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; shoot_cmd_send.shoot_mode = SHOOT_OFF; diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index dd8cccc..83e532d 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -78,10 +78,10 @@ void GimbalInit() .MaxOut = 1000, }, .speed_PID = { - .Kp = 80, // 50 + .Kp = 70, // 50 .Ki = 15, // 350 .Kd = 0.6, // 0 - .MaxOut = 30000,//20000 + .MaxOut = 20000,//20000 }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 41a6ab9..2cdee42 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -68,35 +68,32 @@ void ShootInit() .controller_param_init_config = { .angle_PID = { // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 - .Kp = 50, - .Ki = 0, - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ErrorHandle, - .IntegralLimit = 10000, - .MaxOut = 15000, - .Derivative_LPF_RC = 0.01, + .Kp = 12, // 10 + .Ki = 5, + .Kd = 0, + .MaxOut = 10000, }, .speed_PID = { - .Kp = 5, // 10 - .Ki = 100, // 1 - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 10000, - .MaxOut = 15000, + .Kp = 3, // 10 + .Ki = 10, // 1 + .Kd = 0, + .Improve = PID_Integral_Limit, + .IntegralLimit = 250, + .MaxOut = 10000, }, .current_PID = { - .Kp = 0.7, // 0.7 - .Ki = 0, // 0.1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 10000, - .MaxOut = 15000, + .Kp = 1.5, // 0.7 + .Ki = 0, // 0.1 + .Kd = 0, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 10000, }, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 - .close_loop_type = CURRENT_LOOP | SPEED_LOOP| ANGLE_LOOP, + .close_loop_type = ANGLE_LOOP | SPEED_LOOP|CURRENT_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向 }, .motor_type = M2006 // 英雄使用m3508 @@ -141,17 +138,36 @@ void ShootTask() DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 break; // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) - case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. - DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 - static float last_target_angle = 0; // 存储上次的目标角度 - if (dead_time == 0) { // 第一次触发 - last_target_angle = loader->measure.total_angle; - } - last_target_angle += ONE_BULLET_DELTA_ANGLE * REDUCTION_RATIO_LOADER; // 每次都在上次目标角度基础上增加固定角度 - DJIMotorSetRef(loader, last_target_angle); - hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 - dead_time = shoot_cmd_recv.dead_time; // 完成1发弹丸发射的时间 - break; + // case LOAD_1_BULLET: + // DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 + // static float last_target_angle = 0; // 存储上次的目标角度 + // if (dead_time == 0) { // 第一次触发 + // last_target_angle = loader->measure.total_angle; + // } + // last_target_angle += ONE_BULLET_DELTA_ANGLE * REDUCTION_RATIO_LOADER; // 每次都在上次目标角度基础上增加固定角度 + // DJIMotorSetRef(loader, last_target_angle); + // hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + // dead_time = shoot_cmd_recv.dead_time; // 完成1发弹丸发射的时间 + // break; + // case LOAD_1_BULLET: + // DJIMotorOuterLoop(loader, ANGLE_LOOP);// 切换到角度环 + // DJIMotorSetRef(loader, loader->measure.total_angle+51*36); + // //DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); + // // 控制量增加一发弹丸的角度，其中90为36减速比*1.5 + // hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + // dead_time = 1000; // 完成1发弹丸发射的时间 + // // 记录触发指令的时间 + // //dead_time = 500; + // // 完成1发弹丸发射的时间 + // break; + case LOAD_1_BULLET: + DJIMotorOuterLoop(loader, ANGLE_LOOP);// 切换到角度环 + static float target_angle = 0; + target_angle += 51*36; + DJIMotorSetRef(loader, target_angle); + hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + dead_time = 450; // 完成1发弹丸发射的时间 + break; // 三连发,如果不需要后续可能删除 case LOAD_3_BULLET: DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环 @@ -183,20 +199,20 @@ void ShootTask() switch (shoot_cmd_recv.bullet_speed) { case SMALL_AMU_15: - DJIMotorSetRef(friction_l, 58000); - DJIMotorSetRef(friction_r, 58000); + DJIMotorSetRef(friction_l, 30000); + DJIMotorSetRef(friction_r, 30000); break; case SMALL_AMU_18: - DJIMotorSetRef(friction_l, 58000); - DJIMotorSetRef(friction_r, 58000); + DJIMotorSetRef(friction_l, 30000); + DJIMotorSetRef(friction_r, 30000); break; case SMALL_AMU_30: - DJIMotorSetRef(friction_l, 58000); - DJIMotorSetRef(friction_r, 58000); + DJIMotorSetRef(friction_l, 50000); + DJIMotorSetRef(friction_r, 50000); break; default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 58000); - DJIMotorSetRef(friction_r, 58000); + DJIMotorSetRef(friction_l, 30000); + DJIMotorSetRef(friction_r, 30000); break; } } diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index ec084b6..28c0c0e 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -135,7 +135,6 @@ void INS_Task(void) INS.Gyro[X] = BMI088.Gyro[X]; INS.Gyro[Y] = BMI088.Gyro[Y]; INS.Gyro[Z] = BMI088.Gyro[Z]; - // demo function,用于修正安装误差,可以不管,本demo暂时没用 IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);