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

改了运球，效果一般

Browse Source
This commit is contained in:
ZHAISHUI04 2025-05-10 15:25:41 +08:00
parent 0b0ba31a48
commit 6e780a2920
7 changed files with 105 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
MDK-ARM/R2.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel>
<nTsel>6</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@ -114,9 +114,14 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U00260035480000034E575152 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key>
@ -135,7 +140,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>DLGUARM</Key>
<Name></Name>
<Name>(105=-1,-1,-1,-1,0)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
@ -215,6 +220,26 @@
<WinNumber>1</WinNumber>
<ItemText>Nor_Vy</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>a</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>b</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>count,0x0A</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>count</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
@ -260,7 +285,7 @@
<pMultCmdsp></pMultCmdsp>
<DebugDescription>
<Enable>1</Enable>
<EnableFlashSeq>1</EnableFlashSeq>
<EnableFlashSeq>0</EnableFlashSeq>
<EnableLog>0</EnableLog>
<Protocol>2</Protocol>
<DbgClock>5000000</DbgClock>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

15
User/Algorithm/user_math.c
View File

@ -329,22 +329,18 @@ int abs_value(int num) {
* @param mistake 阈值
* @return 如果到达目标值返回 true，否则返回 false
*/
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}
/**
* @brief 读取标志位状态
/* * @brief 读取标志位状态
* @param flag 当前标志位的值（0 或 1）
* @return 返回状态值：0、1、2...
*/
int read_flag_state(uint8_t flag) {
static uint8_t last_flag = 0; // 保存上一次的标志位状态
static int state = 0; // 当前状态值
static int last_flag = 1; // 保存上一次的标志位状态
static int state = 1; // 当前状态值
if (flag == 1 && last_flag == 0) {
if (flag == 0 && last_flag == 1) {
// 标志位从 0 变为 1，状态值递增
state++;
}
@ -373,4 +369,7 @@ void normalize_vector(double x, double y, double *out_x, double *out_y) {
// 应用缩放并保持方向
*out_x = x * scale;
*out_y = y * scale;
}
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}

4
User/Algorithm/user_math.h
View File

@ -14,7 +14,6 @@
#include <math.h>
#include <stdbool.h>
#include "struct_typedef.h"
#define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f)
@ -160,8 +159,7 @@ uint8_t average(uint8_t arr[], uint8_t n);
int abs_value(int num);
float expo_map(float value, float expo_factor) ;
bool is_reached(float current, float target, float mistake) ;
int read_flag_state(uint8_t flag) ;
void normalize_vector(double x, double y, double *out_x, double *out_y) ;
bool is_reached(float current, float target, float mistake) ;
#endif

10
User/Module/config.c
View File

@ -59,11 +59,11 @@ static const ConfigParam_t param ={
},
.Dribble_M3508_speed_param={ //三摩擦速度环
.p = 5.0f,
.i = 0.3f,
.p = 30.0f,
.i = 0.45f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
.i_limit = 3000.0f,
.out_limit = 5000.0f,
},
.Dribble_translate_M3508_speed_param={//平移用3508速度环
.p = 5.0f,
@ -100,7 +100,7 @@ static const ConfigParam_t param ={
.m3508_init_angle = 50,
.m3508_translate_angle = -930,
.m3508_dribble_Reverse_speed=-3000,
. m3508_dribble_speed= 3000, // 转动速度
. m3508_dribble_speed= 4500, // 转动速度
.m3508_dribble_init_speed=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置

76
User/Module/up.c
View File

@ -25,6 +25,7 @@
// 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
int count = 0; // 满足条件的计数
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
@ -206,8 +207,8 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
//电机控制 ，传进can
DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],-u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],-u->motor_target .Dribble_M3508_speed);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed ,
@ -243,7 +244,8 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
}
int a=0;
int b=0;
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
@ -279,7 +281,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
count = 0 ;
break;
case Pitch :
@ -303,12 +305,13 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
u->DribbleContext .DribbleState=DRIBBLE_PROCESS;
u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE;
}
//光电状态更新
u->DribbleContext .DribbleConfig .light_ball_flag =read_flag_state(HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin));
u->DribbleContext .DribbleConfig .light_3508_flag =read_flag_state(HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin));
u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
a=HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
b=HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
Dribble_Process(u,out);
}break ;
@ -356,7 +359,7 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
case PITCH_PULL_TRIGGER:
if(is_reached ( u->motorfeedback .DJmotor_feedback [4].total_angle,u->PitchContext .PitchConfig .m2006_trigger_angle,1.0f)) //当2006的总角度小于1，可以认为已经勾上,误差为1
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
}
@ -388,27 +391,41 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_translate_angle,1.0f))
{
u->DribbleContext .DribbleState=DRIBBLE_PROCESS;//到达位置后，转移状态
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
}
break;
case DRIBBLE_PROCESS:
case DRIBBLE_PROCESS_DOWN:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
if(is_reached(u->motorfeedback .DJmotor_feedback [0].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&&
is_reached(u->motorfeedback .DJmotor_feedback [1].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&&
is_reached(u->motorfeedback .DJmotor_feedback [2].rpm,u->motor_target.Dribble_M3508_speed,70.0f)
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm,
u->motorfeedback .DJmotor_feedback [2].rpm,
u->DribbleContext .DribbleConfig.m3508_dribble_speed,50.0f,50)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
RELAY1_TOGGLE(1); //速度达到后打开气缸
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测，反转
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP;
}
}
if(u->DribbleContext .DribbleConfig .light_ball_flag == 2){//球上升检测，速度给0，转移状态
u->DribbleContext .DribbleState=DRIBBLE_DONE;
}
break;
case DRIBBLE_PROCESS_UP:
if((u->motorfeedback .DJmotor_feedback [0].rpm<0)&&
(u->motorfeedback .DJmotor_feedback [1].rpm>0)&&
(u->motorfeedback .DJmotor_feedback [2].rpm>0)
){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){
u->DribbleContext .DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸
}
}
break ;
case DRIBBLE_DONE:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
/*标志位清零*/
@ -423,4 +440,27 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
}
return 0;
}
bool is_reached_multiple(float current1, float current2, float current3, float target, float mistake, int threshold) {
// static bool reached = false; // 是否已经满足条件
if (count >=50) {
return true; // 如果已经满足条件，始终返回 1
}
// 判断三个值是否都满足条件
bool all_reached = (fabs(current1 - target) < mistake) &&
(fabs(current2 -( -target)) < mistake) &&
(fabs(current3 - ( -target)) < mistake);
if (all_reached) {
count++; // 所有条件都满足，计数加 1
if (count >= threshold) {
return true;
}
} else {
count = 0; // 如果有一个不满足条件，计数清零
}
return false; // 未满足条件达到阈值，返回 0
}

8
User/Module/up.h
View File

@ -79,7 +79,8 @@ typedef enum {
DRIBBLE_PREPARE, // 准备阶段
DRIBBLE_START,
DRIBBLE_TRANSLATE, // 平移过程
DRIBBLE_PROCESS, // 运球过程
DRIBBLE_PROCESS_DOWN, // 运球过程，球下落
DRIBBLE_PROCESS_UP, // 运球过程，球上升
DRIBBLE_DONE // 运球结束
} DribbleState_t;
@ -95,8 +96,8 @@ typedef struct {
/*光电标志位，初始值均为0，触发为1*/
uint8_t light_3508_flag;//3508平移处的光电，0初始，1到位置
uint8_t light_ball_flag;//检测球的flag
int light_3508_flag;//3508平移处的光电，0初始，1到位置
int light_ball_flag;//检测球的flag
} DribbleConfig_t;
@ -262,5 +263,6 @@ int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
bool is_reached_multiple(float current1, float current2, float current3, float target, float mistake, int threshold) ;
#endif