diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 8334e56..f383ca8 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index 5abd5c0..a49c187 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -47,6 +47,8 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波 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返回值 +// +// +// } +// +// +// } +// +//} diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index bdc6868..8e153bb 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -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; diff --git a/User/Module/config.c b/User/Module/config.c index bbe561f..9753e55 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -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到位置 @@ -127,7 +127,7 @@ static const ConfigParam_t param ={ }, .chassis = {/**/ - + .M3508_param = { .p = 15.1f, .i = 0.02f, @@ -136,21 +136,49 @@ 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, + + }, + }, diff --git a/User/Module/up.c b/User/Module/up.c index 421a8a6..27c43cc 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -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; -// } -// Co_Process(u,out); +// u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 +// } + +// 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: - - u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; + + + case CoPITCH_MID360: + u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; + + break ; break ; } diff --git a/User/Module/up.h b/User/Module/up.h index 5b5711e..76859d7 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -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;