diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index 612ed0e..d70039e 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -173,32 +173,12 @@ 3 1 - UP,0x0A + huart->ErrorCode 4 1 - chassis,0x0A - - - 5 - 1 - \\R2\../User/task/chassis_task.c\chassis.sick_cali.sickparam - - - 6 - 1 - cmd,0x0A - - - 7 - 1 - nucbuf,0x0A - - - 8 - 1 - cmd_fromnuc + UP,0x0A diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx index 893edd4..64c3412 100644 --- a/MDK-ARM/R2.uvprojx +++ b/MDK-ARM/R2.uvprojx @@ -17,8 +17,8 @@ STM32F407IGHx STMicroelectronics - Keil.STM32F4xx_DFP.3.0.0 - https://www.keil.com/pack/ + Keil.STM32F4xx_DFP.2.15.0 + http://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 40d3460..46841aa 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 0747bdd..99a5bdb 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -72,19 +72,19 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre c->sick_cali .sickparam=c->param ->sickparam ; - + + c->ang_cail.is_open=1; return CHASSIS_OK; } -fp32 pian_yaw; void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) { fp64 Nor_Vx, Nor_Vy; normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); - c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前 - c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后 - c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后 - c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +pian_yaw; // 左前 + c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前 + c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后 + c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后 + c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前 Chassis_AngleCompensate(c); @@ -138,8 +138,8 @@ 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); @@ -147,8 +147,8 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { // c->NUC_send.send[0] = 1; 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.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); @@ -175,8 +175,8 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { break ; 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.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); @@ -293,17 +293,21 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) int8_t Chassis_AngleCompensate(Chassis_t *c) { if (c == NULL) return CHASSIS_ERR_NULL; + if(c->ang_cail.is_open==0) return 0; + static fp32 pian_angel,cur_angle; + if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0)) { - pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw); + c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw); + } else { - cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw); - pian_angel=0; + c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw); + c->ang_cail.ang_error=0; } - pian_yaw = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0); + c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0); } diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index 90930c5..91d56cd 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -177,6 +177,16 @@ typedef struct{ extKalman_t extKalman[3]; LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */ + /*角度偏转修正 */ + struct{ + + int is_open; + fp32 ang_error; + fp32 ang_cur; + fp32 out; + + + }ang_cail; struct { fp32 sick_dis[4]; //获取到的sick激光值 diff --git a/User/Module/config.c b/User/Module/config.c index 91ea5a6..efd02a0 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -80,7 +80,7 @@ static const ConfigParam_t param ={ .go_pull_pos = -214.0f, // go上升去合并扳机，扳机位置 .go_up_speed = 12.0f, // 上升速度 .go_down_speed = 6.0f, // 下降速度 - .Pitch_angle = 66, //俯仰角 + .Pitch_angle = 58, //俯仰角 .go_init = -50 }, .PitchCfg = { diff --git a/User/Module/up.c b/User/Module/up.c index f514f97..df4e395 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -182,15 +182,10 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) /*部分数据更新*/ static int is_pitch=1; posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve); -// if (u->PitchContext.Curve == CURVE_58) { -// target->Pitch_angle = 58; -// } else { -// target->Pitch_angle = 66; -// } - u->vofa_send [2] = c->pos; - u->vofa_send [3] = LowPassFilter2p_Apply(&u->filled[0],c->pos); - u->vofa_send [4] =1; - + u->vofa_send[0] = u->MID360Context.MID360Cfg.go_release_pos; + u->vofa_send[1] = u->motorfeedback.go_data.Pos; + u->vofa_send[2] = c->pos; + switch (c->CMD_CtrlType ) { case RCcontrol: //在手动模式下 @@ -263,13 +258,23 @@ return 0; //复用发射， int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){ + /*电机位置到达判断*/ + + 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_Shoot=(u->motorfeedback.DJmotor_feedback[4].total_angle<-10);//是否发射判断 + + + + 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)){ + if(is_GoStartReach&& is_GoSpeedReach){ //根据位置和速度判断是否到达初始位置 LaunchContext->LaunchState = Launch_START; @@ -278,7 +283,7 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP 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位置到达最上面，这里的检测条件可以更改 + if(is_GoEndReach){ //检测go位置到达最上面，这里的检测条件可以更改 u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭 LaunchContext->LaunchState = Launch_TRIGGER; @@ -286,7 +291,7 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP case Launch_TRIGGER: - if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1){ //当2006的总角度小于1，可以认为已经勾上,误差为1 + if( is_HookDone ){ //当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)) @@ -294,11 +299,10 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP } break; case Launch_SHOOT_WAIT: - if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射 + if(is_Shoot) //认为发射 LaunchContext->LaunchState = Launch_DONE; break; - case Launch_DONE: - break ; + default:break; } } @@ -356,7 +360,7 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c) PassCfg ->go_release_pos = - CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3,4,&u->PassContext.Curve); + CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve); switch (*state) { //遥控器按键进行状态切换 case PASS_STOP: @@ -395,7 +399,7 @@ 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->pos)+0.15,3.2,4.3,&u->MID360Context.Curve); + 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 b493bb4..a1e48f0 100644 --- a/User/Module/up_utils.c +++ b/User/Module/up_utils.c @@ -1,29 +1,32 @@ #include "up_utils.h" #include "up.h" -int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle) +int8_t DJ_processdata(DJmotor_feedback_t *f, fp32 ecd_to_angle) { - int8_t cnt=0; - fp32 angle ,delta; - - angle = f->ecd; + fp32 angle, delta; + + angle = f->ecd; + + // 初始化阶段，记录 offset if (f->init_cnt < 50) { - f->orig_angle= angle; - f->last_angle = angle; - f->init_cnt++; - return 0; + f->offset_ecd = (uint16_t)angle; // 记录初始偏移 + f->orig_angle = angle - f->offset_ecd; // orig_angle 归零 + f->last_angle = angle - f->offset_ecd; + f->init_cnt++; + return 0; } - - - delta = angle - f->last_angle; + + // 使用 offset 修正 + angle = angle - f->offset_ecd; + + delta = angle - f->last_angle; if (delta > 4096) { f->round_cnt--; } else if (delta < -4096) { f->round_cnt++; } - f->last_angle = angle; - f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle; - + f->last_angle = angle; + f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle; } /*go电机控制*/ @@ -65,13 +68,13 @@ 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) { - return 4.0310f * d * d + 8.9026f * d -139.5156; -} + return 3.7028f * d * d + 11.2126f * d -142.9446f; + } // 计算58度曲线（偏下） // 计算58度曲线（偏下） static float curve_58(float d) { - return -1.9776f * d * d + 42.8499f * d - 204.2442f; + return 0.9242f * d * d + 19.4246f * d - 154.9055f; } /* diff --git a/User/Module/up_utils.h b/User/Module/up_utils.h index c29ab78..436e403 100644 --- a/User/Module/up_utils.h +++ b/User/Module/up_utils.h @@ -33,6 +33,8 @@ typedef struct int32_t round_cnt; int init_cnt; fp32 total_angle; + uint16_t offset_ecd; + uint32_t msg_cnt; }DJmotor_feedback_t; diff --git a/User/device/cmd.h b/User/device/cmd.h index fba1fb8..03a1af9 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -43,7 +43,7 @@ typedef enum{ }CMD_mode_t; typedef struct { uint8_t status_fromnuc; - uint8_t ctrl_status; //取其中每一个二进制位用作通信 + uint8_t ctrl_status; struct { fp32 vx; @@ -52,6 +52,7 @@ typedef struct { fp32 pos; fp32 angle; + char flag; }MID360; diff --git a/User/device/nuc.c b/User/device/nuc.c index 0685a15..4d866f7 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -40,33 +40,6 @@ int8_t NUC_StartReceiving() return DEVICE_OK; return DEVICE_ERR; } -int8_t NUC_StartSending(fp32 *data) -{ - - union - { - float x[1]; - char data[4]; - } instance; - - // for (int i = 0; i < 1; i++) { - instance.x[0] = data[0]; - // } - - SendBuffer[0] = 0xFC; // 帧头 - SendBuffer[1] = 0x01; // 控制帧 - for (int i = 2; i < 6; i++) - { - SendBuffer[i] = instance.data[i - 2]; - } - SendBuffer[6] = 0xFD; // 帧尾 - - if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC), - (uint8_t *)SendBuffer, - sizeof(SendBuffer)) == HAL_OK) - return DEVICE_OK; - return DEVICE_ERR; -} int8_t NUC_Restart(void) { __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC)); @@ -179,7 +152,7 @@ int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc) if (cmd == NULL) return DEVICE_ERR_NULL; nuc->unc_online = false; // 设置为离线状态 - // memset(cmd, 0, sizeof(*cmd)); + memset(cmd, 0, sizeof(*cmd)); return 0; } diff --git a/User/device/nuc.h b/User/device/nuc.h index 866483a..b80178b 100644 --- a/User/device/nuc.h +++ b/User/device/nuc.h @@ -51,7 +51,6 @@ typedef struct { int8_t NUC_Init(NUC_t *nuc); int8_t NUC_StartReceiving(void); -int8_t NUC_StartSending(fp32 *data) ; bool_t NUC_WaitDmaCplt(void); int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc); int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc); diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index 9be52b0..c66c9ce 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -21,6 +21,8 @@ NUC_send_t NUC_send; #endif fp32 send[4]={11.0f,45.0,1.f,4.0f}; int a=0; + + void Task_nuc(void *argument){ (void)argument; /**/ @@ -34,10 +36,7 @@ void Task_nuc(void *argument){ while (1) { #ifdef DEBUG - task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); - task_runtime .freq.nuc = TASK_FREQ_NUC; - task_runtime.last_up_time.nuc= tick; - + task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); #endif a++; NUC_StartReceiving(); @@ -69,7 +68,7 @@ void Task_nuc(void *argument){ } - NUC_StartSend(&nuc_raw, cmd_update); + // NUC_StartSend(&nuc_raw, cmd_update); tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/ osDelayUntil(tick);