From a3877e40fd33ef7d3555f36e0048b5bf332aa303 Mon Sep 17 00:00:00 2001 From: ws <1621320660@qq.com> Date: Sat, 14 Jun 2025 01:07:21 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9C=89=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MDK-ARM/.vscode/keil-assistant.log | 10 +++ MDK-ARM/.vscode/settings.json | 6 ++ MDK-ARM/.vscode/uv4.log | 6 +- MDK-ARM/.vscode/uv4.log.lock | 2 +- MDK-ARM/R1.uvoptx | 26 ++++++- User/lib/user_math.c | 62 +++++++++++----- User/lib/user_math.h | 7 +- User/module/ball.cpp | 112 +++++++++++++++++++---------- User/module/ball.hpp | 3 +- User/module/shoot.cpp | 11 ++- User/module/shoot.hpp | 20 +++--- User/task/ballTask.cpp | 77 ++++++++++---------- User/task/userTask.h | 2 +- 13 files changed, 228 insertions(+), 116 deletions(-) create mode 100644 MDK-ARM/.vscode/settings.json diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log index 8e93176..eccce91 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -20,3 +20,13 @@ [info] Log at : 2025/6/11|17:37:52|GMT+0800 +[info] Log at : 2025/6/12|17:09:40|GMT+0800 + +[info] Log at : 2025/6/12|21:20:00|GMT+0800 + +[info] Log at : 2025/6/13|11:22:38|GMT+0800 + +[info] Log at : 2025/6/13|19:51:19|GMT+0800 + +[info] Log at : 2025/6/13|19:56:03|GMT+0800 + diff --git a/MDK-ARM/.vscode/settings.json b/MDK-ARM/.vscode/settings.json new file mode 100644 index 0000000..57a9ad9 --- /dev/null +++ b/MDK-ARM/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "djimotor.h": "c", + "user_math.h": "c" + } +} \ No newline at end of file diff --git a/MDK-ARM/.vscode/uv4.log b/MDK-ARM/.vscode/uv4.log index 2d011b3..a7fed36 100644 --- a/MDK-ARM/.vscode/uv4.log +++ b/MDK-ARM/.vscode/uv4.log @@ -1,8 +1,8 @@ *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin' Build target 'R1' -compiling ballTask.cpp... +compiling user_math.c... linking... -Program Size: Code=29728 RO-data=1884 RW-data=284 ZI-data=33268 +Program Size: Code=30000 RO-data=1884 RW-data=284 ZI-data=33268 FromELF: creating hex file... "R1\R1.axf" - 0 Error(s), 0 Warning(s). -Build Time Elapsed: 00:00:06 +Build Time Elapsed: 00:00:05 diff --git a/MDK-ARM/.vscode/uv4.log.lock b/MDK-ARM/.vscode/uv4.log.lock index 5b7b866..bb95b32 100644 --- a/MDK-ARM/.vscode/uv4.log.lock +++ b/MDK-ARM/.vscode/uv4.log.lock @@ -1 +1 @@ -2025/6/9 20:40:44 \ No newline at end of file +2025/6/13 23:04:00 \ No newline at end of file diff --git a/MDK-ARM/R1.uvoptx b/MDK-ARM/R1.uvoptx index 40f82a0..1cf92bb 100644 --- a/MDK-ARM/R1.uvoptx +++ b/MDK-ARM/R1.uvoptx @@ -145,7 +145,7 @@ 0 ST-LINKIII-KEIL_SWO - -U-O142 -O2254 -SF10000 -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 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + -U00160029510000164E574E32 -O2254 -SF10000 -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 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) @@ -175,6 +175,26 @@ 1 goangle,0x0A + + 5 + 1 + ball,0x0A + + + 6 + 1 + jy,0x0A + + + 7 + 1 + angle1,0x0A + + + 8 + 1 + angle2,0x0A + 0 @@ -934,7 +954,7 @@ User/device - 1 + 0 0 0 0 @@ -1026,7 +1046,7 @@ User/module - 1 + 0 0 0 0 diff --git a/User/lib/user_math.c b/User/lib/user_math.c index 35a176b..ca65849 100644 --- a/User/lib/user_math.c +++ b/User/lib/user_math.c @@ -113,23 +113,53 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) { return 60.0f * (float)bullet_speed / (M_2PI * fric_radius); } -float motor_angle_change(uint32_t angle, uint16_t offset_angle) +// fp32 motor_angle_change(uint32_t angle, uint16_t offset_angle) +// { +// int32_t relative_angle = angle - offset_angle; +// if (relative_angle > 180) +// { +// while(relative_angle>180) +// { +// relative_angle -= 360; +// } +// } +// else if (relative_angle < -180) +// { +// while(relative_angle<-180) +// { +// relative_angle += 360; +// } +// } +// return relative_angle; + +// } + +fp32 motor_angle_change(uint32_t angle, uint16_t offset_angle) { int32_t relative_angle = angle - offset_angle; - if (relative_angle > 180) - { - while(relative_angle>180) - { - relative_angle -= 360; - } - } - else if (relative_angle < -180) - { - while(relative_angle<-180) - { - relative_angle += 360; - } - } + + // 使用取模运算将角度限制在 [-180, 180] + relative_angle = (relative_angle + 180) % 360 - 180; + return relative_angle; - +} + +fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd) +{ + int32_t relative_ecd = ecd - offset_ecd; + if (relative_ecd > 4095) + { + while(relative_ecd>4095) + { + relative_ecd -= 8191; + } + } + else if (relative_ecd < -4096) + { + while(relative_ecd<-4096) + { + relative_ecd += 8191; + } + } + return relative_ecd / (float)CAN_MOTOR_ENC_RES * 360.0f; } diff --git a/User/lib/user_math.h b/User/lib/user_math.h index 15d6a9a..97c49f5 100644 --- a/User/lib/user_math.h +++ b/User/lib/user_math.h @@ -14,10 +14,13 @@ extern "C" { #include #include #include +#include "struct_typedef.h" #define M_DEG2RAD_MULT (0.01745329251f) #define M_RAD2DEG_MULT (57.2957795131f) +#define MOTOR_ECD_TO_RAD 0.000766990394f +#define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */ #ifndef M_PI #define M_PI 3.14159265358979323846f @@ -102,7 +105,9 @@ void CircleReverse(float *origin); */ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm); -float motor_angle_change(uint32_t ecd, uint16_t offset_ecd); +fp32 motor_angle_change(uint32_t ecd, uint16_t offset_ecd); + +fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd); #ifdef __cplusplus } diff --git a/User/module/ball.cpp b/User/module/ball.cpp index 8999f8f..f0f45d4 100644 --- a/User/module/ball.cpp +++ b/User/module/ball.cpp @@ -4,13 +4,22 @@ #include "detect.h" #include "userTask.h" #include "user_math.h" +#include "shoot.hpp" + +//int jy=1; + extern RC_ctrl_t rc_ctrl; extern int key; +extern int16_t M2006_result; // C键 sw[5]👆 1800 中1000 👇200 // D键 sw[6]👆 200 👇1800 #define M_SPEED 4000 +#define I_ANGLE1 185 +#define I_ANGLE2 -75 +#define O_ANGLE1 340 +#define O_ANGLE2 -235 #if HANDLING3 == 1 //三摩擦轮运球！！！ @@ -18,7 +27,7 @@ extern int key; const fp32 Ball:: M3508_speed_PID[3] = {15, 0.03, 0}; const fp32 Ball:: Extend_speed_PID[3] = { 25, 0, 0.}; -const fp32 Ball:: Extend_angle_PID[3]= { 60, 0, 0.1}; +const fp32 Ball:: Extend_angle_PID[3]= { 50, 0, 0.1}; //摩擦轮转速 int speedm=0; @@ -42,17 +51,17 @@ Ball ::Ball() speedSet[i] = 0; } - //两个伸缩6020 + //两个伸缩6020 左207 右208 Extern_Motor[0] = get_motor_point(6); Extern_Motor[1] = get_motor_point(7); Extern_Motor[0]->type = GM6020; Extern_Motor[1]->type = GM6020; - PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,25000, 2000); - PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,25000, 1000); + PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,20000, 2000); + PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,10000, 1000); - PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,25000, 2000); - PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,25000, 1000); + PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,20000, 2000); + PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,10000, 1000); e_result[0] = 0; e_result[1] = 0; @@ -65,21 +74,62 @@ Ball ::Ball() } +// void Ball ::Extend_control(int angle) +// { +// int16_t delta; +// angleSet = angle/2; + +// fp32 angle_get; +// angle_get=motor_angle_change(Extern_Motor->real_round, angleSet); + +// delta = PID_calc(&extend_angle_pid, angle_get, angleSet); +// e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta); + +// CAN_cmd_1FF(0,0,e_result,0,&hcan1); +// osDelay(1); + +// } + +//左0 右1 +//real_round 外 176 240 内 89 326 + +// void Ball ::Extend_mcontrol(int angle1,int angle2) +// { +// int16_t delta[2]; +// angleSet[0] = angle1/2; +// angleSet[1] = angle2/2; + +// fp32 angle_get[2]; +// angle_get[0] = motor_angle_change(Extern_Motor[0]->real_round, angleSet[0]); +// angle_get[1] = motor_angle_change(Extern_Motor[1]->real_round, angleSet[1]); + +// delta[0] = PID_calc(&extend_angle_pid[0], angle_get[0] , angleSet[0]); +// e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]); + +// delta[1] = PID_calc(&extend_angle_pid[1], angle_get[1] , angleSet[1]); +// e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]); + +// } + + void Ball ::Extend_mcontrol(int angle1,int angle2) { int16_t delta[2]; - angleSet[0] = angle1; - angleSet[1] = angle2; + angleSet[0] = angle1/2; + angleSet[1] = angle2/2; - delta[0] = PID_calc(&extend_angle_pid[0], Extern_Motor[0]->total_angle , angleSet[0]); + fp32 angle_get[2]; + angle_get[0] = motor_ecd_to_angle_change(Extern_Motor[0]->ecd, angleSet[0]); + angle_get[1] = motor_ecd_to_angle_change(Extern_Motor[1]->ecd, angleSet[1]); + + delta[0] = PID_calc(&extend_angle_pid[0], angle_get[0] , angleSet[0]); e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]); - delta[1] = PID_calc(&extend_angle_pid[1], Extern_Motor[1]->total_angle , angleSet[1]); + delta[1] = PID_calc(&extend_angle_pid[1], angle_get[1] , angleSet[1]); e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]); } - void Ball ::Extend_control(int angle) { int16_t delta[2]; @@ -92,19 +142,6 @@ void Ball ::Extend_control(int angle) delta[1] = PID_calc(&extend_angle_pid[1], Extern_Motor[1]->total_angle , angleSet[1]); e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]); - // delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]); - // e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]); - - // angle_get[0]=motor_angle_change(Extern_Motor[0]->real_round, angleSet[0]); - // angle_get[1]=motor_angle_change(Extern_Motor[1]->real_round, angleSet[1]); - - - // delta[0] = PID_calc(&extend_angle_pid, angle_get[0], angleSet[0]); - // e_result[0] = PID_calc(&extend_speed_pid, Extern_Motor[0]->speed_rpm, delta[0]); - - // delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]); - // e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]); - } @@ -136,17 +173,20 @@ void Ball::ballStop(void) } -int ball_have = 0; -void Ball::ballTake(void) +void Ball::Move_Extend(void) { - ball_have=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0 - HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸 - speedm=2000; - if(ball_have==1) - { - speedm=0; - HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸 - } + if(rc_ctrl.sw[6] == 200) // 左拨杆上 + { + Extend_mcontrol(I_ANGLE1, I_ANGLE2); // 内伸 + } + else if(rc_ctrl.sw[6] == 1800) // 左拨杆下 + { + Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸 + } + else + { + Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸 + } } @@ -223,8 +263,8 @@ void Ball::Send_control() CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1); osDelay(1); - CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1); - osDelay(2); + CAN_cmd_1FF(M2006_result,0,e_result[0],e_result[1],&hcan1); + osDelay(1); } diff --git a/User/module/ball.hpp b/User/module/ball.hpp index 2a2c756..fbd81b3 100644 --- a/User/module/ball.hpp +++ b/User/module/ball.hpp @@ -10,6 +10,7 @@ #include "pid.h" #include "filter.h" + // 定义状态枚举 typedef enum { BALL_IDLE, // 空闲状态 @@ -53,7 +54,7 @@ public: void ballDown(void); void Send_control(void); void ballStop(void); - void ballTake(void); + void Move_Extend(void); void Extend_control(int angle); void Extend_mcontrol(int angle1,int angle2); diff --git a/User/module/shoot.cpp b/User/module/shoot.cpp index 942a070..294b878 100644 --- a/User/module/shoot.cpp +++ b/User/module/shoot.cpp @@ -28,6 +28,7 @@ const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0, 0.1}; #define Tigger_DO -5 #define Tigger_ZERO 130 +int16_t M2006_result =0; float knob_increment; Shoot::Shoot() @@ -65,8 +66,9 @@ void Shoot::trigger_control() int delta = 0; delta = PID_calc(&angle_pid,trigger_Motor->total_angle,t_posSet); // 计算位置环PID result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,delta); // 计算速度环PID + M2006_result=result; // result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet); - CAN_cmd_1FF(result,0,0,0,&hcan1); + // CAN_cmd_1FF(result,0,0,0,&hcan1); } @@ -143,7 +145,7 @@ void Shoot::rc_mode() // 计算旋钮增量 if (current_knob_value >= 200 && current_knob_value <= 1800) { - knob_increment = (current_knob_value - last_knob_value) / 50.0f; // 每80单位对应一个增量 + knob_increment = (current_knob_value - last_knob_value) / 20.0f; // 每80单位对应一个增量 } else { knob_increment = 0; // 如果旋钮值超出范围，不产生增量 } @@ -168,6 +170,7 @@ void Shoot::shoot_control() { if (currentState == SHOOT_READY) { // 如果当前状态是准备发射，松开钩子发射 t_posSet = Tigger_ZERO; // 扳机扣动 + BSP_Buzzer_Stop(); if (t_posSet >= 120) { // 假设120度为发射完成角度 currentState = SHOOT_IDLE; // 切换到空闲状态 is_hooked = false; // 重置钩子状态 @@ -212,7 +215,7 @@ void Shoot :: shoot_Current() case SHOOT_TOP: t_posSet = Tigger_DO; // 扳机扣动钩住 - osDelay(500); // 等待一段时间，确保扳机动作完成 + osDelay(1000); // 等待一段时间，确保扳机动作完成 if (fd_tpos <1) { //判定为钩住 @@ -226,6 +229,8 @@ void Shoot :: shoot_Current() { go1.Pos = fire_pos; // 下拉到中间挡位位置 if (feedback.fd_gopos >= fire_pos - 0.3f && feedback.fd_gopos <= fire_pos + 0.3f) { + BSP_Buzzer_Start(); + BSP_Buzzer_Set(1,5000); //currentState = SHOOT_WAIT; // 等待发射信号 //在拨杆切换时触发了 } diff --git a/User/module/shoot.hpp b/User/module/shoot.hpp index 54b4945..5309a17 100644 --- a/User/module/shoot.hpp +++ b/User/module/shoot.hpp @@ -17,19 +17,19 @@ typedef enum { SHOOT_RETURN // 自动返回状态 } ShootState_t; -// 定义状态枚举 -typedef enum { - BALL_IDLE, // 空闲状态 - BALL_FINISH, // 完成状态 -//持球状态 - BALL_TAKE, - BALL_LOSE +// // 定义状态枚举 +// typedef enum { +// BALL_IDLE, // 空闲状态 +// BALL_FINISH, // 完成状态 +// //持球状态 +// BALL_TAKE, +// BALL_LOSE -} BallState_t; +// } BallState_t; typedef enum { - UP1, + UP1=1, MIDDLE1, DOWN1, SHOOT, @@ -79,7 +79,7 @@ public: int16_t rc_key; //遥控器按键 int16_t trigger_key; ShootState_t currentState; //状态机 - volatile BallState_t ballStatus;//是否有球 + //volatile BallState_t ballStatus;//是否有球 volatile uint32_t flag_thread;//接收传回的线程通知 float distance; //视觉距离 diff --git a/User/task/ballTask.cpp b/User/task/ballTask.cpp index 1c30de0..2c8e5de 100644 --- a/User/task/ballTask.cpp +++ b/User/task/ballTask.cpp @@ -15,8 +15,9 @@ Ball ball; // D键 sw[6]👆 200 外伸 👇1800 归位 //三摩擦架子 // H键 sw[7]👆 200 👇1800 //取球 -int angle1=34; -int angle2=23; +int angle1=330; +int angle2=-240; +int test111=0; //检查光电 int abc=0; @@ -39,50 +40,44 @@ void FunctionBall(void *argument) abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); - if(rc_ctrl.sw[6] == 200) - { - angle1=85; - } - if(rc_ctrl.sw[6]==1800) - { - angle1=10; +// if(rc_ctrl.sw[6] == 200) +// { +// angle1=85; +// } +// if(rc_ctrl.sw[6]==1800) +// { +// angle1=10; - } +// } - //运球 - if(rc_ctrl.sw[5]==200) - { - //👇 - ball.ballHadling(); // 处理摩擦轮转动 - } - //下球 - if(rc_ctrl.sw[5]==1800) - { - //👆 - ball.ballDown(); - } - //中间 - if(rc_ctrl.sw[7]==200) - { - if(rc_ctrl.sw[5]==1000) - { - //中 - ball.ballStop(); - ball.ballHadling(); // 处理摩擦轮转动 - } - } - - if(rc_ctrl.sw[7]==1800) - { - //ball.ballTake(); - - } - - // CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2); +// //运球 +// if(rc_ctrl.sw[5]==200) +// { +// //👇 +// ball.ballHadling(); // 处理摩擦轮转动 +// } +// //下球 +// if(rc_ctrl.sw[5]==1800) +// { +// //👆 +// ball.ballDown(); +// } +// //中间 +// if(rc_ctrl.sw[7]==200) +// { +// if(rc_ctrl.sw[5]==1000) +// { +// //中 +// ball.ballStop(); +// ball.ballHadling(); // 处理摩擦轮转动 +// } +// } + //ball.Extend_control(test111); + ball.Move_Extend(); + // ball.Extend_mcontrol(angle1,angle2); // 内伸 ball.Spin(speedm); ball.Send_control(); - ball.Extend_control(angle1); // vofa[0] = -speedm; // 发送电机角度数据 // vofa[1] = ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据 diff --git a/User/task/userTask.h b/User/task/userTask.h index 5007984..3d11067 100644 --- a/User/task/userTask.h +++ b/User/task/userTask.h @@ -17,7 +17,7 @@ extern "C" { #define TASK_FREQ_CTRL_SHOOT (500u) #define TASK_FREQ_CTRL_COMMAND (500u) #define TASK_FREQ_MONITOR (2u) -#define TASK_FREQ_CAN (1000u) +#define TASK_FREQ_CAN (1500u) #define TASK_FREQ_AI (500u) #define TASK_FREQ_BALL (500u)