From 3d482325fb6d9075cf3e99c7a7448d78d1e5e3fa Mon Sep 17 00:00:00 2001
From: ws <1621320660@qq.com>
Date: Wed, 21 May 2025 13:17:25 +0800
Subject: [PATCH] =?UTF-8?q?=E5=9F=BA=E7=A1=80=E5=8A=A8=E4=BD=9C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Core/Src/gpio.c | 4 +-
MDK-ARM/.vscode/keil-assistant.log | 36 +++++++
MDK-ARM/.vscode/settings.json | 4 +-
MDK-ARM/.vscode/uv4.log | 4 +-
MDK-ARM/.vscode/uv4.log.lock | 2 +-
MDK-ARM/R1-shooter.uvoptx | 90 ++++++++++-------
MDK-ARM/R1-shooter.uvprojx | 10 ++
User/device/djiMotor.c | 11 +++
User/module/ball.cpp | 154 ++++++++++++++++++++---------
User/module/ball.hpp | 21 ++--
User/module/gimbal.cpp | 3 +-
User/module/shoot.cpp | 124 ++++++++++++++---------
User/module/shoot.hpp | 12 ++-
User/task/ballTask.cpp | 75 +++++++++++---
User/task/gimbalTask.cpp | 2 +-
User/task/shootTask.cpp | 32 +++++-
16 files changed, 420 insertions(+), 164 deletions(-)
diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c
index b618657..29176dd 100644
--- a/Core/Src/gpio.c
+++ b/Core/Src/gpio.c
@@ -76,7 +76,7 @@ void MX_GPIO_Init(void)
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = up_ball_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
- GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(up_ball_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
@@ -121,7 +121,7 @@ void MX_GPIO_Init(void)
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = STOP_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
- GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(STOP_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log
index b3c05a0..3a88102 100644
--- a/MDK-ARM/.vscode/keil-assistant.log
+++ b/MDK-ARM/.vscode/keil-assistant.log
@@ -54,3 +54,39 @@
[info] Log at : 2025/4/26|16:18:23|GMT+0800
+[info] Log at : 2025/5/7|20:43:43|GMT+0800
+
+[info] Log at : 2025/5/8|16:11:00|GMT+0800
+
+[info] Log at : 2025/5/10|16:53:30|GMT+0800
+
+[info] Log at : 2025/5/10|20:53:12|GMT+0800
+
+[info] Log at : 2025/5/11|14:55:09|GMT+0800
+
+[info] Log at : 2025/5/11|15:23:30|GMT+0800
+
+[info] Log at : 2025/5/12|19:40:58|GMT+0800
+
+[info] Log at : 2025/5/13|20:50:37|GMT+0800
+
+[info] Log at : 2025/5/14|20:21:54|GMT+0800
+
+[info] Log at : 2025/5/15|14:31:29|GMT+0800
+
+[info] Log at : 2025/5/15|19:24:56|GMT+0800
+
+[info] Log at : 2025/5/17|21:50:24|GMT+0800
+
+[info] Log at : 2025/5/18|16:09:49|GMT+0800
+
+[info] Log at : 2025/5/18|19:22:14|GMT+0800
+
+[info] Log at : 2025/5/18|23:12:00|GMT+0800
+
+[info] Log at : 2025/5/19|21:08:52|GMT+0800
+
+[info] Log at : 2025/5/20|00:19:48|GMT+0800
+
+[info] Log at : 2025/5/20|19:15:21|GMT+0800
+
diff --git a/MDK-ARM/.vscode/settings.json b/MDK-ARM/.vscode/settings.json
index 9d1717c..19c145a 100644
--- a/MDK-ARM/.vscode/settings.json
+++ b/MDK-ARM/.vscode/settings.json
@@ -11,6 +11,8 @@
"nuc.h": "c",
"crc_ccitt.h": "c",
"functional": "cpp",
- "vofa.h": "c"
+ "vofa.h": "c",
+ "user_math.h": "c",
+ "queue": "cpp"
}
}
\ No newline at end of file
diff --git a/MDK-ARM/.vscode/uv4.log b/MDK-ARM/.vscode/uv4.log
index 2ac5c03..8ba7939 100644
--- a/MDK-ARM/.vscode/uv4.log
+++ b/MDK-ARM/.vscode/uv4.log
@@ -2,7 +2,7 @@
Build target 'R1-shooter'
compiling ball.cpp...
linking...
-Program Size: Code=26644 RO-data=1812 RW-data=240 ZI-data=23520
+Program Size: Code=28460 RO-data=1824 RW-data=256 ZI-data=23936
FromELF: creating hex file...
"R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
-Build Time Elapsed: 00:00:03
+Build Time Elapsed: 00:00:02
diff --git a/MDK-ARM/.vscode/uv4.log.lock b/MDK-ARM/.vscode/uv4.log.lock
index e1bf9c5..ea230dc 100644
--- a/MDK-ARM/.vscode/uv4.log.lock
+++ b/MDK-ARM/.vscode/uv4.log.lock
@@ -1 +1 @@
-2025/4/26 16:32:18
\ No newline at end of file
+2025/5/20 23:55:49
\ No newline at end of file
diff --git a/MDK-ARM/R1-shooter.uvoptx b/MDK-ARM/R1-shooter.uvoptx
index a3a851b..e401a6a 100644
--- a/MDK-ARM/R1-shooter.uvoptx
+++ b/MDK-ARM/R1-shooter.uvoptx
@@ -183,77 +183,77 @@
5
1
- angle1,0x0A
+ cnt1,0x0A
6
1
- k,0x0A
+ cmd_fromnuc
7
1
- cnt1,0x0A
+ nucbuf
8
1
- cmd_fromnuc
+ nucData
9
1
- nucbuf
+ send,0x0A
10
1
- nucData
+ \\R1_shooter\../User/task/ballTask.cpp\ball,0x0A
11
1
- send,0x0A
+ abc,0x0A
12
1
- speed1,0x0A
+ ball_state,0x0A
13
1
- \\R1_shooter\../User/task/ballTask.cpp\ball,0x0A
+ flag,0x0A
14
1
- currentState1
+ shoot,0x0A
15
1
- abc,0x0A
+ speedm,0x0A
16
1
- ball_state,0x0A
+ a1,0x0A
17
1
- flag,0x0A
+ a2,0x0A
18
1
- triggerCount,0x0A
+ shoot_flag,0x0A
19
1
- last_ball_state,0x0A
+ angle1,0x0A
@@ -957,6 +957,30 @@
0
0
+
+ 7
+ 50
+ 1
+ 0
+ 0
+ 0
+ ..\User\lib\filter.c
+ filter.c
+ 0
+ 0
+
+
+ 7
+ 51
+ 1
+ 0
+ 0
+ 0
+ ..\User\lib\kalman.c
+ kalman.c
+ 0
+ 0
+
@@ -967,7 +991,7 @@
0
8
- 50
+ 52
1
0
0
@@ -979,7 +1003,7 @@
8
- 51
+ 53
1
0
0
@@ -991,7 +1015,7 @@
8
- 52
+ 54
1
0
0
@@ -1003,7 +1027,7 @@
8
- 53
+ 55
1
0
0
@@ -1015,7 +1039,7 @@
8
- 54
+ 56
1
0
0
@@ -1027,7 +1051,7 @@
8
- 55
+ 57
1
0
0
@@ -1041,13 +1065,13 @@
User/module
- 0
+ 1
0
0
0
9
- 56
+ 58
8
0
0
@@ -1059,7 +1083,7 @@
9
- 57
+ 59
8
0
0
@@ -1071,7 +1095,7 @@
9
- 58
+ 60
8
0
0
@@ -1083,7 +1107,7 @@
9
- 59
+ 61
8
0
0
@@ -1103,7 +1127,7 @@
0
10
- 60
+ 62
1
0
0
@@ -1115,7 +1139,7 @@
10
- 61
+ 63
1
0
0
@@ -1127,7 +1151,7 @@
10
- 62
+ 64
8
0
0
@@ -1139,7 +1163,7 @@
10
- 63
+ 65
8
0
0
@@ -1151,7 +1175,7 @@
10
- 64
+ 66
8
0
0
@@ -1163,7 +1187,7 @@
10
- 65
+ 67
8
0
0
@@ -1175,7 +1199,7 @@
10
- 66
+ 68
8
0
0
diff --git a/MDK-ARM/R1-shooter.uvprojx b/MDK-ARM/R1-shooter.uvprojx
index 7b9eb22..7758149 100644
--- a/MDK-ARM/R1-shooter.uvprojx
+++ b/MDK-ARM/R1-shooter.uvprojx
@@ -658,6 +658,16 @@
1
..\User\lib\user_math.c
+
+ filter.c
+ 1
+ ..\User\lib\filter.c
+
+
+ kalman.c
+ 1
+ ..\User\lib\kalman.c
+
diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c
index 9198d57..f92fe83 100644
--- a/User/device/djiMotor.c
+++ b/User/device/djiMotor.c
@@ -141,6 +141,17 @@ void djiMotorEncode()
}
break;
}
+ case GM6020_2:
+ {
+ if(motor_chassis[7].msg_cnt<=50)
+ {
+ motor_chassis[7].msg_cnt++;
+ get_motor_offset(&motor_chassis[7], dji_rx_data);
+ }else{
+ get_6020_motor_measure(&motor_chassis[7], dji_rx_data);
+ }
+ break;
+ }
default:
{
diff --git a/User/module/ball.cpp b/User/module/ball.cpp
index 1d41eb6..9ca6f6a 100644
--- a/User/module/ball.cpp
+++ b/User/module/ball.cpp
@@ -7,6 +7,9 @@
extern RC_ctrl_t rc_ctrl;
extern int key;
+// C键 sw[4]👆 200 中1000 👇1800
+// D键 sw[5]👆 1800 👇200
+
#define START 0
#define OUT 100
@@ -14,9 +17,9 @@ extern int key;
//三摩擦轮运球！！！
//添加平移3508 得跑位置吧
-const fp32 Ball:: M3508_speed_PID[3] = {5, 0.01, 0};
-const fp32 Ball:: Extend_speed_PID[3] = {5, 0.01, 0};
-const fp32 Ball:: Extend_angle_PID[3]= { 5, 0.01, 0};
+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};
int speedm=0;
int speedm1=0;
@@ -26,14 +29,23 @@ Ball ::Ball()
{
detect_init();
- //伸缩6020
- Extern_Motor = get_motor_point(6);
- //Extern_Motor->type = GM6020;
- PID_init(&extend_speed_pid,PID_POSITION,Extend_speed_PID,3000, 200);
- PID_init(&extend_angle_pid,PID_POSITION,Extend_angle_PID,3000, 200);
+ //两个伸缩6020
+ 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[1],PID_POSITION,Extend_speed_PID,25000, 2000);
+ PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,25000, 1000);
+
+ e_result[0] = 0;
+ e_result[1] = 0;
+ angleSet[0] = 0;
+ angleSet[1] = 0;
- e_result = 0;
- angleSet = 0;
//三摩擦轮
for(int i = 0;i < MOTOR_NUM;i ++)
{
@@ -51,6 +63,16 @@ Ball ::Ball()
//状态机状态初始化
currentState1= BALL_IDLE;
+
+ // for(int i = 0;i < MOTOR_NUM;i ++)
+ // {
+ // //摩擦轮滤波器初始化
+ // LowPassFilter2p_Init(filter.in + i , 500,
+ // -1.0f);
+ // LowPassFilter2p_Init(filter.out + i, 500,
+ // -1.0f);
+ // }
+
}
void Ball :: Filter_init(float target_freq)
@@ -67,69 +89,111 @@ void Ball :: Filter_init(float target_freq)
void Ball ::Extend_control(int angle)
{
- int16_t delta;
- angleSet = angle/2;
+ int16_t delta[2];
+ angleSet[0] = angle;
+ angleSet[1] = -angle;
+
+ delta[0] = PID_calc(&extend_angle_pid[0],Extern_Motor[0]->total_angle , 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]);
+ 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]);
+
- 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);
}
+// void Ball ::Extend_control(int angle)
+// {
+// int16_t delta;
+// angleSet = angle;
-void Ball ::Spin(float speed,float speed2)
+// delta = PID_calc(&extend_angle_pid, Extern_Motor->total_angle, angleSet);
+// e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta);
+
+// }
+
+
+void Ball ::Spin(float speed)
{
- for(int i = 0;i < MOTOR_NUM;i ++)
- {
- hand_Motor[i]->speed_rpm=LowPassFilter2p_Apply(filter.in + i, hand_Motor[i]->speed_rpm);
- }
-
speedSet[MOTOR_1] = -speed;
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
speedSet[MOTOR_2] = speed;
result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);
- speedSet[MOTOR_3] = speed2;
+ speedSet[MOTOR_3] = speed;
result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);
- for(int i = 0;i < MOTOR_NUM;i ++)
- {
- result[i]=LowPassFilter2p_Apply(filter.out + i, result[i]);
- }
+}
+
+void Ball::ballDown(void)
+{
+ HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
+ speedm=-500;
}
+void Ball::ballStop(void)
+{
+ speedm=0;
+
+}
+
+int ball_have = 0;
+void Ball::ballTake(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);//确保闭合气缸
+ }
+}
+
+
+
int flag =0;
int ball_state = 0;
void Ball::ballHadling(void)
{
- ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 0 无球 1
+ ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0
switch (currentState1)
{
case BALL_IDLE:
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
- if (key > 0) // 检测按键是否被按下
+ // if (key > 0) // 检测按键是否被按下
+ if (rc_ctrl.sw[4] > 1000||key > 0) // 检测按键是否被按下,自动回弹的
{
- speedm=-4500;
- speedm1=-4500;
+ speedm=-4000;
currentState1 = BALL_FORWARD; // 切换到正转状态
}
break;
case BALL_FORWARD:
- if ( hand_Motor[MOTOR_1]->speed_rpm >= 4480&&hand_Motor[MOTOR_1]->speed_rpm <= 4530 &&
- hand_Motor[MOTOR_2]->speed_rpm <= -4480&&hand_Motor[MOTOR_2]->speed_rpm >= -4530 &&
- hand_Motor[MOTOR_3]->speed_rpm <= -4480&&hand_Motor[MOTOR_3]->speed_rpm >= -4530 )
+ if ( hand_Motor[MOTOR_1]->speed_rpm >= 3980&&hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
+ hand_Motor[MOTOR_2]->speed_rpm <= -3980&&hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&
+ hand_Motor[MOTOR_3]->speed_rpm <= -3980&&hand_Motor[MOTOR_3]->speed_rpm >= -4020 )
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);// 打开气缸
currentState1 = BALL_DROP; // 切换到球下落状态
@@ -138,11 +202,10 @@ void Ball::ballHadling(void)
case BALL_DROP:
- if (ball_state == 1) //读光电 有球 0 无球 1
+ if (ball_state == 0) //读光电 有球 1 无球 0
{
osDelay(200); // 延时200ms
- speedm=4500;
- speedm1=4500;
+ speedm=3500;
currentState1 = BALL_REVERSE; // 切换到反转状态
}
@@ -150,11 +213,10 @@ void Ball::ballHadling(void)
case BALL_REVERSE:
- if (ball_state == 0)
+ if (ball_state == 1)
{
speedm=0; // 停止电机
- speedm1=0;
currentState1 = BALL_CLOSE; // 切换到完成状态
}
@@ -162,7 +224,7 @@ void Ball::ballHadling(void)
case BALL_CLOSE:
// osDelay(200); // 延时50ms
- if(ball_state == 0)
+ if(ball_state == 1)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
currentState1 = BALL_FINISH; // 切换到完成状态
@@ -175,7 +237,6 @@ void Ball::ballHadling(void)
key=0; // 重置按键状态
flag=0;
speedm=0;
- speedm1=0;
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
currentState1 = BALL_IDLE; // 回到空闲状态
break;
@@ -185,11 +246,16 @@ void Ball::ballHadling(void)
break;
}
+
+
}
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);
}
diff --git a/User/module/ball.hpp b/User/module/ball.hpp
index 94a8e5a..1b681e0 100644
--- a/User/module/ball.hpp
+++ b/User/module/ball.hpp
@@ -19,7 +19,8 @@ typedef enum {
BALL_FLAG,
BALL_CLOSE, // 关闭状态
BALL_FINISH, // 完成状态
-//持球状态
+
+ //持球状态
BALL_TAKE,
BALL_LOSE
@@ -48,16 +49,19 @@ class Ball
public:
Ball();
void Filter_init(float target_freq);
- void Spin(float speed,float speed2);
+ void Spin(float speed);
void ballHadling(void);
+ void ballDown(void);
void Send_control(void);
+ void ballStop(void);
+ void ballTake(void);
void Extend_control(int angle);
BallState_t currentState1; // 当前状态
//伸缩6020
- int16_t e_result;
- motor_measure_t * Extern_Motor;
+ int16_t e_result[2];
+ motor_measure_t * Extern_Motor[2];
int16_t result[MOTOR_NUM];
motor_measure_t *hand_Motor[MOTOR_NUM];
@@ -66,9 +70,10 @@ public:
volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知
+ Filter filter;
private:
//滤波一下
- Filter filter;
+
//三个摩擦轮
static const float M3508_speed_PID[3];
@@ -82,10 +87,10 @@ private:
//位置环pid
pid_type_def angle_pid[MOTOR_NUM];
- pid_type_def extend_speed_pid;
- pid_type_def extend_angle_pid;
+ pid_type_def extend_speed_pid[2];
+ pid_type_def extend_angle_pid[2];
- float angleSet;
+ float angleSet[2];
float speedSet[MOTOR_NUM];
};
diff --git a/User/module/gimbal.cpp b/User/module/gimbal.cpp
index c59a90e..0d64aa3 100644
--- a/User/module/gimbal.cpp
+++ b/User/module/gimbal.cpp
@@ -10,7 +10,6 @@
//可活动角度
#define ANGLE_ALLOW 1.0f
extern RC_ctrl_t rc_ctrl;
-int angle1 = 0;
NUC_t nuc;
const fp32 Gimbal:: Gimbal_speed_PID[3] = {50, 0.1, 0};
@@ -32,7 +31,7 @@ Gimbal::Gimbal()
void Gimbal::gimbalFlow()
{
int16_t delta[1];
- angleSet = angle1;
+ //angleSet = angle1;
delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);
diff --git a/User/module/shoot.cpp b/User/module/shoot.cpp
index 42d418a..fde279c 100644
--- a/User/module/shoot.cpp
+++ b/User/module/shoot.cpp
@@ -5,22 +5,33 @@
#include "FreeRTOS.h"
#include
#include "calc_lib.h"
+#include "vofa.h"
extern RC_ctrl_t rc_ctrl;
-#define SHOOT_SPEED 40000
-#define SHOOT_SPEED_BACK -2000
-#define STOP 0
-#define Trigger 3000
+float vofa[8];
+#define SHOOT_SPEED 30500
+#define SHOOT_SPEED_BACK -2500
+#define Error 50
-const fp32 Shoot:: M2006_speed_PID[3] = {5, 0.01, 0};
+#define STOP 0
+
+#define Trigger_Torque -5000
+
+//sw[7]👆 1694 中 1000 👇306
+//sw[2]👆 1694 👇306
+
+//F键 sw[0]👆 1800 中 1000 👇200
+//E键 sw[1]👆 1800 👇200
+
+const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
int t=0;
Shoot::Shoot()
{
//扳机初始化
trigger_Motor= get_motor_point(4);
- trigger_Motor->type=M3508;
- PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,16000, 10000);//pid初始化
+ trigger_Motor->type=M2006;
+ PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,6000, 1000);//pid初始化
speedSet = 0;
result = 0;
@@ -31,64 +42,65 @@ Shoot::Shoot()
currentState= SHOOT_IDLE;
}
-void Shoot::trigger_spin()
+void Shoot::trigger_control()
{
- if(t>0)
- {
- speedSet=Trigger;
- if(trigger_Motor->speed_rpm<5)
- {
- speedSet=-Trigger;
- }
- }
+ ///speedSet=speed;
+ //result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, speedSet);
+ //result = Trigger_Torque;
+ CAN_cmd_1FF(result,0,0,0,&hcan1);
}
void Shoot::shootThree()
{
- if((rc_ctrl.sw[1]>500))
+ if((rc_ctrl.sw[7]==1694))
{
+
speed_5065 = SHOOT_SPEED;
+
+ }
+ if((rc_ctrl.sw[7]==1000))
+ {
+ speed_5065=STOP;
+ //发一次会堵塞另一个
+// CAN_VESC_HEAD(1);
+// CAN_VESC_HEAD(2);
+ }
+ if(rc_ctrl.sw[7]==306)
+ {
+ speed_5065=SHOOT_SPEED_BACK;
}
- else
- {
- speed_5065=STOP;
+ CAN_VESC_Control(1,speed_5065,&hcan2);
+ // CAN_VESC_RPM(1, speed_5065);
+ // CAN_VESC_RPM(2, speed_5065);
- }
- if(rc_ctrl.sw[5]==1694)
- {
- speed_5065=SHOOT_SPEED_BACK;
-
- }
- CAN_VESC_Control(1,speed_5065,&hcan2);
- // CAN_VESC_RPM(1, speed_5065);
- // CAN_VESC_RPM(2, speed_5065);
+ // vofa[0] = motor_5065[1]->rotor_speed;
+ // vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
+ // vofa_tx_main(vofa); // 发送数据到虚拟串口
}
void Shoot::shootBack()
{
- if(rc_ctrl.sw[5]==1694)
- {
-
- CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
- CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
- }
-
+ speed_5065=SHOOT_SPEED_BACK;
+ CAN_VESC_Control(1,speed_5065,&hcan2);
+
}
void Shoot::shootStop()
{
- CAN_VESC_HEAD(1);
- CAN_VESC_HEAD(2);
+ speed_5065=STOP;
+ CAN_VESC_Control(1,speed_5065,&hcan2);
}
void Shoot::shootStateMachine() {
switch (currentState) {
case SHOOT_IDLE: {
+ speed_5065=STOP;
+ result=STOP;
// 空闲状态，等待遥控器输入
- if (rc_ctrl.sw[1] > 500) {
+ if((rc_ctrl.sw[0]==1800)) {
currentState = SHOOT_FIRE; // 切换到发射状态
}
break;
@@ -96,21 +108,25 @@ void Shoot::shootStateMachine() {
case SHOOT_FIRE: {
// 发射状态，控制电机发射
- speed_5065 = map((float)rc_ctrl.sw[1], 500, 1694, 30000, 45000);
- CAN_VESC_RPM(1, speed_5065);
- CAN_VESC_RPM(2, speed_5065);
+ speed_5065 = SHOOT_SPEED;
+ if(motor_5065[0]->rotor_speed>=SHOOT_SPEED-Error && motor_5065[0]->rotor_speed<=SHOOT_SPEED+Error &&
+ motor_5065[1]->rotor_speed>=SHOOT_SPEED-Error && motor_5065[1]->rotor_speed<=SHOOT_SPEED+Error)
+ {
+ result=Trigger_Torque;//恒力扳机
+ }
// 检测光电传感器是否触发
if (IS_PHOTOELECTRIC_TRIGGERED()) {
- currentState = SHOOT_TRIGGERED; // 切换到光电触发状态
+ currentState = SHOOT_BACK; // 切换到光电触发状态
}
break;
}
- case SHOOT_TRIGGERED: {
+ case SHOOT_BACK: {
// 光电触发状态，停止发射
- CAN_VESC_HEAD(1);
- CAN_VESC_HEAD(2);
+ result=STOP;
+ speed_5065=STOP;
+
// 切换到返回状态
currentState = SHOOT_RETURN;
break;
@@ -118,11 +134,13 @@ void Shoot::shootStateMachine() {
case SHOOT_RETURN: {
// 自动返回状态，控制电机反转
- CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
- CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
+ speed_5065=SHOOT_SPEED_BACK;
+ result=-Trigger_Torque;
// 检测返回完成（可以通过时间或其他传感器判断）
- if (rc_ctrl.sw[1] < 500) { // 假设遥控器开关控制返回完成
+ if (rc_ctrl.sw[0]==1000) { // 假设遥控器开关控制返回完成
+ speed_5065=SHOOT_SPEED_BACK;
+ result=STOP;
currentState = SHOOT_IDLE; // 切换回空闲状态
}
break;
@@ -134,4 +152,12 @@ void Shoot::shootStateMachine() {
break;
}
}
+ CAN_VESC_Control(1,speed_5065,&hcan2);
+
+ vofa[0] = motor_5065[1]->rotor_speed;
+ vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
+ vofa[2] = speed_5065; // 发送电机速度数据
+
+ vofa_tx_main(vofa); // 发送数据到虚拟串口
+
}
diff --git a/User/module/shoot.hpp b/User/module/shoot.hpp
index b7d8a83..361c0ba 100644
--- a/User/module/shoot.hpp
+++ b/User/module/shoot.hpp
@@ -7,7 +7,7 @@
typedef enum {
SHOOT_IDLE, // 空闲状态
SHOOT_FIRE, // 发射状态
- SHOOT_TRIGGERED, // 光电触发状态
+ SHOOT_BACK, // 光电触发状态
SHOOT_RETURN // 自动返回状态
} ShootState_t;
@@ -32,11 +32,15 @@ public:
void shootStop(void);
void shootBack(void);
void shootStateMachine(void);
- void trigger_spin(void);
+ void trigger_control(void);
float speed_5065;
+ float speed_trigger;
ShootState_t currentState;
- //==========================公共变量==========================
+ int16_t result;
+ //暂时放在公共,task里使用
+ CAN_MotorFeedback_t *motor_5065[2];
+ //==========================公共变量==========================
volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知
@@ -47,10 +51,8 @@ private:
//电机速度pid结构体
pid_type_def speed_pid;
motor_measure_t *trigger_Motor;
- int16_t result;
float speedSet;
- CAN_MotorFeedback_t *motor_5065[2];
};
diff --git a/User/task/ballTask.cpp b/User/task/ballTask.cpp
index 27d51a4..e27921b 100644
--- a/User/task/ballTask.cpp
+++ b/User/task/ballTask.cpp
@@ -8,11 +8,23 @@
#include "vofa.h"
extern RC_ctrl_t rc_ctrl;
Ball ball;
-float vofa[8];
+//float vofa[8];
-int out=0;
+// 左旋钮 sw[2] 左1800 右200
+// 右旋钮 sw[3] 左1800 右200
+// C键 sw[4]👆 200 中1000 👇1800
+// D键 sw[5]👆 1800 👇200
+
+int angle1=34;
+int angle2=23;
int abc=0;
+
+int a1=0;
+
+int speed_set=0;
+int speed_feedback=0;
+
extern int speedm;
extern int speedm1;
@@ -21,7 +33,6 @@ void FunctionBall(void *argument)
(void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
- ball.Filter_init(TASK_FREQ_BALL);
uint32_t tick = osKernelGetTickCount();
while(1)
@@ -29,20 +40,56 @@ void FunctionBall(void *argument)
#ifdef DEBUG
task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());
#endif
- abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
- //abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
- ball.Extend_control(out);
- ball.ballHadling(); // 处理摩擦轮转动
- ball.Spin(speedm,speedm1);
- ball.Send_control();
+ abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
- vofa[0] = speedm; // 发送电机角度数据
- vofa[1] = -ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
- vofa[2] = ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
- vofa[3] = ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据
- vofa_tx_main(vofa); // 发送数据到虚拟串口
+ if(rc_ctrl.sw[2]>=1200)
+ {
+ angle1=75;
+ }
+ if(rc_ctrl.sw[2]<1200)
+ {
+ angle1=0;
+
+ }
+ //运球
+ if(rc_ctrl.sw[3]>=1200)
+ {
+ a1=1;
+ //👇
+ ball.ballHadling(); // 处理摩擦轮转动
+ }
+ //下球
+ if(rc_ctrl.sw[4]==200)
+ {
+ //👆
+ ball.ballDown();
+ }
+ //回弹停止
+ if(rc_ctrl.sw[3]<1000)
+ {
+ //👆
+ ball.ballStop();
+ }
+ if(rc_ctrl.sw[7]==1800)
+ {
+ ball.ballTake();
+
+ }
+
+
+
+
+ ball.Extend_control(angle1);
+ ball.Spin(speedm);
+ ball.Send_control();
+
+ // vofa[0] = -speedm; // 发送电机角度数据
+ // vofa[1] = ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
+ // vofa[2] = -ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
+ // vofa[3] = -ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据
+ // vofa_tx_main(vofa); // 发送数据到虚拟串口
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick);
diff --git a/User/task/gimbalTask.cpp b/User/task/gimbalTask.cpp
index 7db5aa3..5138330 100644
--- a/User/task/gimbalTask.cpp
+++ b/User/task/gimbalTask.cpp
@@ -28,7 +28,7 @@ void FunctionGimbal(void *argument)
task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());
#endif
- cnt1++;
+ //cnt1++;
// gimbal.gimbalFlow();
// 从消息队列接收视觉数据
diff --git a/User/task/shootTask.cpp b/User/task/shootTask.cpp
index c7c67ed..ea16fe1 100644
--- a/User/task/shootTask.cpp
+++ b/User/task/shootTask.cpp
@@ -9,8 +9,15 @@
extern RC_ctrl_t rc_ctrl;
Shoot shoot;
+int shoot_flag = 0;
+
+int a2;
+
// sw[0]2 下306上1694 sw[5]3前306后1694 sw[4]4前1694后306 sw[1]xuan1 sw[3]xuan2
+//F键 sw[0]👆 1800 中 1000 👇200
+//E键 sw[1]👆 1800 👇200
+
void FunctionShoot(void *argument)
{
(void)argument; /* 未使用argument，消除警告 */
@@ -25,15 +32,36 @@ while(1)
task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId());
#endif
+ //我放的任务通知 运球成功放置过来后
shoot.flag_thread=osThreadFlagsGet();
if(shoot.flag_thread & BALL_OK)
{
- shoot.shootThree();
+ a2=2;
+ // shoot.shootThree();
}
- //shoot.shootBack();
+ shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
+ if(rc_ctrl.sw[1]>1000)
+ {
+ shoot.shootStateMachine();
+ if(rc_ctrl.sw[0]==200)
+ {
+ shoot.shootBack();
+ }
+ if(rc_ctrl.sw[0]==1000)
+ {
+ shoot.shootStop();
+ }
+ }
+ if(rc_ctrl.sw[1]==200)
+ {
+ shoot.shootStop();
+
+ }
+
+ shoot.trigger_control();
osDelay(2);
tick += delay_tick; /* 计算下一个唤醒时刻 */