From 8ef2a0a3481918fd498ed0fc486049dcc641f58b Mon Sep 17 00:00:00 2001 From: ZHAISHUI04 <3150778793@qq.com> Date: Tue, 3 Jun 2025 23:16:33 +0800 Subject: [PATCH] =?UTF-8?q?=E9=9B=B7=E8=BE=BE=E5=A2=9E=E5=8A=A0=E5=8F=91?= =?UTF-8?q?=E9=80=81=EF=BC=8C=E5=A2=9E=E5=8A=A0=E8=A7=86=E8=A7=89=E5=A4=87?= =?UTF-8?q?=E7=94=A8=EF=BC=8C=E5=A2=9E=E5=8A=A0go=E7=94=B5=E6=9C=BA?= =?UTF-8?q?=E9=99=90=E4=BD=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Src/main.c | 1 + User/Module/Chassis.c | 120 ++++++++++++++++-------------------------- User/Module/up.c | 27 ++++++---- User/device/cmd.c | 28 +++++----- User/device/cmd.h | 22 ++++---- User/device/nuc.c | 68 ++++++++++++------------ User/task/nuc_task.c | 2 +- 7 files changed, 124 insertions(+), 144 deletions(-) diff --git a/Core/Src/main.c b/Core/Src/main.c index f48044b..2372718 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -80,6 +80,7 @@ int main(void) /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ + HAL_Init(); /* USER CODE BEGIN Init */ diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index d1ea42d..f2ce673 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -151,87 +151,57 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) break; - case AUTO : //在自动模式下 - - switch(c->chassis_ctrl.mode ){ - + case AUTO: // 自动模式 + switch (c->chassis_ctrl.mode) { - case AUTO_NAVI: //自动雷达 -// //这套是全向轮的方向,一定要注意这里的xy方向 - 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 =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw); -// c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy); -// c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx); -// -// c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw); -// c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx); -// c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy); -// //电机速度限幅 + case AUTO_MID360: // 自动雷达 + // 全向轮方向, 注意xy方向 + 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,2000.0f); + 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); - abs_limit_fp(&c->move_vec.Vy,2000.0f); + c->NUC_send .send [0]=0; + break; - abs_limit_fp(&c->move_vec.Vw,2000.0f); + case AUTO_MID360_Pitch: + c->move_vec.Vw = 0; + c->move_vec.Vy = 0; + c->move_vec.Vx = 0; + + c->NUC_send .send [0]=0; + break; + case AUTO_MID360_Adjust: + c->move_vec.Vw = ctrl->Vw * 6000; + c->move_vec.Vx = ctrl->Vy * 6000; + c->move_vec.Vy = ctrl->Vx * 6000; + + c->NUC_send .send [0]=1; + break; + default: + c->move_vec.Vw = 0; + c->move_vec.Vx = 0; + c->move_vec.Vy = 0; + c->NUC_send .send [0]=0; + + break; + } + break; - if(ctrl->status[5] ==1) - { - c->move_vec.Vw = c->move_vec.Vw * 0.8f; - c->move_vec.Vx = c->move_vec.Vx * 0.5f; - c->move_vec.Vy = c->move_vec.Vy * 0.5f; - } + default: + c->move_vec.Vw = 0; + c->move_vec.Vx = 0; + c->move_vec.Vy = 0; + break; + } - break; - - case AUTO_NAVI_Pitch: //自动视觉 - - c->move_vec.Vw =0; - c->move_vec.Vy =0; - c->move_vec.Vx =0 ; - - - -// if(abs_value(ctrl ->cmd_pick .posx )>20) -// { -// c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0); -// -// } -// else if(abs_value(ctrl ->cmd_pick .posx )<0.1) -// { -// c->move_vec.Vw =0; -// } -// else -// c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_LOW),(c->move_vec .Vw) ,0); - -// c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw); - -// -// c->vofa_send[0]=c->move_vec.Vw; -// c->vofa_send[1]=-ctrl->cmd_pick .posx; -// - - - - - break ; - - - - } - break ; - -} - - - //电机速度限幅 - - abs_limit_fp(&c->move_vec.Vx,6000.0f); - - abs_limit_fp(&c->move_vec.Vy,6000.0f); - - abs_limit_fp(&c->move_vec.Vw,6000.0f); + // 电机速度限幅 + abs_limit_fp(&c->move_vec.Vx, 6000.0f); + abs_limit_fp(&c->move_vec.Vy, 6000.0f); + abs_limit_fp(&c->move_vec.Vw, 6000.0f); Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw); diff --git a/User/Module/up.c b/User/Module/up.c index df7dcd1..ad0209e 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -331,7 +331,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) case AUTO: switch(c-> CMD_mode) { - case AUTO_NAVI_Pitch: + case AUTO_MID360_Pitch: u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos); if (u->PitchContext .PitchState ==PITCH_PREPARE) @@ -344,7 +344,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) Pitch_Process(u,out); break ; - case AUTO_NAVI: + case AUTO_MID360: u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; u->PitchContext .PitchState = PITCH_PREPARE; @@ -430,6 +430,8 @@ return 0; int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) { static int common_speed_flag=0;//是否共速 + static int common_speed_reverse_flag=0;//是否共速 + switch (u->DribbleContext.DribbleState) { @@ -465,6 +467,7 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP; + } } @@ -472,20 +475,24 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) break; case DRIBBLE_PROCESS_UP: - common_speed_flag =0; + common_speed_flag =0; - if((u->motorfeedback .DJmotor_feedback [0].rpm<-2000)&& - (u->motorfeedback .DJmotor_feedback [1].rpm>2000)&& - (u->motorfeedback .DJmotor_feedback [2].rpm>2000) + if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&& + (u->motorfeedback .DJmotor_feedback [1].rpm<-500)&& + (u->motorfeedback .DJmotor_feedback [2].rpm<-500) ){ - - if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){ + common_speed_reverse_flag=1; + + } + if(common_speed_reverse_flag){ + if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){ u->DribbleContext .DribbleState=DRIBBLE_DONE; RELAY1_TOGGLE(0); //关闭气缸 - } - } + } + } break ; case DRIBBLE_DONE: + common_speed_reverse_flag=0; for(int i=0;i<3;i++){ u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0 } diff --git a/User/device/cmd.c b/User/device/cmd.c index eec5297..35e7d0c 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -99,17 +99,17 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){ switch(cmd->cmd_status){ case MID: - cmd->cmd_MID360.posx = n->navi.vx; - cmd->cmd_MID360.posy = n->navi.vy; - cmd->cmd_MID360.posw = n->navi.wz; + cmd->cmd_MID360.posx = n->MID360.vx; + cmd->cmd_MID360.posy = n->MID360.vy; + cmd->cmd_MID360.posw = n->MID360.wz; - cmd->pos =n->navi .pos ; + cmd->pos =n->MID360 .pos ; break; - case PICK : - cmd ->cmd_pick .posx =n->pick .posx ; - cmd ->cmd_pick .posy =n->pick .posy ; - cmd ->cmd_pick .posw =n->pick .posw ; + case VISION : + cmd ->cmd_pick .posx =n->camera.data1 ; + cmd ->cmd_pick .posy =n->camera.data2 ; + cmd ->cmd_pick .posw =n->camera.data3 ; break; @@ -153,11 +153,11 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) { { cmd ->CMD_CtrlType =AUTO; - if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达 + if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360; //左中，右中，雷达 if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式 - if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_NAVI_Pitch; //左中，右下，视觉 + if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_MID360_Pitch; //左中，右下，视觉 } else if(rc->dr16.sw_l==CMD_SW_DOWN) @@ -191,9 +191,12 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){ if(cmd ->CMD_CtrlType ==AUTO){ /*自动下的*/ + if (rc->LD.key_C ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Adjust; + else { + if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =Normal; - else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI_Pitch; - else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_NAVI; + else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Pitch; + else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_MID360; else cmd ->CMD_mode =Normal ; } @@ -205,3 +208,4 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){ else cmd ->CMD_mode =Normal; } } + diff --git a/User/device/cmd.h b/User/device/cmd.h index 52d5703..6f77a9a 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -8,7 +8,7 @@ #include #define MID (0x09) -#define PICK (0x06) +#define VISION (0x02) #define NUC (0x08) typedef enum{ @@ -23,8 +23,9 @@ typedef enum{ Normal, //无模式 - AUTO_NAVI, - AUTO_NAVI_Pitch, + AUTO_MID360, + AUTO_MID360_Pitch, + AUTO_MID360_Adjust,//雷达调整 UP_Adjust,//上层调整 @@ -47,18 +48,13 @@ typedef struct { fp32 angle; char flag; - }navi; + }MID360; struct { - fp32 posx; - fp32 posy; - fp32 posw; - }pick; - struct - { - fp32 angle; - }sick_cali; - + fp32 data1; + fp32 data2; + fp32 data3; + }camera;//相机 } CMD_NUC_t; /* 拨杆位置 */ diff --git a/User/device/nuc.c b/User/device/nuc.c index 57fce4c..49435ba 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -32,6 +32,7 @@ int8_t NUC_StartReceiving() { int8_t NUC_StartSending(fp32 *data) { + union { float x[4]; @@ -81,33 +82,34 @@ int8_t NUC_RawParse(CMD_NUC_t *n){ n->ctrl_status =nucbuf[2]; switch (n->status_fromnuc) { -// case MID://控制帧0x09 -// /* 协议格式 -// 0xFF HEAD -// 0x09 控制帧 -// 0x01 相机帧 -// vx fp32 -// vy fp32 -// wz fp32 -// 0xFE TAIL -// */ -//// if(nucbuf[15]!=TAIL)goto error; -// instance.data[3] = nucbuf[6]; -// instance.data[2] = nucbuf[5]; -// instance.data[1] = nucbuf[4]; -// instance.data[0] = nucbuf[3]; -// n->navi.vx = instance.x[0]; // -// instance.data[7] = nucbuf[10]; -// instance.data[6] = nucbuf[9]; -// instance.data[5] = nucbuf[8]; -// instance.data[4] = nucbuf[7]; -// n->navi.vy = instance.x[1];// -// instance.data[11] = nucbuf[14]; -// instance.data[10] = nucbuf[13]; -// instance.data[9] = nucbuf[12]; -// instance.data[8] = nucbuf[11]; -// n->navi.wz = instance.x[2];// -// break; + case VISION://控制帧0x02 + /* 协议格式 + 0xFF HEAD + 0x02 控制帧 + 0x01 相机帧 + vx fp32 + vy fp32 + wz fp32 + 0xFE TAIL + */ + + + instance.data[3] = nucbuf[3]; + instance.data[2] = nucbuf[4]; + instance.data[1] = nucbuf[5]; + instance.data[0] = nucbuf[6]; + n->camera .data1 = instance.x[0]; // + instance.data[7] = nucbuf[7]; + instance.data[6] = nucbuf[8]; + instance.data[5] = nucbuf[9]; + instance.data[4] = nucbuf[10]; + n->camera .data2 = instance.x[1];// + instance.data[11] = nucbuf[11]; + instance.data[10] = nucbuf[12]; + instance.data[9] = nucbuf[13]; + instance.data[8] = nucbuf[14]; + n->camera .data3 = instance.x[2];// + break; case MID ://控制帧0x08 /* 协议格式 0xFF HEAD @@ -123,29 +125,29 @@ int8_t NUC_RawParse(CMD_NUC_t *n){ instance.data[2] = nucbuf[5]; instance.data[1] = nucbuf[4]; instance.data[0] = nucbuf[3]; - n->navi.vx = instance.x[0]; // + n->MID360.vx = instance.x[0]; // instance.data[7] = nucbuf[10]; instance.data[6] = nucbuf[9]; instance.data[5] = nucbuf[8]; instance.data[4] = nucbuf[7]; - n->navi.vy = instance.x[1];// + n->MID360.vy = instance.x[1];// instance.data[11] = nucbuf[14]; instance.data[10] = nucbuf[13]; instance.data[9] = nucbuf[12]; instance.data[8] = nucbuf[11]; - n->navi.wz = instance.x[2];// + n->MID360.wz = instance.x[2];// instance.data[15] = nucbuf[18]; instance.data[14] = nucbuf[17]; instance.data[13] = nucbuf[16]; instance.data[12] = nucbuf[15]; - n->navi.pos = instance.x[3];// + n->MID360.pos = instance.x[3];// instance.data[19] = nucbuf[22]; instance.data[18] = nucbuf[21]; instance.data[17] = nucbuf[20]; instance.data[16] = nucbuf[19]; - n->navi.angle = instance.x[4];// + n->MID360.angle = instance.x[4];// - n->navi.flag = nucbuf[23];// + n->MID360.flag = nucbuf[23];// break; diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index cbe6d3d..8befda0 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -29,7 +29,7 @@ void Task_nuc(void *argument){ task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); #endif NUC_StartReceiving(); - NUC_StartSending(send); +// NUC_StartSending(NUC_send.send); if (NUC_WaitDmaCplt()){ NUC_RawParse(&cmd_fromnuc); }