diff --git a/.gitignore b/.gitignore index 2d6ff72..3b68972 100644 --- a/.gitignore +++ b/.gitignore @@ -17,7 +17,7 @@ *.dbg* *.uvguix.* .mxproject - +*.axf RTE/ Templates/ Examples/ diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index 929c293..63ca8c3 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -28,7 +28,7 @@ 25000000 - 0 + 1 1 0 1 @@ -77,7 +77,7 @@ 0 1 - 0 + 18 0 1 @@ -103,7 +103,7 @@ 1 0 0 - 0 + 3 @@ -114,9 +114,34 @@ - BIN\UL2CM3.DLL + BIN\CMSIS_AGDI.dll + + 0 + ARMRTXEVENTFLAGS + -L70 -Z18 -C0 -M0 -T1 + + + 0 + DLGTARM + (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) + + + 0 + ARMDBGFLAGS + + + + 0 + DLGUARM + + + + 0 + CMSIS_AGDI + -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 UL2CM3 @@ -170,19 +195,59 @@ 1 NUC_send,0x0A + + 9 + 1 + NUC_send + + + 10 + 1 + nuc_raw,0x0A + + + 11 + 1 + ang_58 + + + 12 + 1 + ang_66 + + + 13 + 1 + cmd_fromnuc,0x0A + + + 14 + 1 + Nuc,0x0A + + + 15 + 1 + rec_flag + + + 16 + 1 + ctrl + 0 0 - 0 - 0 + 1 + 1 0 0 0 0 - 0 + 1 0 0 0 @@ -817,7 +882,7 @@ User/Module - 0 + 1 0 0 0 @@ -1025,7 +1090,7 @@ User/Task - 0 + 1 0 0 0 @@ -1233,7 +1298,7 @@ User/Device - 0 + 1 0 0 0 diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx index 64c3412..893edd4 100644 --- a/MDK-ARM/R2.uvprojx +++ b/MDK-ARM/R2.uvprojx @@ -17,8 +17,8 @@ STM32F407IGHx STMicroelectronics - Keil.STM32F4xx_DFP.2.15.0 - http://www.keil.com/pack/ + Keil.STM32F4xx_DFP.3.0.0 + https://www.keil.com/pack/ IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index ec604b4..e1def32 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 7ce6beb..a354b11 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -98,12 +98,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z); /*初始数据*/ - + c->move_vec.Vx = 0; + c->move_vec.Vy = 0; + c->move_vec.Vw = 0; c->to_nuc.send = 0; c->sick_cali.is_reach = 0; c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]); - c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]); + c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]); c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]); @@ -129,9 +131,9 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { break; case Chassis_Adjust_Sick: - sick_calibration(c, ctrl, out); +// sick_calibration(c, ctrl, out); // c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0; - break; + break; } break; @@ -139,18 +141,20 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { case AUTO: switch (c->chassis_ctrl.mode) { case AUTO_MID360: -// c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; -// c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000; +// 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; -// abs_limit_fp(&c->move_vec.Vx, 5000.0f); -// abs_limit_fp(&c->move_vec.Vy, 5000.0f); -// abs_limit_fp(&c->move_vec.Vw, 5000.0f); + c->move_vec.Vw =-ctrl->cmd_MID360.posw * 3000; + + abs_limit_fp(&c->move_vec.Vx, 5000.0f); + abs_limit_fp(&c->move_vec.Vy, 5000.0f); + abs_limit_fp(&c->move_vec.Vw, 5000.0f); break; case AUTO_MID360_Pitch: - 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; +// 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; abs_limit_fp(&c->move_vec.Vx, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f); diff --git a/User/Module/config.c b/User/Module/config.c index beea43a..24a3a08 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -75,7 +75,7 @@ static const ConfigParam_t param ={ /*投球*/ .LaunchCfg = { - .m2006_init = -150.0f, // M2006初始角度 + .m2006_init = -130.0f, // M2006初始角度 .m2006_trig = 0.0f, // M2006触发角度 .go_pull_pos = -214.0f, // go上升去合并扳机，扳机位置 .go_up_speed = 12.0f, // 上升速度 diff --git a/User/Module/up.c b/User/Module/up.c index 1120ddf..4f048a1 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -403,7 +403,14 @@ 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; + if(u->MID360Context.Curve==CURVE_58){ + MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.25,3.2,4.3,&u->MID360Context.Curve); + } + else { MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve); + + } + if (u->MID360Context.Curve == CURVE_58) { target->Pitch_angle = 58; } else { diff --git a/User/Module/up_utils.c b/User/Module/up_utils.c index a1e48f0..212d1cc 100644 --- a/User/Module/up_utils.c +++ b/User/Module/up_utils.c @@ -67,10 +67,9 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo // 计算66度曲线（偏上） // 计算66度曲线（偏上） -static float curve_66(float d) { + static float curve_66(float d) { return 3.7028f * d * d + 11.2126f * d -142.9446f; } - // 计算58度曲线（偏下） // 计算58度曲线（偏下） static float curve_58(float d) { @@ -83,6 +82,9 @@ static float curve_58(float d) { 曲线x重合区，根据当前函数和变化方向切换 */ int abdddd=0; +fp32 ang_58; +fp32 ang_66; + float CurveChange(float d, float x, float y, CurveType *cs) { if (d<3.2) abdddd++; @@ -98,8 +100,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/bsp/protocol.h b/User/bsp/protocol.h index c0d7390..41bd713 100644 --- a/User/bsp/protocol.h +++ b/User/bsp/protocol.h @@ -83,7 +83,7 @@ typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) { - uint8_t status; + fp32 status; } Protocol_UpDataCMD_t; /* 视觉 -> 电控 上层机构数据结构体*/ diff --git a/User/device/cmd.c b/User/device/cmd.c index d28e009..73bd548 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -87,7 +87,7 @@ return 0; int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){ if(cmd == NULL) return -1; - if(n == NULL) return -1; + if(n == NULL) return -1; cmd->cmd_status = n->status_fromnuc; cmd->raw_status = n->ctrl_status; @@ -112,6 +112,16 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){ cmd ->cmd_pick .posw =n->camera.data3 ; break; + + default: + cmd->cmd_MID360.posx = 0; + cmd->cmd_MID360.posy = 0; + cmd->cmd_MID360.posw = 0; + + cmd->pos =0 ; + + + break ; } diff --git a/User/device/cmd.h b/User/device/cmd.h index 3b41b4f..de48449 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -43,7 +43,8 @@ typedef enum{ }CMD_mode_t; typedef struct { uint8_t status_fromnuc; - uint8_t ctrl_status; + uint8_t ctrl_status; + struct { fp32 vx; diff --git a/User/task/cmd_task.c b/User/task/cmd_task.c index 8bf0203..ad1c765 100644 --- a/User/task/cmd_task.c +++ b/User/task/cmd_task.c @@ -16,7 +16,7 @@ static CMD_NUC_t Nuc; #endif - +int rec_flag=0; void Task_cmd(void *argument){ (void)argument; /*未使用传递参数 消除警告*/ @@ -38,14 +38,16 @@ void Task_cmd(void *argument){ osKernelLock(); /*锁住RTOS内核调度*/ /*将各任务接收到的原始数据解析为通用的控制命令*/ + - - /*注意，不能将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)//遥控器 CMD_ParseRc(&cmd, &rc_ctrl); diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index 5c0aa74..e0ad619 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -47,19 +47,23 @@ void Task_nuc(void *argument){ last_online_tick = tick; } else{ - if (tick - last_online_tick > 300) NUC_HandleOffline(&cmd_fromnuc, &nuc_raw); + if (tick - last_online_tick > 300) + NUC_HandleOffline(&cmd_fromnuc, &nuc_raw); } if (nuc_raw.unc_online) { osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc); osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0); } + else + osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc); + osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0); osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0); osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0); - //osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0); + bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &(NUC_send), NULL, 0) == osOK); @@ -73,6 +77,7 @@ void Task_nuc(void *argument){ } + // NUC_StartSend(&nuc_raw, cmd_update); tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/ diff --git a/User/task/user_task.h b/User/task/user_task.h index 9f2ba0f..a03d285 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 (250u) +#define TASK_FREQ_NUC (500u) #define TASK_FREQ_CAN (1000u) #define TASK_FREQ_RC (1000u)