From 2e0c3649139d2da66e6cb5013ae26e76bcc7e05a 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?=E6=95=B4=E4=BD=93=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Core/Src/main.c | 1 +
MDK-ARM/R2.uvoptx | 5 ++
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 +-
8 files changed, 129 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/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 56ffa23..8c8a165 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -215,6 +215,11 @@
1
rc_ctrl,0x0A
+
+ 38
+ 1
+ cmd_fromnuc
+
0
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index 434ac0f..64281ab 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -172,87 +172,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 50969eb..8f2f7b4 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 03ece18..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);
}