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-06-05 16:47:27 +08:00
parent 9567222df6
commit 5556d48921
6 changed files with 121 additions and 37 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.

40
User/Module/Chassis.c
View File

@ -48,6 +48,8 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); // x滤波
c->sick_cali .sickparam=c->param ->sickparam ;
return CHASSIS_OK;
}
@ -187,9 +189,41 @@ c->NUC_send .send [1]=1;
/*
sick0,左1
sick1,左2
sick2,下1
*/
//int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
//{
//// int32_t x_set;
//// int32_t y_set;
// if (c == NULL) return CHASSIS_ERR_NULL;
// if (ctrl == NULL) return CHASSIS_ERR_NULL;
//
// //yaw修正
// if(fabs((fp32)c->sick_cali .sick_dis [0]-c->sick_cali .sick_dis [1])>(fp32)c->sick_cali .sickparam.w_error){
// c->move_vec .Vw = PID_calc(&(c->pid .chassis_SickVxPID ),c->sick_cali .sick_dis [0],c->sick_cali .sick_dis [1]);
// }
// else { //大于误差
// if(fabs((fp32)c->sick_cali .sick_dis[0]-c->sick_cali .sick_dis[1])>c->sick_cali .sickparam .xy_error)
// {//xy修正
// c->move_vec .Vx =PID_calc (&(c->pid .chassis_SickVxPID ),c->sick_cali .sick_dis [0], c->sick_cali .sickparam.x_set);
// c->move_vec .Vy =PID_calc (&(c->pid .chassis_SickVyPID),c->sick_cali .sick_dis [2], c->sick_cali .sickparam.y_set);
// }
// else {//修正OK后，给nuc返回值
//
//
// }
//
//
// }
//
//}

22
User/Module/Chassis.h
View File

@ -58,7 +58,12 @@ typedef struct {
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
}ChassisImu_t;
typedef struct {
int32_t w_error;//角度纠正时的允许误差量
int32_t xy_error;//xy允许误差量
int32_t x_set;
int32_t y_set;
}sickparam_t;
/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
typedef struct
@ -74,10 +79,13 @@ typedef struct
pid_param_t NaviVx_param;
pid_param_t NaviVy_param;
pid_param_t NaviVw_param;
pid_param_t Sick_CaliWparam;
pid_param_t Sick_CaliYparam;
pid_param_t Sick_CaliXparam;
sickparam_t sickparam;
}Chassis_Param_t;
@ -148,9 +156,9 @@ typedef struct{
pid_type_def chassis_NaviWzPID;
pid_type_def chassis_NaviVxPID;
pid_type_def chassis_NaviVyPID;
pid_type_def chassis_SickWzPID;
pid_type_def chassis_SickVxPID;
pid_type_def chassis_SickVyPID;
pid_type_def sick_CaliforYPID;
pid_type_def sick_CaliforXPID;
@ -161,8 +169,10 @@ typedef struct{
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
int32_t sick_dis[4]; //获取到的sick激光值
struct {
int32_t sick_dis[3]; //获取到的sick激光值
sickparam_t sickparam;
}sick_cali;
NUC_send_t NUC_send;

36
User/Module/config.c
View File

@ -98,9 +98,9 @@ static const ConfigParam_t param ={
.DribbleConfig_Config = {
.dribble_flag=0,
.m3508_init_angle = -5,
.m3508_translate_angle = -930,
.m3508_translate_angle = 1000,
.m3508_dribble_Reverse_speed=-3500,
. m3508_dribble_speed= 4000, // 转动速度
.m3508_dribble_speed= 4000, // 转动速度
.m3508_dribble_init_speed=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
@ -136,20 +136,48 @@ static const ConfigParam_t param ={
.out_limit =6000.0f,
},
/*视觉*/
.chassis_PICKWzPID_HIGN_param ={ //高响应
.chassis_PICKWzPID_HIGN_param ={
.p = 1.0f,
.i = 0.03f,
.d = 0.03f,
.i_limit = 100.0f,
.out_limit =2000.0f,
},
.chassis_PICKWzPID_LOW_param ={ //高精度
.chassis_PICKWzPID_LOW_param ={
.p = 0.5f, //1.0 0.5
.i = 0.5f, //0.01 0.04
.d = 0.0f, //0.02 0.02
.i_limit = 50.0f,
.out_limit =1000.0f,
},
.Sick_CaliWparam ={
.p = 0.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =0.0f,
},
.Sick_CaliYparam ={
.p = 0.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =0.0f,
},
.Sick_CaliXparam ={
.p = 0.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =0.0f,
},
.sickparam={
.w_error=1000,
.xy_error=1000,
.x_set=1000,
.y_set=1000,
},
},

49
User/Module/up.c
View File

@ -296,17 +296,17 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break;
case Pitch :
if (u->PitchContext .PitchState ==PITCH_PREPARE)
{
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
}
Pitch_Process(u,out);
// if (u->PitchContext .PitchState ==PITCH_PREPARE)
// {
// u->CoContext .CoState =CoTRANSLATE_OUT;
// u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
// }
// Co_Process(u,out);
// Pitch_Process(u,out);
if (u->PitchContext .PitchState ==PITCH_PREPARE)
{
u->CoContext .CoState =CoTRANSLATE_OUT;
}
Co_Process(u,out);
break ;
case UP_Adjust: //测试用
@ -344,15 +344,20 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
}
/*根据距离实时计算所需pos*/
// u->PitchContext .PitchConfig .go1_release_threshold=c->pos;
Pitch_Process(u,out);
u->PitchContext .PitchConfig .go1_init_position=c->pos;
if(u->CoContext .CoState == CoPITCH){
u->CoContext.CoState=CoPITCH_MID360;
}
Co_Process(u,out);
// Pitch_Process(u,out);
break ;
case AUTO_MID360:
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->PitchContext .PitchState = PITCH_PREPARE;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE;
break ;
@ -382,7 +387,7 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
switch(u->PitchContext .PitchState){
case PITCH_START:
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
u->PitchContext .PitchConfig .pull_speed=10;
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
@ -407,7 +412,7 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
break ;
case PITCH_LAUNCHING: //等待发射
u->PitchContext .PitchState=PITCH_COMPLETE;
// u->PitchContext .PitchState=PITCH_COMPLETE;
break ;
case PITCH_COMPLETE: //发射完成
@ -528,7 +533,7 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out)
case CoTRANSLATE_OUT:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主
{
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态
u->PitchContext .PitchState = PITCH_START;
@ -539,7 +544,7 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out)
case CoDRIBBLE:
Dribble_Process(u,out); //状态停在DRIBBLE_DONE
Pitch_Process(u,out); //状态停在PITCH_PULL_TRIGGER
Pitch_Process(u,out); //状态停在PITCH_LAUNCHING
//状态停在对应位置时,平移去给发射送球
if(u->DribbleContext .DribbleState ==DRIBBLE_DONE && u->PitchContext.PitchState ==PITCH_LAUNCHING)
{
@ -548,8 +553,8 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out)
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_init_angle,1.0f))
{
u->motor_target.Dribble_M3508_speed[0]=1000;
u->motor_target.Dribble_M3508_speed[1]=-1000;
u->motor_target.Dribble_M3508_speed[2]=-1000;
u->motor_target.Dribble_M3508_speed[1]=1000;
u->motor_target.Dribble_M3508_speed[2]=1000;
u->CoContext .CoState =CoTRANSLATE_IN;
}
@ -576,17 +581,23 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out)
}
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主,卡到最右端
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主,卡到最右端
{
u->CoContext .CoState =CoPITCH;
}
break ;
case CoPITCH:
case CoPITCH_MID360:
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
break ;
break ;
}

3
User/Module/up.h
View File

@ -34,6 +34,7 @@ typedef enum {
CoDRIBBLE, //运球和蓄力阶段,再平移回去
CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
CoPITCH, //发射
CoPITCH_MID360, //雷达发射
CoDONE,
@ -63,7 +64,7 @@ typedef enum {
typedef struct {
fp32 m2006_init_angle; // M2006初始角度-140
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机
fp32 go1_init_position; // GO电机触发位置,0,初始位置
fp32 go1_init_position; // GO电机触发位置,初始位置，后续更改用于发射的位置
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
fp32 go1_Receive_ball; //用在配合过程，用来接平移后的球
fp32 m2006_Screw_init;