diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index ffaf9a7..c08fbe8 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -135,7 +135,7 @@ 0 CMSIS_AGDI - -X"" -O206 -S8 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + -X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) 0 @@ -200,6 +200,16 @@ 1 \\R2\../User/task/up_task.c\UP.MID360Context.IsLaunch + + 10 + 1 + ang_58,0x0A + + + 11 + 1 + ang_66,0x0A + 0 @@ -207,7 +217,7 @@ 0 1 - 0 + 1 0 0 0 diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 178b9e3..c50144b 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/User/Module/config.c b/User/Module/config.c index e1e4a53..34b8bbe 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -75,11 +75,11 @@ static const ConfigParam_t param ={ /*投球*/ .LaunchCfg = { - .m2006_init = -130.0f, // M2006初始角度 + .m2006_init = -125.0f, // M2006初始角度 .m2006_trig = 0.0f, // M2006触发角度 - .go_pull_pos = -214.0f, // go上升去合并扳机,扳机位置 + .go_pull_pos = -210.0f, // go上升去合并扳机,扳机位置 .go_up_speed = 12.0f, // 上升速度 - .go_down_speed = 6.0f, // 下降速度 + .go_down_speed = 10.0f, // 下降速度 .Pitch_angle = 58, //俯仰角 .go_init = -50 }, @@ -89,14 +89,14 @@ static const ConfigParam_t param ={ .go_release_pos=-200, .Pitch_angle =66, .go_up_speed =12, - .go_down_speed =6, + .go_down_speed =10, }, .PassCfg={ .go_wait =-10, .go_release_pos =-150, .Pitch_angle=66, .go_up_speed =12, - .go_down_speed =6, + .go_down_speed =10, }, .MID360Cfg={ diff --git a/User/Module/up.c b/User/Module/up.c index bf8da88..cabda56 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -199,7 +199,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) is_pitch=0; } //初始上电 //LaunchCfg->go_up_speed=15; - target->Pitch_angle =LaunchCfg->Pitch_angle; +// target->Pitch_angle =LaunchCfg->Pitch_angle; target->go_pull_speed=LaunchCfg->go_up_speed; target->Shoot_M2006_angle=LaunchCfg->m2006_init; u->PitchContext .PitchState = PITCH_PREPARE; //状态更新 @@ -260,7 +260,7 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP bool is_GoStartReach=is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f); //go开始位置 bool is_GoSpeedReach=is_reached(u->motorfeedback.go_data.W,0,1.0f) ; //go速度归零判断 bool is_GoEndReach =(u->motorfeedback .go_data .Pos < -209); //go去上拉钩子位置,判断到达 - bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-10); //2006钩子放下判断 + bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-5.0f); //2006钩子放下判断 bool is_Shoot=(u->motorfeedback.DJmotor_feedback[4].total_angle<-10);//是否发射判断 @@ -400,13 +400,20 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){ LaunchContext_t *LaunchContext = &u->LaunchContext; MID360Context_t *MID360Context=&u->MID360Context; MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg; - MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.2,4.3,&u->MID360Context.Curve); +// MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.2,4.3,&u->MID360Context.Curve); + + if(u->MID360Context.Curve==CURVE_58){ + MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.3,4.3,&u->MID360Context.Curve); + } + else { + MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)-0.05f,3.3,4.3,&u->MID360Context.Curve); + + } if (u->MID360Context.Curve == CURVE_58) { target->Pitch_angle = 58; } else { target->Pitch_angle = 66; - } - + } LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed; LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed; diff --git a/User/Module/up_utils.c b/User/Module/up_utils.c index ea889d5..e579749 100644 --- a/User/Module/up_utils.c +++ b/User/Module/up_utils.c @@ -68,12 +68,12 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo // 计算66度曲线(偏上) static float curve_66(float d) { - return 3.7028f * d * d + 11.2126f * d -142.9446f; - } + return 2.9851104*pow(d,4) -36.164382*pow(d,3) + 159.54844*pow(d,2) -273.62856*d + 43.092452; } // 计算58度曲线(偏下) // 计算58度曲线(偏下) static float curve_58(float d) { - return 0.9242f * d * d + 19.4246f * d - 154.9055f; +// return 0.9242f * d * d + 19.4246f * d - 154.9055f; + return 2.638517*pow(d,4) -47.996138*pow(d,3) + 325.38515*pow(d,2) -950.18155*d + 919.86543; } /* @@ -81,12 +81,15 @@ static float curve_58(float d) { 迟滞区x-y 曲线x重合区,根据当前函数和变化方向切换 */ + fp32 ang_58; +fp32 ang_66; + float CurveChange(float d, float x, float y, CurveType *cs) { + if (*cs == CURVE_66) { if (d > y) { - - *cs = CURVE_58; + *cs = CURVE_58; } } else { // CURVE_58 if (d < x) { @@ -96,8 +99,10 @@ float CurveChange(float d, float x, float y, CurveType *cs) // 根据当前曲线返回结果 if (*cs == CURVE_58) { + ang_58=d; return curve_58(d); } else { + ang_66=d; return curve_66(d); } } diff --git a/User/task/cmd_task.c b/User/task/cmd_task.c index 8bf0203..5ad30e2 100644 --- a/User/task/cmd_task.c +++ b/User/task/cmd_task.c @@ -40,10 +40,15 @@ void Task_cmd(void *argument){ /*将各任务接收到的原始数据解析为通用的控制命令*/ - /*注意,不能将nuc和码盘导航一块使用*/ + - if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc - CMD_ParseNuc(&cmd,&Nuc); + if((osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)){ //nuc + } + else + { + memset(&Nuc, 0, sizeof(CMD_NUC_t)); + } + CMD_ParseNuc(&cmd,&Nuc); if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器 diff --git a/User/task/user_task.h b/User/task/user_task.h index 5587be0..52e20d5 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -17,7 +17,7 @@ #define TASK_FREQ_CHASSIS (900u) #define TASK_FREQ_UP (900u) -#define TASK_FREQ_CTRL_CMD (500u) +#define TASK_FREQ_CTRL_CMD (100u) #define TASK_FREQ_NUC (100u) #define TASK_FREQ_CAN (1000u) #define TASK_FREQ_RC (100u)