diff --git a/Core/Inc/main.h b/Core/Inc/main.h
index 9f65da1..9d6d8cb 100644
--- a/Core/Inc/main.h
+++ b/Core/Inc/main.h
@@ -75,11 +75,17 @@ void Error_Handler(void);
#define ACCL_INT_Pin GPIO_PIN_4
#define ACCL_INT_GPIO_Port GPIOC
#define ACCL_INT_EXTI_IRQn EXTI4_IRQn
+#define LIGHT_C_Pin GPIO_PIN_13
+#define LIGHT_C_GPIO_Port GPIOE
#define GYRO_INT_Pin GPIO_PIN_5
#define GYRO_INT_GPIO_Port GPIOC
#define GYRO_INT_EXTI_IRQn EXTI9_5_IRQn
-#define FlagForUpper_Pin GPIO_PIN_9
-#define FlagForUpper_GPIO_Port GPIOE
+#define LIGHT_A_Pin GPIO_PIN_9
+#define LIGHT_A_GPIO_Port GPIOE
+#define LIGHT_B_Pin GPIO_PIN_11
+#define LIGHT_B_GPIO_Port GPIOE
+#define RELAY_Pin GPIO_PIN_14
+#define RELAY_GPIO_Port GPIOE
#define GYRO_CS_Pin GPIO_PIN_0
#define GYRO_CS_GPIO_Port GPIOB
diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c
index 7b59a61..3fe1da3 100644
--- a/Core/Src/gpio.c
+++ b/Core/Src/gpio.c
@@ -61,7 +61,7 @@ void MX_GPIO_Init(void)
HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
- HAL_GPIO_WritePin(FlagForUpper_GPIO_Port, FlagForUpper_Pin, GPIO_PIN_SET);
+ HAL_GPIO_WritePin(RELAY_GPIO_Port, RELAY_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
@@ -92,12 +92,18 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+ /*Configure GPIO pins : PEPin PEPin */
+ GPIO_InitStruct.Pin = LIGHT_C_Pin|LIGHT_B_Pin;
+ GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+ HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
+
/*Configure GPIO pin : PtPin */
- GPIO_InitStruct.Pin = FlagForUpper_Pin;
+ GPIO_InitStruct.Pin = RELAY_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
- GPIO_InitStruct.Pull = GPIO_PULLUP;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
- HAL_GPIO_Init(FlagForUpper_GPIO_Port, &GPIO_InitStruct);
+ HAL_GPIO_Init(RELAY_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GYRO_CS_Pin;
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 557aee4..f48044b 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -80,7 +80,7 @@ int main(void)
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
- HAL_Init();
+ HAL_Init();
/* USER CODE BEGIN Init */
diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 19d1e8d..df637c7 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -208,7 +208,12 @@
11
1
- \\R2\../User/task/up_task.c\UP.motorfeedback.DJmotor_feedback
+ Nor_Vx
+
+
+ 12
+ 1
+ Nor_Vy
@@ -972,18 +977,6 @@
0
0
0
- ..\User\bsp\map.c
- map.c
- 0
- 0
-
-
- 7
- 56
- 1
- 0
- 0
- 0
..\User\bsp\bsp_can.c
bsp_can.c
0
@@ -991,7 +984,7 @@
7
- 57
+ 56
1
0
0
@@ -1003,7 +996,7 @@
7
- 58
+ 57
1
0
0
@@ -1015,7 +1008,7 @@
7
- 59
+ 58
5
0
0
@@ -1027,7 +1020,7 @@
7
- 60
+ 59
1
0
0
@@ -1039,7 +1032,7 @@
7
- 61
+ 60
1
0
0
@@ -1051,7 +1044,7 @@
7
- 62
+ 61
1
0
0
@@ -1071,7 +1064,7 @@
0
8
- 63
+ 62
1
0
0
@@ -1083,7 +1076,7 @@
8
- 64
+ 63
1
0
0
@@ -1095,7 +1088,7 @@
8
- 65
+ 64
1
0
0
@@ -1107,7 +1100,7 @@
8
- 66
+ 65
1
0
0
@@ -1119,7 +1112,7 @@
8
- 67
+ 66
1
0
0
@@ -1131,7 +1124,7 @@
8
- 68
+ 67
1
0
0
@@ -1143,7 +1136,7 @@
8
- 69
+ 68
1
0
0
@@ -1155,7 +1148,7 @@
8
- 70
+ 69
1
0
0
@@ -1167,7 +1160,7 @@
8
- 71
+ 70
1
0
0
@@ -1179,7 +1172,7 @@
8
- 72
+ 71
1
0
0
@@ -1199,7 +1192,7 @@
0
9
- 73
+ 72
1
0
0
@@ -1211,7 +1204,7 @@
9
- 74
+ 73
1
0
0
@@ -1223,7 +1216,7 @@
9
- 75
+ 74
1
0
0
@@ -1235,7 +1228,7 @@
9
- 76
+ 75
1
0
0
@@ -1247,7 +1240,7 @@
9
- 77
+ 76
1
0
0
@@ -1259,7 +1252,7 @@
9
- 78
+ 77
1
0
0
@@ -1279,7 +1272,7 @@
0
10
- 79
+ 78
1
0
0
@@ -1291,7 +1284,7 @@
10
- 80
+ 79
1
0
0
@@ -1303,7 +1296,7 @@
10
- 81
+ 80
1
0
0
@@ -1315,7 +1308,7 @@
10
- 82
+ 81
1
0
0
@@ -1327,7 +1320,7 @@
10
- 83
+ 82
1
0
0
@@ -1339,7 +1332,7 @@
10
- 84
+ 83
1
0
0
@@ -1351,7 +1344,7 @@
10
- 85
+ 84
1
0
0
@@ -1371,7 +1364,7 @@
0
11
- 86
+ 85
1
0
0
@@ -1383,7 +1376,7 @@
11
- 87
+ 86
1
0
0
@@ -1395,7 +1388,7 @@
11
- 88
+ 87
1
0
0
@@ -1415,7 +1408,7 @@
0
12
- 89
+ 88
1
0
0
@@ -1435,7 +1428,7 @@
0
13
- 90
+ 89
1
0
0
@@ -1447,7 +1440,7 @@
13
- 91
+ 90
1
0
0
@@ -1459,7 +1452,7 @@
13
- 92
+ 91
1
0
0
@@ -1471,7 +1464,7 @@
13
- 93
+ 92
1
0
0
diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx
index ee3c96d..2e3293a 100644
--- a/MDK-ARM/R2.uvprojx
+++ b/MDK-ARM/R2.uvprojx
@@ -1143,11 +1143,6 @@
1
..\User\bsp\pwm.c
-
- map.c
- 1
- ..\User\bsp\map.c
-
bsp_can.c
1
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index d54ecb2..d1b083c 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/R2.ioc b/R2.ioc
index 730c0b8..4b4230a 100644
--- a/R2.ioc
+++ b/R2.ioc
@@ -155,25 +155,28 @@ Mcu.Pin22=PD14
Mcu.Pin23=PA0-WKUP
Mcu.Pin24=PA4
Mcu.Pin25=PC4
-Mcu.Pin26=PC5
-Mcu.Pin27=PE9
-Mcu.Pin28=PA7
-Mcu.Pin29=PB0
+Mcu.Pin26=PE13
+Mcu.Pin27=PC5
+Mcu.Pin28=PE9
+Mcu.Pin29=PE11
Mcu.Pin3=PB3
-Mcu.Pin30=VP_CRC_VS_CRC
-Mcu.Pin31=VP_FREERTOS_VS_CMSIS_V2
-Mcu.Pin32=VP_SYS_VS_Systick
-Mcu.Pin33=VP_TIM4_VS_ClockSourceINT
-Mcu.Pin34=VP_TIM7_VS_ClockSourceINT
-Mcu.Pin35=VP_TIM10_VS_ClockSourceINT
-Mcu.Pin36=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
+Mcu.Pin30=PE14
+Mcu.Pin31=PA7
+Mcu.Pin32=PB0
+Mcu.Pin33=VP_CRC_VS_CRC
+Mcu.Pin34=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin35=VP_SYS_VS_Systick
+Mcu.Pin36=VP_TIM4_VS_ClockSourceINT
+Mcu.Pin37=VP_TIM7_VS_ClockSourceINT
+Mcu.Pin38=VP_TIM10_VS_ClockSourceINT
+Mcu.Pin39=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
Mcu.Pin4=PA14
Mcu.Pin5=PA13
Mcu.Pin6=PB7
Mcu.Pin7=PB6
Mcu.Pin8=PD0
Mcu.Pin9=PC11
-Mcu.PinsNb=37
+Mcu.PinsNb=40
Mcu.ThirdPartyNb=0
Mcu.UserConstants=
Mcu.UserName=STM32F407IGHx
@@ -285,12 +288,28 @@ PD14.GPIOParameters=GPIO_Label
PD14.GPIO_Label=Buzzer
PD14.Locked=true
PD14.Signal=S_TIM4_CH3
+PE11.GPIOParameters=GPIO_PuPd,GPIO_Label
+PE11.GPIO_Label=LIGHT_B
+PE11.GPIO_PuPd=GPIO_PULLDOWN
+PE11.Locked=true
+PE11.Signal=GPIO_Input
+PE13.GPIOParameters=GPIO_PuPd,GPIO_Label
+PE13.GPIO_Label=LIGHT_C
+PE13.GPIO_PuPd=GPIO_PULLDOWN
+PE13.Locked=true
+PE13.Signal=GPIO_Input
+PE14.GPIOParameters=GPIO_PuPd,GPIO_Label
+PE14.GPIO_Label=RELAY
+PE14.GPIO_PuPd=GPIO_PULLDOWN
+PE14.Locked=true
+PE14.Signal=GPIO_Output
PE9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label
-PE9.GPIO_Label=FlagForUpper
-PE9.GPIO_PuPd=GPIO_PULLUP
+PE9.GPIO_Label=LIGHT_A
+PE9.GPIO_PuPd=GPIO_PULLDOWN
PE9.Locked=true
PE9.PinState=GPIO_PIN_SET
-PE9.Signal=GPIO_Output
+PE9.Signal=SharedStack_PE9
+PE9.Stacked=true
PF6.GPIOParameters=GPIO_Speed,GPIO_Label
PF6.GPIO_Label=IMU_HEAT_PWM
PF6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
@@ -401,6 +420,9 @@ SH.S_TIM10_CH1.0=TIM10_CH1,PWM Generation1 CH1
SH.S_TIM10_CH1.ConfNb=1
SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3
SH.S_TIM4_CH3.ConfNb=1
+SH.SharedStack_PE9.0=GPIO_Output+0
+SH.SharedStack_PE9.1=GPIO_Input
+SH.SharedStack_PE9.ConfNb=2
SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_16
SPI1.CLKPhase=SPI_PHASE_2EDGE
SPI1.CLKPolarity=SPI_POLARITY_HIGH
diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c
index 49ab692..ba15259 100644
--- a/User/Algorithm/user_math.c
+++ b/User/Algorithm/user_math.c
@@ -332,3 +332,45 @@ int abs_value(int num) {
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}
+
+
+
+/**
+ * @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; // 当前状态值
+
+ if (flag == 1 && last_flag == 0) {
+ // 标志位从 0 变为 1，状态值递增
+ state++;
+ }
+
+ // 更新上一次的标志位状态
+ last_flag = flag;
+
+ return state;
+}
+// 归一化函数，将正方形坐标映射到单位圆
+void normalize_vector(double x, double y, double *out_x, double *out_y) {
+ // 处理原点情况
+ if (x == 0.0 && y == 0.0) {
+ *out_x = 0.0;
+ *out_y = 0.0;
+ return;
+ }
+
+ // 计算最大坐标绝对值和原始模长
+ const double abs_x = fabs(x);
+ const double abs_y = fabs(y);
+ const double s = fmax(abs_x, abs_y); // 最大坐标绝对值
+ const double r = sqrt(x*x + y*y); // 原始向量模长
+ const double scale = s / r; // 缩放因子
+
+ // 应用缩放并保持方向
+ *out_x = x * scale;
+ *out_y = y * scale;
+}
\ No newline at end of file
diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h
index 0f5ce0e..01c3df7 100644
--- a/User/Algorithm/user_math.h
+++ b/User/Algorithm/user_math.h
@@ -161,5 +161,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) ;
#endif
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index 0291a32..3be8a8e 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -82,15 +82,16 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
}
-void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
+void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算,
{
+ fp64 Nor_Vx,Nor_Vy;//归一化后的数据
+ normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
-
- c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
- c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
- c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
- c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw + c->pos088 .bmi088.gyro.z;//左前
+ c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
+ c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
+ c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
+ c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左前
}
diff --git a/User/Module/config.c b/User/Module/config.c
index a21b32e..6440431 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -37,7 +37,7 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f,
},
.Pitch_M2006_angle_param = {
- .p = 400.0f,
+ .p = 600.0f,
.i = 0.0f,
.d = 3.0f,
.i_limit = 2000.0f,
@@ -96,10 +96,16 @@ static const ConfigParam_t param ={
/*上层其他参数*/
/*运球*/
.DribbleConfig_Config = {
+ .dribble_flag=0,
.m3508_init_angle = 50,
- .m3508_translate_angle = 1200,
+ .m3508_translate_angle = -930,
.m3508_dribble_Reverse_speed=-3000,
. m3508_dribble_speed= 3000, // 转动速度
+ .m3508_dribble_init_speed=0,
+
+ .light_3508_flag=0,//3508平移处的光电，0初始，1到位置
+ .light_ball_flag=0,//检测球的flag
+
},
/*投球*/
@@ -109,7 +115,7 @@ static const ConfigParam_t param ={
.go1_init_position = -50,
.go1_release_threshold =-210,
.m2006_Screw_init=0,
- .Pitch_angle =57,
+ .Pitch_angle =56,
},
diff --git a/User/Module/up.c b/User/Module/up.c
index eb850c5..6ac4061 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -3,6 +3,15 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
+/*接线
+
+上层用到三个光电
+1.PE9 发射用 输出高电平 LIGHTA
+2.PE11 运球上3508 输出高电平 LIGHTB
+3.PE13 三摩擦检测球 输出高电平LIGHTC
+
+
+*/
#define GEAR_RATIO_2006 (36) // 2006减速比
#define GEAR_RATIO_3508 (19)
@@ -15,8 +24,7 @@
// 定义继电器控制
-#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
-#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
+#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
@@ -257,20 +265,19 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
/*投篮*/
if(is_pitch){
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
- is_pitch=0;
- }
- u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
+ is_pitch=0;
+ } //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
+ u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
/*运球*/
RELAY1_TOGGLE(0);//关闭气缸
-
- u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
u->motor_target.Dribble_M3508_speed=0;
-
+ u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
+ u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
break;
@@ -284,7 +291,8 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
Pitch_Process(u,out,c);
break ;
- case UP_Adjust:
+ case UP_Adjust: //测试用
+
u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
@@ -297,6 +305,10 @@ 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 .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));
+
Dribble_Process(u,out);
}break ;
@@ -344,7 +356,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 [0].total_angle,u->PitchContext .PitchConfig .m2006_trigger_angle,1.0f)) //当2006的总角度小于1，可以认为已经勾上,误差为1
+ if(is_reached ( u->motorfeedback .DJmotor_feedback [4].total_angle,u->PitchContext .PitchConfig .m2006_trigger_angle,1.0f)) //当2006的总角度小于1，可以认为已经勾上,误差为1
{
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
}
@@ -369,27 +381,41 @@ return 0;
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
-
+
switch (u->DribbleContext.DribbleState) {
case DRIBBLE_TRANSLATE:
- 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))
+ {
+ u->DribbleContext .DribbleState=DRIBBLE_PROCESS;//到达位置后，转移状态
+ }
break;
case DRIBBLE_PROCESS:
+
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)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
+
+ if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测，反转
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
+ }
+ }
+ if(u->DribbleContext .DribbleConfig .light_ball_flag == 2){//球上升检测，速度给0，转移状态
+ u->DribbleContext .DribbleState=DRIBBLE_DONE;
+
}
-
break;
case DRIBBLE_DONE:
- u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
-
+ u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
+ /*标志位清零*/
+ u->DribbleContext.DribbleConfig.light_ball_flag=0;
+ u->DribbleContext.DribbleConfig.light_3508_flag=0;
+
+
break;
default:
// 处理未知状态
diff --git a/User/Module/up.h b/User/Module/up.h
index 315be94..f2cdafb 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -85,11 +85,19 @@ typedef enum {
/* 参数配置结构体 */
typedef struct {
+ int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
+
fp32 m3508_init_angle; // 平移前位置
fp32 m3508_translate_angle; // 平移后位置
+ fp32 m3508_dribble_init_speed;
fp32 m3508_dribble_speed; // 转动速度
fp32 m3508_dribble_Reverse_speed;
+
+ /*光电标志位，初始值均为0，触发为1*/
+ uint8_t light_3508_flag;//3508平移处的光电，0初始，1到位置
+ uint8_t light_ball_flag;//检测球的flag
+
} DribbleConfig_t;
/* 状态机上下文 */
@@ -99,6 +107,8 @@ typedef struct {
uint8_t is_initialized;
+
+
} DribbleContext_t;
diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c
index 40f6485..28549a0 100644
--- a/User/task/chassis_task.c
+++ b/User/task/chassis_task.c
@@ -51,9 +51,7 @@ void Task_Chassis(void *argument)
uint32_t tick = osKernelGetTickCount();
-
- HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除
-
+
while(1)
{
#ifdef DEBUG