diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log index 28d9a59..6c4a5e0 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -475,3 +475,7 @@ [info] Log at : 2025/6/24|03:28:50|GMT+0800 +[info] Log at : 2025/6/26|19:47:59|GMT+0800 + +[info] Log at : 2025/6/27|16:40:58|GMT+0800 + diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index b84add3..ce2b613 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -188,12 +188,17 @@ 6 1 - a,0x0A + tar_angle 7 1 - posss + up_cmd + + + 8 + 1 + a,0x0A diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index c030fe1..3e4b0a7 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 ff17d3f..f24b59f 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -47,9 +47,14 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre } PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param)); PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param)); - PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam)); - PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam)); - PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam)); + /*sick*/ + PID_init(&(c->pid.SickCaliWzInPID), PID_POSITION, &(c->param->SickCali_WInparam)); + PID_init(&(c->pid.SickCaliWzOutPID), PID_POSITION, &(c->param->SickCali_WOutparam)); + PID_init(&(c->pid.SickCaliVxInPID), PID_POSITION, &(c->param->SickCali_XInparam)); + PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam)); + PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam)); + PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam)); + /*修正 */ PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param)); LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波 @@ -138,7 +143,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { c->move_vec.Vy = ctrl->Vx * 6000; break; - case PassBall: // + + default: + + break; + } + case PASS_BALL: + switch (c->chassis_ctrl.mode) + { + case PB_UP: + case PB_MID: + case PB_DOWN: c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000; c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; @@ -146,10 +161,13 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f); break ; - default: - - break; - } + + } + + + + + break; break; default: @@ -171,7 +189,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i]; } - c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2]; +// c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2]; return CHASSIS_OK; @@ -193,8 +211,8 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { if (c == NULL) return CHASSIS_ERR_NULL; if (ctrl == NULL) return CHASSIS_ERR_NULL; - - + fp32 delta_w,delta_x,delta_y; + fp32 sick0 = c->sick_cali.sick_dis[0] ; fp32 sick1 = c->sick_cali.sick_dis[1]; fp32 sick2 = c->sick_cali.sick_dis[2]; @@ -204,10 +222,9 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set); fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set); - - c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0; - c->move_vec.Vx = (fabsf(diff_x) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVxPID), diff_x, 0) : 0; - c->move_vec.Vy = (fabsf(diff_y) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVyPID), diff_y, 0) : 0; + delta_w=(&c->pid.SickCaliWzInPID, diff_yaw, 0); + delta_x=(&c->pid.SickCaliVxInPID, diff_x, 0); + delta_y=(&c->pid.SickCaliVyInPID, diff_y,0); static uint16_t reach_cnt = 0; if (fabsf(diff_yaw) <= param->w_error && diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index 6020adc..810a826 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -80,9 +80,13 @@ typedef struct 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; + pid_param_t SickCali_WInparam; + pid_param_t SickCali_WOutparam; + pid_param_t SickCali_YInparam; + pid_param_t SickCali_YOutparam; + pid_param_t SickCali_XInparam; + pid_param_t SickCali_XOutparam; + pid_param_t Chassis_AngleAdjust_param; sickparam_t sickparam; @@ -154,12 +158,15 @@ typedef struct{ pid_type_def chassis_PICKWzPID_HIGN; /*存在较低误差*/ pid_type_def chassis_PICKWzPID_LOW; - pid_type_def Chassis_AngleAdjust; - pid_type_def SickCaliWzPID; - pid_type_def SickCaliVxPID; - pid_type_def SickCaliVyPID; + /*sick */ + pid_type_def SickCaliWzInPID; + pid_type_def SickCaliWzOutPID; + pid_type_def SickCaliVxInPID; + pid_type_def SickCaliVxOutPID; + pid_type_def SickCaliVyInPID; + pid_type_def SickCaliVyOutPID; }pid; diff --git a/User/Module/config.c b/User/Module/config.c index d748f9c..889ed2d 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -37,18 +37,18 @@ static const ConfigParam_t param ={ .out_limit = 3000.0f, }, .Pitch_M2006_angle_param = { - .p = 600.0f, + .p = 1600.0f, .i = 0.0f, .d = 3.0f, - .i_limit = 2000.0f, - .out_limit = 3000.0f, + .i_limit = 4000.0f, + .out_limit = 20000.0f, }, .Pitch_M2006_speed_param={ - .p = 5.0f, - .i = 0.3f, + .p = 8.5f, + .i = 0.03f, .d = 0.0f, .i_limit = 2000.0f, - .out_limit = 3000.0f, + .out_limit = 15000.0f, }, .Pitch_Angle_M2006_speed_param={ //俯仰跑速度环 .p = 20.0f, @@ -71,20 +71,32 @@ static const ConfigParam_t param ={ }, -/*上层其他参数*/ +/*上层其他参数,一些只是初始值,在运行过程中会被更改*/ /*投球*/ - .PitchCfg = { - .m2006_init =-150, //释放的角度 - .m2006_trig =0, //可以拉住发射的角度 - .go_init = -50, //仅用在go上电,初始位置 - .go_pull_pos =-214, - .Pitch_angle =58, - - .pull_speed =5, - + .LaunchCfg = { + .m2006_init = -150.0f, // M2006初始角度 + .m2006_trig = 0.0f, // M2006触发角度 + .go_pull_pos = -214.0f, // go上升去合并扳机,扳机位置 + .go_up_speed = 12.0f, // 上升速度 + .go_down_speed = 6.0f, // 下降速度 + .Pitch_angle = 66, //俯仰角 + .go_init = -50 }, + .PitchCfg = { + .go_init = -50, //仅用在go上电,初始位置 + .go_release_pos=-200, + .Pitch_angle =66, + +}, + .PassCfg={ + .go_wait =-10, + .go_release_pos =-150, + .Pitch_angle=66, + .go_up_speed =12, + .go_down_speed =5, + }, }, .chassis = {/**/ @@ -111,28 +123,49 @@ static const ConfigParam_t param ={ .i_limit = 50.0f, .out_limit =1000.0f, }, - .Sick_CaliWparam ={ - .p = 4.5f, - .i = 0.005f, - .d = 0.0f, - .i_limit = 500.0f, - .out_limit =1000.0f, - }, - .Sick_CaliYparam ={ - .p = 2.9f, - .i = 0.0051f, - .d = 0.0f, - .i_limit = 500.0f, - .out_limit =3000.0f, - }, - .Sick_CaliXparam ={ - .p = 2.9f, - .i = 0.0065f, - .d = 0.0f, - .i_limit = 500.0f, - .out_limit =3000.0f, - }, - .Chassis_AngleAdjust_param={ + .SickCali_WInparam = { + .p = 0.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 0.0f, + }, + .SickCali_WOutparam = { + .p = 0.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 0.0f, + }, + .SickCali_YInparam = { + .p = 0.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 0.0f, + }, + .SickCali_YOutparam = { + .p = 0.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 0.0f, + }, + .SickCali_XInparam = { + .p = 0.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 0.0f, + }, + .SickCali_XOutparam = { + .p = 0.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 0.0f, + }, + .Chassis_AngleAdjust_param={ .p = 10.0f, .i = 0.0f, .d = 0.0f, diff --git a/User/Module/up.c b/User/Module/up.c index a99de06..888ea41 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -43,13 +43,19 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq) LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f); - if (!u->Pitch_Cfg .is_init) { - u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值 - u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 - u->Pitch_Cfg .is_init = 1; + if (!u->PitchContext .is_init) { + u->PitchContext .PitchConfig = u->param ->PitchCfg ;//赋值 + u->PitchContext .PitchState = PITCH_PREPARE; //状态更新 + u->PitchContext .is_init = 1; } - - BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数,bsp层 + if(!u->PassContext.is_init) { + u->PassContext .PassCfg = u->param ->PassCfg ;//赋值 + u->PassContext .PassState = PASS_STOP; //状态更新 + u->PassContext .is_init = 1; + } + u->LaunchContext.LaunchCfg = u->param->LaunchCfg; + + BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数,bsp层 } @@ -118,8 +124,6 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed) - - int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) { //电机控制 ,传进can @@ -131,16 +135,17 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) /*俯仰2006,双环,内使用oid角度环,外电机速度环 */ + DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed, (PID_calc (&(u->pid .Pitch_M2006_angle ), - u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle)) + u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle)+0.14) ); - + GO_SendData( &u->motorfeedback.go_data, &u->go_cmd,u->motor_target .go_shoot, - u->Pitch_Cfg .PitchConfig .pull_speed - );//对应的拉动速度 + u->motor_target.go_pull_speed + ); for(int i=0;i<4;i++){ out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ; @@ -166,17 +171,18 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) if(c ==NULL) return 0; /*指针简化*/ - PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig; + PitchCfg_t *pitch_cfg = &u->PitchContext.PitchConfig; + LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg; up_motor_target_t *target=&u->motor_target ; /*config值限位*/ - abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-215.0f,0.0f); - abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.Pitch_angle ,48.0f,67.0f); + abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go_release_pos ,-215.0f,0.0f); + abs_limit_min_max_fp(&u->PitchContext.PitchConfig.Pitch_angle ,48.0f,67.0f); /*部分数据更新*/ static int is_pitch=1; - posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve); -// if (u->Pitch_Cfg.Curve == CURVE_58) { + posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve); +// if (u->PitchContext.Curve == CURVE_58) { // target->Pitch_angle = 58; // } else { // target->Pitch_angle = 66; @@ -185,85 +191,100 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) switch (c->CMD_CtrlType ) { case RCcontrol: //在手动模式下 - + + switch (c-> CMD_mode ) { - case Normal : + case Normal : //复位,go位置和俯仰角保持LaunchCfg最后修改后的位置,扳机2006角度复位用于发射 /*投篮*/ if(is_pitch){ - target->Shoot_M2006_angle = pitch_cfg->go_init ; - target->Pitch_angle = pitch_cfg->Pitch_angle; + target->go_shoot = LaunchCfg->go_init ; + target->Pitch_angle = LaunchCfg->Pitch_angle; is_pitch=0; - } //让初始go位置只读一次,后面按调整模式更改的来,后面稳定了之后,可以跟随调整模式下一块删 + } //初始上电 + //LaunchCfg->go_up_speed=15; + target->Pitch_angle =LaunchCfg->Pitch_angle; - target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ; - u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 - + + target->Shoot_M2006_angle=LaunchCfg->m2006_init; + u->PitchContext .PitchState = PITCH_PREPARE; //状态更新 + u->LaunchContext.LaunchState = Launch_Stop; break; case Pitch : - if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE) + if (u->PitchContext .PitchState ==PITCH_PREPARE) { - u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮 + u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 } - Pitch_Process(u,out); + Pitch_Process(u,out,c); break ; - case UP_Adjust: //测试用 - pitch_cfg ->pull_speed=5; + case UP_Adjust: //测试用,手动使用pitch下的cfg + LaunchCfg->go_up_speed=5; pitch_cfg ->go_release_pos += c->Vx ; - pitch_cfg->Pitch_angle += c->Vy/100; + LaunchCfg->Pitch_angle += c->Vy/100; - target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos; - target->Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle; + target->go_shoot=pitch_cfg ->go_release_pos; + target->Pitch_angle=LaunchCfg->Pitch_angle; break ; - + default: + break; } break; case AUTO: /*自动下数据更新,根据距离切换俯仰角,如需自动调整,放在*/ - if (u->Pitch_Cfg.Curve == CURVE_58) { - target->Pitch_angle = 58; - } else { - target->Pitch_angle = 66; - } +// if (u->PitchContext.Curve == CURVE_58) { +// target->Pitch_angle = 58; +// } else { +// target->Pitch_angle = 66; +// } switch(c-> CMD_mode) { case AUTO_MID360_Pitch: - if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE) + if (u->PitchContext .PitchState ==PITCH_PREPARE || u->PitchContext .PitchState ==PITCH_COMPLETE) { - u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮 + u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 } /*根据距离实时计算所需pos*/ - + if(c->pos){ pitch_cfg ->go_release_pos= - CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve); - - Pitch_Process(u,out); + CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve); + } + else pitch_cfg ->go_release_pos=0 ; + Pitch_Process(u,out,c); break ; case AUTO_MID360: - target->Shoot_M2006_angle =pitch_cfg ->m2006_init ; + target->Shoot_M2006_angle =LaunchCfg ->m2006_init ; - u->Pitch_Cfg .PitchState = PITCH_PREPARE; + break ; + default: + break; } break ; - + case PASS_BALL: +// if (u->PitchContext.Curve == CURVE_58) { +// target->Pitch_angle = 58; +// } else { +// target->Pitch_angle = 66; +// } + Pass_Process(u,out,c); + break; case RELAXED: // target->go_shoot= pitch_cfg . - - target->go_shoot = pitch_cfg->go_init; target->Pitch_angle = 58; - target->Shoot_M2006_angle =pitch_cfg->m2006_init; + target->go_shoot = -210.0f; +// target->Pitch_angle = 58; + break ; @@ -275,58 +296,133 @@ return 0; - - - -int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) -{ +//复用发射, +int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){ - PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig; - PitchState_t *state =&u->Pitch_Cfg .PitchState; - up_motor_target_t *target=&u->motor_target ; - - switch(*state){ - - case PITCH_START: -// u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 - cfg->pull_speed=12; - target->go_shoot = cfg->go_pull_pos; + switch(LaunchContext->LaunchState){ + case Launch_Stop: break; + case Launch_PREPARE: + u->motor_target.go_shoot = StartPos; + if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&& + is_reached(u->motorfeedback.go_data.W,0,1.0f)){ + //根据位置和速度判断是否到达初始位置 + LaunchContext->LaunchState = Launch_START; + + }break; + + case Launch_START: + u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed; + u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos; + if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面,这里的检测条件可以更改 + u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度,关闭 + + LaunchContext->LaunchState = Launch_TRIGGER; + }break; - if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面,这里的检测条件可以更改 - target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度,0 - u->Pitch_Cfg .PitchConfig .pull_speed=6; - *state=PITCH_PULL_TRIGGER; -}//更改标志位 - - break ; - case PITCH_PULL_TRIGGER: + case Launch_TRIGGER: + if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1){ //当2006的总角度小于1,可以认为已经勾上,误差为1 + u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed; + u->motor_target.go_shoot = EndPos ; + if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f)) + LaunchContext->LaunchState = Launch_SHOOT_WAIT; + } break; - if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1,可以认为已经勾上,误差为1 - { - target->go_shoot=cfg->go_release_pos; - if(is_reached (u->motorfeedback .go_data .Pos ,target->go_shoot ,1.0f)) - { - *state=PITCH_LAUNCHING; - } - } + case Launch_SHOOT_WAIT: + if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射 + LaunchContext->LaunchState = Launch_DONE; + break; + case Launch_DONE: break ; - - case PITCH_LAUNCHING: //等待发射 -// *state=PITCH_COMPLETE; - break ; - case PITCH_COMPLETE: //发射完成 - - - break ; - - } - -return 0; } +int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) +{ + /*简化*/ + PitchCfg_t *cfg = &u->PitchContext.PitchConfig; + PitchState_t *state =&u->PitchContext .PitchState; + up_motor_target_t *target=&u->motor_target ; + LaunchContext_t *LaunchContext = &u->LaunchContext; + // 可根据实际需要传入不同的起始和末位置,起始:当前位置 + Pitch_Launch_Sequence(u, LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out); + + + switch(*state){ + + case PITCH_START: + LaunchContext->LaunchState = Launch_PREPARE; + + *state=PITCH_WAIT; + break; + + case PITCH_WAIT: + + if(LaunchContext->LaunchState==Launch_DONE) + *state=PITCH_COMPLETE; + break; + case PITCH_COMPLETE: + + break; + default: + break; + + } + + + return 0; + +} + + + +int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c) +{ + + PassCfg_t *PassCfg = &u->PassContext.PassCfg; + PassState_t *state = &u->PassContext.PassState; + up_motor_target_t *target=&u->motor_target ; + LaunchContext_t *LaunchContext = &u->LaunchContext; + + Pass_Sequence_Check(u,c); + + if(c->pos) // + { + PassCfg ->go_release_pos = + CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve); + + } + switch (*state) { //遥控器按键进行状态切换 + case PASS_STOP: + + break; + case PASS_IDLE: + LaunchContext->LaunchState = Launch_PREPARE; + break; + case PASS_PREPARE: + target->go_pull_speed=PassCfg->go_up_speed; + + Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out); + + + break; + case PASS_START: + if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){ + target->go_pull_speed=PassCfg->go_down_speed; + target->go_shoot = PassCfg->go_release_pos ; + } + break ; + case PASS_POS_PREPARE: + target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射 + break; + case PASS_COMPLETE: + break; + default: + break; + } + return 0; +} diff --git a/User/Module/up.h b/User/Module/up.h index 51c96f5..599c0f8 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -28,26 +28,50 @@ */ +/*共用发射,重复部分*/ +typedef struct { + fp32 m2006_init; // M2006初始角度 + fp32 m2006_trig; // M2006触发角度0,用来合并扳机 + fp32 go_init; + fp32 go_pull_pos; // go上升去合并扳机,扳机位置 + fp32 go_up_speed; + fp32 go_down_speed; + + fp32 Pitch_angle; +} LaunchCfg_t; +typedef enum { + Launch_Stop, + Launch_PREPARE, // + Launch_START, //启动,从指定位置上升,扣动扳机 + Launch_TRIGGER, //拉动到指定位置 + Launch_SHOOT_WAIT, // 发射等待 + Launch_SHOOT, + Launch_DONE, + + +}LaunchState_t; +typedef struct { + LaunchCfg_t LaunchCfg; + LaunchState_t LaunchState; +} LaunchContext_t; + /* 投球状态定义 */ typedef enum { + PITCH_PREPARE, // 准备阶段 PITCH_START, //启动,拉扳机 - PITCH_PULL_TRIGGER, - PITCH_LAUNCHING, // 发射中 + + PITCH_WAIT, // 发射等待 PITCH_COMPLETE // 完成 } PitchState_t; /* 投球参数配置 */ typedef struct { - fp32 m2006_init; // M2006初始角度 - fp32 m2006_trig; // M2006触发角度0,用来合并扳机 - fp32 go_init; // GO电机触发位置,初始位置,后续更改用于发射的位置 - fp32 go_pull_pos; // go上升去合并扳机,扳机位置 - fp32 Pitch_angle; //俯仰角度,以oid为准 - - fp32 pull_speed;//go速度 + + fp32 go_init; // GO电机触发位置,初始位置,后续更改用于发射的位置 + fp32 Pitch_angle; //俯仰角度,以oid为准 fp32 go_release_pos;//go松开,发射位置 } PitchCfg_t; @@ -57,17 +81,32 @@ typedef struct { PitchCfg_t PitchConfig; CurveType Curve; //当前函数,雷达距离->pos uint8_t is_init ; -} Pitch_Cfg_t; +} PitchContext_t; -/*运球*/ +/*传球过程 */ typedef enum { - DRIBBLE_PREPARE, // 准备阶段 - DRIBBLE_START, - DRIBBLE_TRANSLATE, // 平移过程 - DRIBBLE_PROCESS_DOWN, // 运球过程,球下落 - DRIBBLE_PROCESS_UP, // 运球过程,球上升 - DRIBBLE_DONE // 运球结束 -} DribbleState_t; + PASS_STOP, // 停止状态,未进入传球模式 + PASS_IDLE, // 空闲状态,进入未开始 + PASS_PREPARE, // 传球准备 + PASS_START, // 启动传球,拉动到等球位置 + PASS_POS_PREPARE, // 传球位置准备,对准篮筐,go的位置 + PASS_COMPLETE // 发射 +} PassState_t; +typedef struct { + + fp32 go_wait; // GO等待位置 + fp32 go_release_pos; // GO电机发射位置 + fp32 Pitch_angle; // 俯仰角度,以oid为准 + fp32 go_up_speed; // GO电机上升速度 + fp32 go_down_speed; // GO电机下降速度 + +} PassCfg_t; +typedef struct { + PassState_t PassState; + PassCfg_t PassCfg; + CurveType Curve; //当前函数,雷达距离->pos + uint8_t is_init ; +} PassContext_t; typedef struct { @@ -91,7 +130,10 @@ typedef struct pid_param_t Pitch_Angle_M2006_speed_param; + LaunchCfg_t LaunchCfg; PitchCfg_t PitchCfg; + PassCfg_t PassCfg; + GO_MotorCmd_t go_cmd; }UP_Param_t; @@ -103,7 +145,7 @@ typedef struct{ fp32 Shoot_M2006_angle; fp32 go_shoot; - + fp32 go_pull_speed; fp32 Pitch_angle; //以oid的角度为准,目标俯仰角 }up_motor_target_t; @@ -128,10 +170,12 @@ typedef struct{ typedef struct{ uint8_t up_task_run; - const UP_Param_t *param; + const UP_Param_t *param; + LaunchContext_t LaunchContext; /*投篮过程*/ - Pitch_Cfg_t Pitch_Cfg; - + PitchContext_t PitchContext; + /*传球过程*/ + PassContext_t PassContext; CMD_t *cmd; struct{ @@ -184,7 +228,10 @@ int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle); int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle); int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed); -int8_t Pitch_Process(UP_t *u, CAN_Output_t *out); +int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c); +int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c); +int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c); +int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out); #endif diff --git a/User/Module/up_utils.c b/User/Module/up_utils.c index bbcbd26..9faeae8 100644 --- a/User/Module/up_utils.c +++ b/User/Module/up_utils.c @@ -1,5 +1,5 @@ #include "up_utils.h" - +#include "up.h" int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle) { @@ -99,3 +99,62 @@ float CurveChange(float d, float x, float y, CurveType *cs) } } +int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测,传球,按需求改 +{ + PassState_t *state = &u->PassContext.PassState; + static enum { + SEQ_IDLE, + SEQ_MID1, + SEQ_UP, + SEQ_MID2, + SEQ_DOWN + } seq = SEQ_IDLE; + + switch (seq) { + case SEQ_IDLE: + if (c->CMD_mode == PB_MID) { + seq = SEQ_MID1; + *state = PASS_IDLE; + } + break; + case SEQ_MID1: + if (c->CMD_mode == PB_UP) { + seq = SEQ_UP; + *state = PASS_PREPARE; + } else if (c->CMD_mode != PB_MID) { + seq = SEQ_IDLE; + *state = PASS_STOP; + } + break; + case SEQ_UP: + if (c->CMD_mode == PB_MID) { + seq = SEQ_MID2; + *state = PASS_START; + } else if (c->CMD_mode != PB_UP) { + seq = SEQ_IDLE; + *state = PASS_STOP; + } + break; + case SEQ_MID2: + if (c->CMD_mode == PB_DOWN) { + seq = SEQ_DOWN; + *state = PASS_POS_PREPARE; + } else if (c->CMD_mode != PB_MID) { + seq = SEQ_IDLE; + *state = PASS_STOP; + } + break; + case SEQ_DOWN: + if (c->CMD_mode == PB_MID) { + seq = SEQ_IDLE; + *state = PASS_IDLE; // 或 PASS_COMPLETE,看你需求 + } else if (c->CMD_mode != PB_DOWN) { + seq = SEQ_IDLE; + *state = PASS_STOP; + } + break; + } + return 0; +} + + diff --git a/User/device/cmd.c b/User/device/cmd.c index 7646fbe..c8d6c43 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -164,10 +164,10 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) { } else if(rc->dr16.sw_l==CMD_SW_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; //左下,右上,无模式 + 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; //左下,右上,无模式 } diff --git a/User/device/cmd.h b/User/device/cmd.h index d48c436..fba1fb8 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -14,6 +14,7 @@ typedef enum{ RCcontrol,//遥控器控制,左按键上,控制上层 AUTO, + PASS_BALL,//传球模式 RELAXED,//异常模式 }CMD_CtrlType_t; @@ -29,11 +30,13 @@ typedef enum{ UP_Adjust,//上层调整 Chassis_Adjust_Sick, - - PassBall, + /*传球占三个,逻辑在up.h里,简化就这样写了*/ + PB_UP, + PB_MID, + PB_DOWN, - Pitch, //投篮,底盘锁定 + Pitch, Dribbl_transfer diff --git a/User/device/nuc.c b/User/device/nuc.c index 032a0db..c093310 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -62,7 +62,8 @@ int8_t NUC_Restart(void) { return DEVICE_OK; } bool_t NUC_WaitDmaCplt(void) { - return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,20) == + + return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,50) == SIGNAL_NUC_RAW_REDY); } @@ -169,7 +170,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n){ int8_t NUC_HandleOffline(CMD_NUC_t *cmd) { if(cmd == NULL) return DEVICE_ERR_NULL; - memset(cmd, 0, sizeof(*cmd)); +// memset(cmd, 0, sizeof(*cmd)); return 0; } diff --git a/User/device/vofa.c b/User/device/vofa.c index b897999..6094936 100644 --- a/User/device/vofa.c +++ b/User/device/vofa.c @@ -59,7 +59,7 @@ void vofa_tx_main(float *data) /*在下面使用对应的串口发送函数*/ // HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet)); -// CDC_Transmit_FS( packet, sizeof(packet)); -// osDelay(1); + CDC_Transmit_FS( packet, sizeof(packet)); + osDelay(1); } \ No newline at end of file diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c index 58d3938..5a09e1f 100644 --- a/User/task/chassis_task.c +++ b/User/task/chassis_task.c @@ -98,7 +98,7 @@ void Task_Chassis(void *argument) vofa_send[6] = chassis.vofa_send[6]; vofa_send[7] = chassis.vofa_send[7]; - vofa_tx_main(vofa_send); +// vofa_tx_main(vofa_send); tick += delay_tick; osDelayUntil(tick); diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index 54c5af5..3249396 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -36,7 +36,7 @@ void Task_nuc(void *argument){ a++; NUC_StartReceiving(); NUC_StartSending(NUC_send.send); -// NUC_StartSending (send); + NUC_StartSending (send); if (NUC_WaitDmaCplt()){ NUC_RawParse(&cmd_fromnuc); } diff --git a/User/task/up_task.c b/User/task/up_task.c index 6422d55..14e014a 100644 --- a/User/task/up_task.c +++ b/User/task/up_task.c @@ -91,8 +91,11 @@ void Task_up(void *argument) osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0); -// vofa_send [0]=UP.vofa_send [0]; -// vofa_send [1]=UP.vofa_send [1]; + vofa_send [0]=UP.vofa_send [0]; + vofa_send [1]=UP.vofa_send [1]; + vofa_send [2]=UP.vofa_send [2]; + vofa_send [3]=UP.vofa_send [3]; + vofa_send [4]=UP.vofa_send [4]; vofa_tx_main (vofa_send); tick += delay_tick; diff --git a/User/task/user_task.h b/User/task/user_task.h index 2d0a387..c24ab3b 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -18,7 +18,7 @@ #define TASK_FREQ_UP (900u) #define TASK_FREQ_CTRL_CMD (500u) -#define TASK_FREQ_NUC (50u) +#define TASK_FREQ_NUC (200u) #define TASK_FREQ_CAN (1000u) #define TASK_FREQ_RC (1000u)