diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 551071d..55731e3 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -240,6 +240,11 @@
1
count
+
+ 17
+ 1
+ raw_rx1,0x0A
+
0
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index acdf706..5c9f8b8 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 c2b6843..ccce2e5 100644
--- a/User/Algorithm/user_math.c
+++ b/User/Algorithm/user_math.c
@@ -372,4 +372,23 @@ void normalize_vector(double x, double y, double *out_x, double *out_y) {
}
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
+}
+bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) {
+ static int count = 0; // 满足条件的计数
+
+ if (count >= threshold) {
+ count=0;//重置
+ return true; // 如果已经满足条件，返回 1
+
+ }
+
+ // 判断三个值是否都满足条件
+ bool all_reached = (fabs(current1 - target1) < mistake) &&
+ (fabs(current2 - target2) < mistake) &&
+ (fabs(current3 - target3) < mistake);
+
+ if (all_reached) {
+ count++; // 所有条件都满足，计数加 1
+ }
+ return false; // 未满足条件达到阈值，返回 0
}
\ No newline at end of file
diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h
index ad77fba..dd4d8ab 100644
--- a/User/Algorithm/user_math.h
+++ b/User/Algorithm/user_math.h
@@ -160,6 +160,9 @@ uint8_t average(uint8_t arr[], uint8_t n);
int abs_value(int num);
float expo_map(float value, float expo_factor) ;
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) ;
+bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
+
#endif
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index 3be8a8e..bbcc4ec 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -88,10 +88,15 @@ void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆
normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
- 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;//左前
+// 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;//左前
+ c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw ;//右前
+ c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw ;//右后
+ c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw ;//左后
+ c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw ;//左前
+
}
diff --git a/User/Module/config.c b/User/Module/config.c
index 466d4f8..82ef92b 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -99,8 +99,8 @@ static const ConfigParam_t param ={
.dribble_flag=0,
.m3508_init_angle = 50,
.m3508_translate_angle = -930,
- .m3508_dribble_Reverse_speed=-3000,
- . m3508_dribble_speed= 4500, // 转动速度
+ .m3508_dribble_Reverse_speed=-3500,
+ . m3508_dribble_speed= 4000, // 转动速度
.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 2bd22fa..ceaeddf 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -25,7 +25,6 @@
// 定义继电器控制
#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,9 +205,9 @@ int8_t GO_SendData(UP_t *u, float pos, float limit)
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,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]);
+ DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]);
+ DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed ,
@@ -271,17 +270,18 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
is_pitch=0;
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
-
+ u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
/*运球*/
RELAY1_TOGGLE(0);//关闭气缸
- u->motor_target.Dribble_M3508_speed=0;
+ for(int i=0;i<3;i++){
+ u->motor_target.Dribble_M3508_speed[i]=0;
+ }
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
- count = 0 ;
break;
case Pitch :
@@ -384,11 +384,11 @@ return 0;
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
-
+ static int common_speed_flag=0;//是否共速
+
switch (u->DribbleContext.DribbleState) {
case DRIBBLE_TRANSLATE:
- 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_DOWN;//到达位置后，转移状态
@@ -396,16 +396,27 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
break;
case DRIBBLE_PROCESS_DOWN:
- u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+ u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+ u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+ u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+
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)
+ u->motor_target.Dribble_M3508_speed [0],
+ u->motor_target.Dribble_M3508_speed [1],
+ u->motor_target.Dribble_M3508_speed[2],
+ 50.0f,50)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
-
+ common_speed_flag =1;
+ }
+ if(common_speed_flag){
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[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
+ u->motor_target.Dribble_M3508_speed[1]=-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;
}
@@ -427,7 +438,10 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
}
break ;
case DRIBBLE_DONE:
- u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
+ for(int i=0;i<3;i++){
+ u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
+ }
+
/*标志位清零*/
u->DribbleContext.DribbleConfig.light_ball_flag=0;
u->DribbleContext.DribbleConfig.light_3508_flag=0;
@@ -440,27 +454,5 @@ 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 120dec9..f06865c 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -199,10 +199,11 @@ typedef struct{
fp32 Pitch_M2006_angle;
fp32 go_shoot;
- fp32 Dribble_M3508_speed;//运球转速
+ fp32 Dribble_M3508_speed[3];//运球转速
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
fp32 Shoot_Pitch_angle;
+
}motor_target;
struct{
@@ -263,6 +264,5 @@ 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
diff --git a/User/device/can_use.c b/User/device/can_use.c
index 357db27..4010af2 100644
--- a/User/device/can_use.c
+++ b/User/device/can_use.c
@@ -56,14 +56,31 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
feedback->torque_current =
raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
}
-static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,
+static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id,
const uint8_t *raw) {
- if (feedback == NULL || raw == NULL) return;
- feedback->raw_dis[0] = (uint16_t)((raw[0] << 8) | raw[1]);
- feedback->raw_dis[1] = (uint16_t)((raw[2] << 8) | raw[3]);
- feedback->raw_dis[2] = (uint16_t)((raw[4] << 8) | raw[5]);
- feedback->raw_dis[3]= (uint16_t)((raw[6] << 8) | raw[7]);
-
+ if (feedback == NULL || raw == NULL) return;
+ switch (id)
+ {
+ case CAN_SICK_ID_1:
+ feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);
+
+ break ;
+
+ case CAN_SICK_ID_2:
+ feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]);
+
+ break ;
+
+ case CAN_SICK_ID_3:
+ feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]);
+
+ break ;
+
+ case CAN_SICK_ID_4:
+ feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]);
+
+ break ;
+ }
}
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) {
@@ -366,7 +383,19 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index);
break;
-
+ case CAN_SICK_ID_1:
+ CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data);
+ break;
+ case CAN_SICK_ID_2:
+ CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data);
+ break;
+ case CAN_SICK_ID_3:
+ CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data);
+ break;
+ case CAN_SICK_ID_4:
+ CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data);
+
+ break;
default:
break;
}
@@ -396,10 +425,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data);
break;
- case CAN_SICK_ID:
- // 存储消息到sickfed结构体中
- CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
- break;
+
case CAN_Encoder_ID:
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
can->recive_flag |= 1 << 10;
diff --git a/User/device/can_use.h b/User/device/can_use.h
index 8bdfe47..066ded2 100644
--- a/User/device/can_use.h
+++ b/User/device/can_use.h
@@ -18,7 +18,10 @@ typedef enum {
CAN_UP_M3508_M5_ID = 0x205, /* 1 */
CAN_UP_M3508_M6_ID = 0x206, /* 2 */
- CAN_SICK_ID=0x301,
+ CAN_SICK_ID_1=0x301,
+ CAN_SICK_ID_2=0x302,
+ CAN_SICK_ID_3=0x303,
+ CAN_SICK_ID_4=0x304,
CAN_Encoder_ID=0x01,
@@ -121,7 +124,7 @@ typedef struct {
/* sick反馈信息 */
typedef struct {
uint16_t raw_dis[4];
- int changed_dis[4];
+
}CAN_SickFeedback_t;
/* encoder编码器反馈信息 */
typedef struct {
@@ -197,5 +200,6 @@ void CAN_Encoder_Control(CAN_t *can);
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) ;
-
+static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id,
+ const uint8_t *raw) ;
#endif