diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index d886012..f1b3416 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c index b0eb3ad..b16a7d3 100644 --- a/User/Algorithm/user_math.c +++ b/User/Algorithm/user_math.c @@ -405,4 +405,19 @@ bool is_reached_multiple(float current1, float current2, float current3, float t count++; // 所有条件都满足,计数加 1 } return false; // 未满足条件达到阈值,返回 0 +} + /** + * @brief 底盘无头模式转换函数 + * @param vx 输入 X 方向速度(全局坐标系),输出转换后的底盘 X 速度 + * @param vy 输入 Y 方向速度(全局坐标系),输出转换后的底盘 Y 速度 + * @param vw 旋转速度(直接控制,不受影响) + * @param yaw 当前陀螺仪 yaw 角度(弧度制) + */ +void ChassisHeadlessMode(double *vx, double *vy, double yaw) { + double vx_global = *vx; + double vy_global = *vy; + + // 标准二维旋转变换(逆时针为正) + *vx = vx_global * cosf(yaw) - vy_global * sinf(yaw); + *vy = vx_global * sinf(yaw) + vy_global * cosf(yaw); } \ No newline at end of file diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h index 0dad6dd..7464dd8 100644 --- a/User/Algorithm/user_math.h +++ b/User/Algorithm/user_math.h @@ -165,5 +165,6 @@ void normalize_vector(double x, double y, double *out_x, double *out_y) ; bool is_reached(float current, float target, float mistake) ; bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ; fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max); +void ChassisHeadlessMode(double *vx, double *vy, double yaw) ; #endif diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index 40c1642..ef04f52 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -81,10 +81,22 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre return CHASSIS_OK; } +//void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) { +// fp64 Nor_Vx, Nor_Vy; +// normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); +// +// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前 +// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后 +// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后 +// c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前 +// +// Chassis_AngleCompensate(c); + +//} void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) { fp64 Nor_Vx, Nor_Vy; normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); - + ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw); c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前 c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后 c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后 @@ -150,13 +162,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { case AUTO: switch (c->chassis_ctrl.mode) { case AUTO_MID360: - - c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000; - abs_limit_fp(&c->move_vec.Vw, 5000.0f); + c->move_vec.Vw = ctrl->Vw * 8000; + c->move_vec.Vx = ctrl->Vy * 8000; + c->move_vec.Vy = ctrl->Vx * 8000; +// c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000; +// abs_limit_fp(&c->move_vec.Vw, 5000.0f); break; case AUTO_MID360_Pitch: - c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000; + c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000; + c->move_vec.Vx = ctrl->Vy * 8000; + c->move_vec.Vy = ctrl->Vx * 8000; abs_limit_fp(&c->move_vec.Vw, 5000.0f); break; case REPOSITION: diff --git a/User/Module/config.c b/User/Module/config.c index bef7c04..b87c488 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -80,7 +80,7 @@ static const ConfigParam_t param ={ .go_pull_pos = -210.0f, // go上升去合并扳机,扳机位置 .go_up_speed = 15.0f, // 上升速度 .go_down_speed = 12.0f, // 下降速度 - .Pitch_angle = 58, //俯仰角 + .Pitch_angle = 66, //俯仰角 .go_init = -50 }, .PitchCfg = { @@ -88,20 +88,20 @@ static const ConfigParam_t param ={ .go_init = -50, //仅用在go上电,初始位置 .go_release_pos=-200, .Pitch_angle =66, - .go_up_speed =12, + .go_up_speed =15, .go_down_speed =10, }, .PassCfg={ .go_wait =-10, .go_release_pos =-150, .Pitch_angle=66, - .go_up_speed =12, + .go_up_speed =15, .go_down_speed =10, }, .MID360Cfg={ .go_release_pos=-200, // GO电机发射位置 - .go_up_speed=20, // GO电机上升速度 + .go_up_speed=15, // GO电机上升速度 .go_down_speed=10, // GO电机下降速度 diff --git a/User/Module/up.c b/User/Module/up.c index e2561c7..7dccb07 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -223,7 +223,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) break ; case UP_Adjust: //测试用,手动使用pitch下的cfg - LaunchCfg->go_up_speed=5; + pitch_cfg ->go_release_pos += c->Vx ; LaunchCfg->Pitch_angle += c->Vy/100; @@ -242,7 +242,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) break; case RELAXED: // target->go_shoot= pitch_cfg . - target->Pitch_angle = 58; + target->Pitch_angle = 66; target->go_shoot = -190.0f; //失控下,最好切手动,手动用pitch pitch_cfg ->go_release_pos=-190; @@ -420,15 +420,19 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){ MID360Context_t *MID360Context=&u->MID360Context; MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg; + u->PitchContext.PitchConfig.go_release_pos =MID360Cfg->go_release_pos; /*俯仰角度,力度转换,加数值限位*/ - + if(c->cmd_MID360.dis<1.3f){ + MID360Cfg->go_release_pos=-150.0f; + }else { if(u->MID360Context.Curve==CURVE_58){ - MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.25f,3.3,4.3,&u->MID360Context.Curve); + MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.20f,3.3,4.3,&u->MID360Context.Curve); } else { MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.1f,3.3,4.3,&u->MID360Context.Curve); } + } if (u->MID360Context.Curve == CURVE_58) { target->Pitch_angle = 58; } else { diff --git a/User/device/cmd.c b/User/device/cmd.c index 28ba857..cae197e 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -137,7 +137,7 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) { if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上,右中,无模式 - if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust_Sick; //左上,右上,手动调整 + if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上,右上, if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust; } @@ -150,15 +150,15 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) { if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =AUTO_MID360; //左中,右中,无模式 - if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =REPOSITION; //左中,右下,视觉 + if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中,右下,视觉 } else if(rc->dr16.sw_l==CMD_SW_DOWN) { - cmd ->CMD_CtrlType =PASS_BALL; - if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =PB_UP; //左下,右上,无模式 - if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = PB_MID; //左下,右中,无模式 - if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =PB_DOWN; //左下,右上,无模式 + cmd ->CMD_CtrlType =RCcontrol; + if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下,右上,无模式 + if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下,右中,无模式 + if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下,右上,无模式 }