LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

投篮赛完赛

Browse Source
This commit is contained in:
ZHAISHUI04 2025-07-13 17:55:54 +08:00
parent bea23c3860
commit 2a15ca25ed
7 changed files with 55 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

15
User/Algorithm/user_math.c
View File

@ -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);
}

1
User/Algorithm/user_math.h
View File

@ -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

26
User/Module/Chassis.c
View File

@ -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:

8
User/Module/config.c
View File

@ -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电机下降速度

12
User/Module/up.c
View File

@ -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 {

12
User/device/cmd.c
View File

@ -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; //左下，右上，无模式
}