diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index df637c7..551071d 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -103,7 +103,7 @@
1
0
0
- 3
+ 6
@@ -114,9 +114,14 @@
- BIN\CMSIS_AGDI.dll
+ STLink\ST-LINKIII-KEIL_SWO.dll
+
+ 0
+ ST-LINKIII-KEIL_SWO
+ -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)
+
0
ARMRTXEVENTFLAGS
@@ -135,7 +140,7 @@
0
DLGUARM
-
+ (105=-1,-1,-1,-1,0)
0
@@ -215,6 +220,26 @@
1
Nor_Vy
+
+ 13
+ 1
+ a
+
+
+ 14
+ 1
+ b
+
+
+ 15
+ 1
+ count,0x0A
+
+
+ 16
+ 1
+ count
+
0
@@ -260,7 +285,7 @@
1
- 1
+ 0
0
2
5000000
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index d1b083c..acdf706 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c
index ba15259..c2b6843 100644
--- a/User/Algorithm/user_math.c
+++ b/User/Algorithm/user_math.c
@@ -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;
}
\ No newline at end of file
diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h
index 01c3df7..ad77fba 100644
--- a/User/Algorithm/user_math.h
+++ b/User/Algorithm/user_math.h
@@ -14,7 +14,6 @@
#include
#include
#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
diff --git a/User/Module/config.c b/User/Module/config.c
index 6440431..466d4f8 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -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到位置
diff --git a/User/Module/up.c b/User/Module/up.c
index 6ac4061..2bd22fa 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -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
+}
diff --git a/User/Module/up.h b/User/Module/up.h
index f2cdafb..120dec9 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -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