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> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel> <nTsel>6</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@ -114,9 +114,14 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <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> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key> <Key>ARMRTXEVENTFLAGS</Key>
@ -135,7 +140,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name></Name> <Name>(105=-1,-1,-1,-1,0)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -215,6 +220,26 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>Nor_Vy</ItemText> <ItemText>Nor_Vy</ItemText>
</Ww> </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> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -260,7 +285,7 @@
<pMultCmdsp></pMultCmdsp> <pMultCmdsp></pMultCmdsp>
<DebugDescription> <DebugDescription>
<Enable>1</Enable> <Enable>1</Enable>
<EnableFlashSeq>1</EnableFlashSeq> <EnableFlashSeq>0</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>5000000</DbgClock> <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 阈值 * @param mistake 阈值
* @return 如果到达目标值返回 true，否则返回 false * @return 如果到达目标值返回 true，否则返回 false
*/ */
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}
/** /* * @brief 读取标志位状态
* @brief 读取标志位状态
* @param flag 当前标志位的值（0 或 1） * @param flag 当前标志位的值（0 或 1）
* @return 返回状态值：0、1、2... * @return 返回状态值：0、1、2...
*/ */
int read_flag_state(uint8_t flag) { int read_flag_state(uint8_t flag) {
static uint8_t last_flag = 0; // 保存上一次的标志位状态 static int last_flag = 1; // 保存上一次的标志位状态
static int state = 0; // 当前状态值 static int state = 1; // 当前状态值
if (flag == 1 && last_flag == 0) { if (flag == 0 && last_flag == 1) {
// 标志位从 0 变为 1，状态值递增 // 标志位从 0 变为 1，状态值递增
state++; state++;
} }
@ -374,3 +370,6 @@ void normalize_vector(double x, double y, double *out_x, double *out_y) {
*out_x = x * scale; *out_x = x * scale;
*out_y = y * 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 <math.h>
#include <stdbool.h> #include <stdbool.h>
#include "struct_typedef.h" #include "struct_typedef.h"
#define M_DEG2RAD_MULT (0.01745329251f) #define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f) #define M_RAD2DEG_MULT (57.2957795131f)
@ -160,8 +159,7 @@ uint8_t average(uint8_t arr[], uint8_t n);
int abs_value(int num); int abs_value(int num);
float expo_map(float value, float expo_factor) ; float expo_map(float value, float expo_factor) ;
bool is_reached(float current, float target, float mistake) ;
int read_flag_state(uint8_t flag) ; int read_flag_state(uint8_t flag) ;
void normalize_vector(double x, double y, double *out_x, double *out_y) ; void normalize_vector(double x, double y, double *out_x, double *out_y) ;
bool is_reached(float current, float target, float mistake) ;
#endif #endif

10
User/Module/config.c
View File

@ -59,11 +59,11 @@ static const ConfigParam_t param ={
}, },
.Dribble_M3508_speed_param={ //三摩擦速度环 .Dribble_M3508_speed_param={ //三摩擦速度环
.p = 5.0f, .p = 30.0f,
.i = 0.3f, .i = 0.45f,
.d = 0.0f, .d = 0.0f,
.i_limit = 2000.0f, .i_limit = 3000.0f,
.out_limit = 3000.0f, .out_limit = 5000.0f,
}, },
.Dribble_translate_M3508_speed_param={//平移用3508速度环 .Dribble_translate_M3508_speed_param={//平移用3508速度环
.p = 5.0f, .p = 5.0f,
@ -100,7 +100,7 @@ static const ConfigParam_t param ={
.m3508_init_angle = 50, .m3508_init_angle = 50,
.m3508_translate_angle = -930, .m3508_translate_angle = -930,
.m3508_dribble_Reverse_speed=-3000, .m3508_dribble_Reverse_speed=-3000,
. m3508_dribble_speed= 3000, // 转动速度 . m3508_dribble_speed= 4500, // 转动速度
.m3508_dribble_init_speed=0, .m3508_dribble_init_speed=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置 .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) #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) 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 //电机控制 ，传进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,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,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,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] , DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle , &u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed , &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) 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->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态 u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
count = 0 ;
break; break;
case Pitch : 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){ 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_ball_flag =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_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); Dribble_Process(u,out);
}break ; }break ;
@ -356,7 +359,7 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
case PITCH_PULL_TRIGGER: 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; 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;//平行移动 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)) 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; break;
case DRIBBLE_PROCESS: case DRIBBLE_PROCESS_DOWN:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed; 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)&& if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
is_reached(u->motorfeedback .DJmotor_feedback [1].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&& u->motorfeedback .DJmotor_feedback [1].rpm,
is_reached(u->motorfeedback .DJmotor_feedback [2].rpm,u->motor_target.Dribble_M3508_speed,70.0f) 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){//球下落检测，反转 if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测，反转
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; 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; 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: case DRIBBLE_DONE:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0 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; 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_PREPARE, // 准备阶段
DRIBBLE_START, DRIBBLE_START,
DRIBBLE_TRANSLATE, // 平移过程 DRIBBLE_TRANSLATE, // 平移过程
DRIBBLE_PROCESS, // 运球过程 DRIBBLE_PROCESS_DOWN, // 运球过程，球下落
DRIBBLE_PROCESS_UP, // 运球过程，球上升
DRIBBLE_DONE // 运球结束 DRIBBLE_DONE // 运球结束
} DribbleState_t; } DribbleState_t;
@ -95,8 +96,8 @@ typedef struct {
/*光电标志位，初始值均为0，触发为1*/ /*光电标志位，初始值均为0，触发为1*/
uint8_t light_3508_flag;//3508平移处的光电，0初始，1到位置 int light_3508_flag;//3508平移处的光电，0初始，1到位置
uint8_t light_ball_flag;//检测球的flag int light_ball_flag;//检测球的flag
} DribbleConfig_t; } 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 Dribble_Process(UP_t *u, CAN_Output_t *out);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c); 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 #endif