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-07 14:04:51 +08:00
parent 410d62198f
commit 52668517bd
12 changed files with 117 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Src/main.c
View File

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

5
MDK-ARM/R2.uvoptx
View File

@ -205,6 +205,11 @@
<WinNumber>1</WinNumber>
<ItemText>can_out</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>\\R2\../User/task/up_task.c\UP.motorfeedback.DJmotor_feedback</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

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

Binary file not shown.

11
User/Algorithm/user_math.c
View File

@ -321,3 +321,14 @@ int abs_value(int num) {
return -num; // 如果数是负的，返回它的相反数
}
}
/**
* @brief 判断是否到达目标值
* @param current 当前值
* @param target 目标值
* @param mistake 阈值
* @return 如果到达目标值返回 true，否则返回 false
*/
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}

2
User/Algorithm/user_math.h
View File

@ -160,4 +160,6 @@ 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) ;
#endif

27
User/Module/Chassis.c
View File

@ -22,7 +22,6 @@
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/
//此处源代码处做了pid的reset 待添加
c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ;
c->chassis_ctrl.mode =ctrl->CMD_mode ;
@ -85,20 +84,15 @@ 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) //底盘逆运动学的解算
{
c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw;//右前
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
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;//左前
}
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
//{
// uint16_t xArrive = 0, yArrive = 0;
// xArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
// yArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
// if(xArrive && yArrive) return true;
// else return false;
//}
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
{
@ -215,11 +209,12 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
//电机速度限幅
abs_limit_fp(&c->move_vec.Vx,8000.0f);
abs_limit_fp(&c->move_vec.Vx,6000.0f);
abs_limit_fp(&c->move_vec.Vy,8000.0f);
abs_limit_fp(&c->move_vec.Vy,6000.0f);
abs_limit_fp(&c->move_vec.Vw,6000.0f);
abs_limit_fp(&c->move_vec.Vw,8000.0f);
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);

15
User/Module/config.c
View File

@ -37,10 +37,10 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f,
},
.Pitch_M2006_angle_param = {
.p = 25.0f,
.p = 400.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.d = 3.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.Pitch_M2006_speed_param={
@ -97,11 +97,10 @@ static const ConfigParam_t param ={
/*运球*/
.DribbleConfig_Config = {
.m3508_init_angle = 50,
.m3508_high_angle = 1200,
.go2_init_angle = 0,
.go2_flip_angle = -250,
.flip_timing = 200,
.go2_release_threshold = -550.0f,
.m3508_translate_angle = 1200,
.m3508_dribble_Reverse_speed=-3000,
. m3508_dribble_speed= 3000, // 转动速度
},
/*投球*/
.PitchConfig_Config = {

215
User/Module/up.c
View File

@ -36,16 +36,16 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param ));
PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param ));
PID_init (&u->pid .Dribble_M3508_speed ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param ));
for(int i=0;i<3;i++){
PID_init (&u->pid .Dribble_M3508_speed[i] ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param ));
}
u->go_cmd =u->param ->go_cmd ;
// 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
u->DribbleContext .DribbleState = Dribble_PREPARE;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE;
u->DribbleContext .is_initialized = 1;
}
@ -197,10 +197,10 @@ 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,u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
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_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed ,
u->motor_target .Dribble_translate_M3508_angle );
@ -212,8 +212,10 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
// &u->pid .Pitch_M2006_angle ,
// &u->pid .Pitch_M2006_speed ,
// u->motor_target .Pitch_M2006_angle );
u->final_out .DJfinal_out [5]=PID_calc (&(u->pid .Pitch_Angle_M2006_speed ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle);
DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
(PID_calc (&(u->pid .Pitch_M2006_angle ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
);
GO_SendData(u,u->motor_target .go_shoot,5 );
@ -241,9 +243,12 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
if(u ==NULL) return 0;
static int is_pitch=1;
switch (c->CMD_CtrlType )
{
case RCcontrol: //在手动模式下
case RCcontrol: //在手动模式下
switch (c-> CMD_mode )
{
@ -259,15 +264,14 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
// /*运球*/
// u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
// u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ;
// u->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态
/*运球*/
RELAY1_TOGGLE(0);//关闭气缸
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
u->motor_target.Dribble_M3508_speed=0;
// RELAY1_TOGGLE (1);//夹球，0夹1开
// RELAY2_TOGGLE (0);//球，0接1收
//
break;
@ -282,29 +286,20 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break ;
case UP_Adjust:
u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
// u->PitchContext.PitchConfig.m2006_Screw_init = c->Vy;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
// u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
break ;
case Dribble:
{
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
if(u->DribbleContext.DribbleState== Dribble_PREPARE){
u->DribbleContext .DribbleState =STATE_GRAB_BALL;
if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
u->DribbleContext .DribbleState=DRIBBLE_PROCESS;
}
// Dribble_Process(u,out);
Dribble_Process(u,out);
}break ;
case Dribbl_transfer:
break ;
// Dribbl_transfer_a(u,out);
}
break;
default:
@ -319,7 +314,7 @@ return 0;
}
}
@ -334,10 +329,6 @@ return 0;
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{
// go1_position=u->PitchContext .PitchConfig .go1_init_position ;
// M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
switch(u->PitchContext .PitchState){
case PITCH_START:
@ -353,19 +344,15 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
case PITCH_PULL_TRIGGER:
if(u->motorfeedback .DJmotor_feedback [0].total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
if(is_reached ( u->motorfeedback .DJmotor_feedback [0].total_angle,u->PitchContext .PitchConfig .m2006_trigger_angle,1.0f)) //当2006的总角度小于1，可以认为已经勾上,误差为1
{
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
// u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
// u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
}
break ;
}
return 0;
@ -376,124 +363,38 @@ return 0;
int8_t Pitch_Adjust(UP_t *u, CAN_Output_t *out,CMD_t *c)
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
switch (u->DribbleContext.DribbleState) {
case DRIBBLE_TRANSLATE:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
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); //速度达到后打开气缸
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
}
break;
case DRIBBLE_DONE:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
break;
default:
// 处理未知状态
break;
}
return 0;
}
//int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
//{
//
// switch (u->DribbleContext.DribbleState) {
// case STATE_GRAB_BALL://开始
//
// RELAY1_TOGGLE (0);//夹球
//
// u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
// u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle;
//
// if((u->motorfeedback .M3508 .total_angle >400)) {
// RELAY2_TOGGLE (1);
//
//// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
////
//// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
//// }
//
// }
// break;
// case STATE_RELEASE_BALL:
// RELAY1_TOGGLE (1);//松球
//
//
//
//// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
//// u->motor_target.go_spin =-40; //恢复go2位置
//// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
//// }
// break;
// case STATE_CATCH_PREP:
//// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
//// u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
////
//// RELAY2_TOGGLE (0);//接球
//// }
// if(u->motorfeedback .M3508 .total_angle <51.0f){
// RELAY1_TOGGLE (0);//夹球，0夹1开
// u->DribbleContext .DribbleState = STATE_TRANSFER;
// }
//
// break;
//
//
//
// break ;
// default:
// break;
// }
// return 0;
//}
//void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
//{
// switch (u->DribbleContext.DribbleState) {
//
//
//
// case STATE_TRANSFER:
// if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
// {
//
// u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;
// }
//
//// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
//// {
//// RELAY1_TOGGLE (1);//夹球，0夹1开
////
//// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
//// {
//// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
//// }
//// }
//
//
// break ;
// case STATE_CATCH_DONE:
//
// RELAY1_TOGGLE (1);//夹球，0夹1开
// RELAY2_TOGGLE (0);//夹球，0接1收
// u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
// break;
// break;
// }
//}

26
User/Module/up.h
View File

@ -76,23 +76,20 @@ typedef struct {
/*运球*/
typedef enum {
Dribble_PREPARE,
STATE_GRAB_BALL, // 夹球升起阶段
STATE_RELEASE_BALL, // 释放球体
STATE_CATCH_PREP, // 接球准备
STATE_CATCH_BALL, // 接球动作
STATE_TRANSFER, //转移球
STATE_CATCH_DONE //完成
DRIBBLE_PREPARE, // 准备阶段
DRIBBLE_START,
DRIBBLE_TRANSLATE, // 平移过程
DRIBBLE_PROCESS, // 运球过程
DRIBBLE_DONE // 运球结束
} DribbleState_t;
/* 参数配置结构体 */
typedef struct {
fp32 m3508_init_angle; // 3508初始角度
fp32 m3508_high_angle; // 3508升起角度
fp32 go2_init_angle; // GO2初始角度
fp32 go2_flip_angle; // GO2翻转角度
fp32 flip_timing; // 翻转触发时机
fp32 go2_release_threshold; // 释放球
fp32 m3508_init_angle; // 平移前位置
fp32 m3508_translate_angle; // 平移后位置
fp32 m3508_dribble_speed; // 转动速度
fp32 m3508_dribble_Reverse_speed;
} DribbleConfig_t;
/* 状态机上下文 */
@ -211,7 +208,7 @@ typedef struct{
pid_type_def Pitch_Angle_M2006_speed;
pid_type_def Dribble_M3508_speed;//三摩擦用的速度环
pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环
pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
@ -252,6 +249,7 @@ int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle);
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_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);

2
User/device/can_use.c
View File

@ -76,7 +76,7 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
{
case 0x01:
feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24;
feedback->angle=feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f;
feedback->angle=(fp32)feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f;
break;
}
}

3
User/device/cmd.c
View File

@ -196,8 +196,9 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
}
else if(cmd ->CMD_CtrlType ==RCcontrol){
/*手动下的*/
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode =UP_Adjust;
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ;
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
else cmd ->CMD_mode =Normal;
}
}

6
User/task/error_detect_task.c
View File

@ -96,10 +96,10 @@ void Task_error_detect(void *argument){
//若所有设备在线，会不断翻转绿灯 存在设备离线则会红灯灯翻转
if(online_dev == ERROR_LIST_LENGHT)
{
// if(online_dev == ERROR_LIST_LENGHT)
// {
HAL_GPIO_TogglePin(GPIOH,LED_G_Pin);
}
// }
// else HAL_GPIO_TogglePin(GPIOH,LED_R_Pin);
for (int i = 0; i < ERROR_LIST_LENGHT; i++)