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

改运球，can1适配sick，sick的id为0x301-0x304

Browse Source
This commit is contained in:
ZHAISHUI04 2025-05-12 21:07:38 +08:00
parent 6e780a2920
commit 47ac0cf2a0
10 changed files with 113 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
MDK-ARM/R2.uvoptx
View File

@ -240,6 +240,11 @@
<WinNumber>1</WinNumber>
<ItemText>count</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>raw_rx1,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

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

Binary file not shown.

19
User/Algorithm/user_math.c
View File

@ -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
}

3
User/Algorithm/user_math.h
View File

@ -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

13
User/Module/Chassis.c
View File

@ -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 ;//左前
}

4
User/Module/config.c
View File

@ -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到位置

64
User/Module/up.c
View File

@ -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
}

4
User/Module/up.h
View File

@ -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

50
User/device/can_use.c
View File

@ -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;

10
User/device/can_use.h
View File

@ -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