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)