LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

雷达增加发送，增加视觉备用，增加go电机限位

Browse Source
This commit is contained in:
ZHAISHUI04 2025-06-03 23:16:33 +08:00
parent 7499e75a50
commit 8ef2a0a348
7 changed files with 124 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Core/Src/main.c
View File

@ -80,6 +80,7 @@ int main(void)
/* MCU Configuration--------------------------------------------------------*/ /* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init(); HAL_Init();
/* USER CODE BEGIN Init */ /* USER CODE BEGIN Init */

120
User/Module/Chassis.c
View File

@ -151,87 +151,57 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
break; break;
case AUTO : //在自动模式下 case AUTO: // 自动模式
switch (c->chassis_ctrl.mode) {
switch(c->chassis_ctrl.mode ){
case AUTO_NAVI: //自动雷达 case AUTO_MID360: // 自动雷达
// //这套是全向轮的方向,一定要注意这里的xy方向 // 全向轮方向, 注意xy方向
c->move_vec.Vw =ctrl->cmd_MID360 .posw *1000 ; c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy =-ctrl->cmd_MID360.posy *1000 ; c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx =-ctrl->cmd_MID360.posx *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);
// //电机速度限幅
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) default:
{ c->move_vec.Vw = 0;
c->move_vec.Vw = c->move_vec.Vw * 0.8f; c->move_vec.Vx = 0;
c->move_vec.Vx = c->move_vec.Vx * 0.5f; c->move_vec.Vy = 0;
c->move_vec.Vy = c->move_vec.Vy * 0.5f; break;
} }
break; // 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f);
case AUTO_NAVI_Pitch: //自动视觉 abs_limit_fp(&c->move_vec.Vy, 6000.0f);
abs_limit_fp(&c->move_vec.Vw, 6000.0f);
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);
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw); Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);

27
User/Module/up.c
View File

@ -331,7 +331,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
case AUTO: case AUTO:
switch(c-> CMD_mode) 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); u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos);
if (u->PitchContext .PitchState ==PITCH_PREPARE) 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); Pitch_Process(u,out);
break ; break ;
case AUTO_NAVI: case AUTO_MID360:
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->PitchContext .PitchState = PITCH_PREPARE; u->PitchContext .PitchState = PITCH_PREPARE;
@ -430,6 +430,8 @@ return 0;
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{ {
static int common_speed_flag=0;//是否共速 static int common_speed_flag=0;//是否共速
static int common_speed_reverse_flag=0;//是否共速
switch (u->DribbleContext.DribbleState) { 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->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP; u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP;
} }
} }
@ -472,20 +475,24 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
break; break;
case DRIBBLE_PROCESS_UP: case DRIBBLE_PROCESS_UP:
common_speed_flag =0; common_speed_flag =0;
if((u->motorfeedback .DJmotor_feedback [0].rpm<-2000)&& if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [1].rpm>2000)&& (u->motorfeedback .DJmotor_feedback [1].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [2].rpm>2000) (u->motorfeedback .DJmotor_feedback [2].rpm<-500)
){ ){
common_speed_reverse_flag=1;
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){
}
if(common_speed_reverse_flag){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){
u->DribbleContext .DribbleState=DRIBBLE_DONE; u->DribbleContext .DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸 RELAY1_TOGGLE(0); //关闭气缸
} }
} }
break ; break ;
case DRIBBLE_DONE: case DRIBBLE_DONE:
common_speed_reverse_flag=0;
for(int i=0;i<3;i++){ for(int i=0;i<3;i++){
u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0 u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
} }

28
User/device/cmd.c
View File

@ -99,17 +99,17 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
switch(cmd->cmd_status){ switch(cmd->cmd_status){
case MID: case MID:
cmd->cmd_MID360.posx = n->navi.vx; cmd->cmd_MID360.posx = n->MID360.vx;
cmd->cmd_MID360.posy = n->navi.vy; cmd->cmd_MID360.posy = n->MID360.vy;
cmd->cmd_MID360.posw = n->navi.wz; cmd->cmd_MID360.posw = n->MID360.wz;
cmd->pos =n->navi .pos ; cmd->pos =n->MID360 .pos ;
break; break;
case PICK : case VISION :
cmd ->cmd_pick .posx =n->pick .posx ; cmd ->cmd_pick .posx =n->camera.data1 ;
cmd ->cmd_pick .posy =n->pick .posy ; cmd ->cmd_pick .posy =n->camera.data2 ;
cmd ->cmd_pick .posw =n->pick .posw ; cmd ->cmd_pick .posw =n->camera.data3 ;
break; break;
@ -153,11 +153,11 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
{ {
cmd ->CMD_CtrlType =AUTO; 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_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) 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(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; 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_UP) cmd ->CMD_mode =AUTO_MID360_Pitch;
else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_NAVI; else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_MID360;
else cmd ->CMD_mode =Normal ; 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; else cmd ->CMD_mode =Normal;
} }
} }

22
User/device/cmd.h
View File

@ -8,7 +8,7 @@
#include <string.h> #include <string.h>
#define MID (0x09) #define MID (0x09)
#define PICK (0x06) #define VISION (0x02)
#define NUC (0x08) #define NUC (0x08)
typedef enum{ typedef enum{
@ -23,8 +23,9 @@ typedef enum{
Normal, //无模式 Normal, //无模式
AUTO_NAVI, AUTO_MID360,
AUTO_NAVI_Pitch, AUTO_MID360_Pitch,
AUTO_MID360_Adjust,//雷达调整
UP_Adjust,//上层调整 UP_Adjust,//上层调整
@ -47,18 +48,13 @@ typedef struct {
fp32 angle; fp32 angle;
char flag; char flag;
}navi; }MID360;
struct struct
{ {
fp32 posx; fp32 data1;
fp32 posy; fp32 data2;
fp32 posw; fp32 data3;
}pick; }camera;//相机
struct
{
fp32 angle;
}sick_cali;
} CMD_NUC_t; } CMD_NUC_t;
/* 拨杆位置 */ /* 拨杆位置 */

68
User/device/nuc.c
View File

@ -32,6 +32,7 @@ int8_t NUC_StartReceiving() {
int8_t NUC_StartSending(fp32 *data) { int8_t NUC_StartSending(fp32 *data) {
union union
{ {
float x[4]; float x[4];
@ -81,33 +82,34 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
n->ctrl_status =nucbuf[2]; n->ctrl_status =nucbuf[2];
switch (n->status_fromnuc) switch (n->status_fromnuc)
{ {
// case MID://控制帧0x09 case VISION://控制帧0x02
// /* 协议格式 /* 协议格式
// 0xFF HEAD 0xFF HEAD
// 0x09 控制帧 0x02 控制帧
// 0x01 相机帧 0x01 相机帧
// vx fp32 vx fp32
// vy fp32 vy fp32
// wz fp32 wz fp32
// 0xFE TAIL 0xFE TAIL
// */ */
//// if(nucbuf[15]!=TAIL)goto error;
// instance.data[3] = nucbuf[6];
// instance.data[2] = nucbuf[5]; instance.data[3] = nucbuf[3];
// instance.data[1] = nucbuf[4]; instance.data[2] = nucbuf[4];
// instance.data[0] = nucbuf[3]; instance.data[1] = nucbuf[5];
// n->navi.vx = instance.x[0]; // instance.data[0] = nucbuf[6];
// instance.data[7] = nucbuf[10]; n->camera .data1 = instance.x[0]; //
// instance.data[6] = nucbuf[9]; instance.data[7] = nucbuf[7];
// instance.data[5] = nucbuf[8]; instance.data[6] = nucbuf[8];
// instance.data[4] = nucbuf[7]; instance.data[5] = nucbuf[9];
// n->navi.vy = instance.x[1];// instance.data[4] = nucbuf[10];
// instance.data[11] = nucbuf[14]; n->camera .data2 = instance.x[1];//
// instance.data[10] = nucbuf[13]; instance.data[11] = nucbuf[11];
// instance.data[9] = nucbuf[12]; instance.data[10] = nucbuf[12];
// instance.data[8] = nucbuf[11]; instance.data[9] = nucbuf[13];
// n->navi.wz = instance.x[2];// instance.data[8] = nucbuf[14];
// break; n->camera .data3 = instance.x[2];//
break;
case MID ://控制帧0x08 case MID ://控制帧0x08
/* 协议格式 /* 协议格式
0xFF HEAD 0xFF HEAD
@ -123,29 +125,29 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
instance.data[2] = nucbuf[5]; instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4]; instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[3]; instance.data[0] = nucbuf[3];
n->navi.vx = instance.x[0]; // n->MID360.vx = instance.x[0]; //
instance.data[7] = nucbuf[10]; instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9]; instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8]; instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7]; instance.data[4] = nucbuf[7];
n->navi.vy = instance.x[1];// n->MID360.vy = instance.x[1];//
instance.data[11] = nucbuf[14]; instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13]; instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12]; instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11]; instance.data[8] = nucbuf[11];
n->navi.wz = instance.x[2];// n->MID360.wz = instance.x[2];//
instance.data[15] = nucbuf[18]; instance.data[15] = nucbuf[18];
instance.data[14] = nucbuf[17]; instance.data[14] = nucbuf[17];
instance.data[13] = nucbuf[16]; instance.data[13] = nucbuf[16];
instance.data[12] = nucbuf[15]; instance.data[12] = nucbuf[15];
n->navi.pos = instance.x[3];// n->MID360.pos = instance.x[3];//
instance.data[19] = nucbuf[22]; instance.data[19] = nucbuf[22];
instance.data[18] = nucbuf[21]; instance.data[18] = nucbuf[21];
instance.data[17] = nucbuf[20]; instance.data[17] = nucbuf[20];
instance.data[16] = nucbuf[19]; 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; break;

2
User/task/nuc_task.c
View File

@ -29,7 +29,7 @@ void Task_nuc(void *argument){
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
#endif #endif
NUC_StartReceiving(); NUC_StartReceiving();
NUC_StartSending(send); // NUC_StartSending(NUC_send.send);
if (NUC_WaitDmaCplt()){ if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc); NUC_RawParse(&cmd_fromnuc);
} }