diff --git a/MDK-ARM/.vscode/c_cpp_properties.json b/MDK-ARM/.vscode/c_cpp_properties.json
index 120b12d..479213b 100644
--- a/MDK-ARM/.vscode/c_cpp_properties.json
+++ b/MDK-ARM/.vscode/c_cpp_properties.json
@@ -3,26 +3,26 @@
{
"name": "R1-shooter",
"includePath": [
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Inc",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\bsp",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\module",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\task",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\lib",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\device",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Inc",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\bsp",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\module",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\task",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\lib",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\device",
"D:\\keil\\ARM\\ARMCC\\include",
"D:\\keil\\ARM\\ARMCC\\include\\rw",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\MDK-ARM",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Src",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
- "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\MDK-ARM",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Src",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
+ "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
],
"defines": [
"USE_HAL_DRIVER",
diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log
index 5d1bd7b..210b1cd 100644
--- a/MDK-ARM/.vscode/keil-assistant.log
+++ b/MDK-ARM/.vscode/keil-assistant.log
@@ -106,3 +106,19 @@
[info] Log at : 2025/5/23|21:04:08|GMT+0800
+[info] Log at : 2025/5/25|14:26:08|GMT+0800
+
+[info] Log at : 2025/5/26|19:07:26|GMT+0800
+
+[info] Log at : 2025/5/26|20:16:52|GMT+0800
+
+[info] Log at : 2025/5/27|00:06:58|GMT+0800
+
+[info] Log at : 2025/5/27|19:35:30|GMT+0800
+
+[info] Log at : 2025/5/28|13:50:56|GMT+0800
+
+[info] Log at : 2025/5/29|19:03:54|GMT+0800
+
+[info] Log at : 2025/5/30|19:34:50|GMT+0800
+
diff --git a/MDK-ARM/.vscode/uv4.log b/MDK-ARM/.vscode/uv4.log
index fb7ea63..03df30f 100644
--- a/MDK-ARM/.vscode/uv4.log
+++ b/MDK-ARM/.vscode/uv4.log
@@ -1,16 +1,8 @@
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
Build target 'R1-shooter'
-compiling motor.cpp...
compiling ball.cpp...
-compiling djiMotor.c...
-compiling gimbal.cpp...
-compiling encodeCan.cpp...
-compiling shoot.cpp...
-compiling ballTask.cpp...
-compiling shootTask.cpp...
-compiling gimbalTask.cpp...
linking...
-Program Size: Code=28808 RO-data=1800 RW-data=264 ZI-data=23712
+Program Size: Code=30584 RO-data=2144 RW-data=276 ZI-data=24012
FromELF: creating hex file...
"R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
-Build Time Elapsed: 00:00:05
+Build Time Elapsed: 00:00:03
diff --git a/MDK-ARM/.vscode/uv4.log.lock b/MDK-ARM/.vscode/uv4.log.lock
index 1473684..83f0a16 100644
--- a/MDK-ARM/.vscode/uv4.log.lock
+++ b/MDK-ARM/.vscode/uv4.log.lock
@@ -1 +1 @@
-2025/5/23 21:17:13
\ No newline at end of file
+2025/5/28 19:17:30
\ No newline at end of file
diff --git a/MDK-ARM/R1-shooter.uvoptx b/MDK-ARM/R1-shooter.uvoptx
index 2b9c404..a8fdc74 100644
--- a/MDK-ARM/R1-shooter.uvoptx
+++ b/MDK-ARM/R1-shooter.uvoptx
@@ -103,7 +103,7 @@
1
0
0
- 6
+ 3
@@ -114,7 +114,7 @@
- STLink\ST-LINKIII-KEIL_SWO.dll
+ BIN\CMSIS_AGDI.dll
@@ -140,7 +140,7 @@
0
DLGUARM
- (105=-1,-1,-1,-1,0)
+
0
@@ -153,40 +153,7 @@
-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)
-
-
- 0
- 0
- 169
- 1
- 134235782
- 0
- 0
- 0
- 0
- 0
- 1
- ..\User\device\djiMotor.c
-
- \\R1_shooter\../User/device/djiMotor.c\169
-
-
- 1
- 0
- 174
- 1
- 134235770
- 0
- 0
- 0
- 0
- 0
- 1
- ..\User\device\djiMotor.c
-
- \\R1_shooter\../User/device/djiMotor.c\174
-
-
+
0
@@ -303,6 +270,36 @@
1
motor_5065
+
+ 23
+ 1
+ test_distance
+
+
+ 24
+ 1
+ xxx
+
+
+ 25
+ 1
+ test_speed,0x0A
+
+
+ 26
+ 1
+ speedm,0x0A
+
+
+ 27
+ 1
+ Torque,0x0A
+
+
+ 28
+ 1
+ triggerspeed,0x0A
+
@@ -1113,7 +1110,7 @@
User/module
- 0
+ 1
0
0
0
diff --git a/README.md b/README.md
index e0529d1..5e0cdac 100644
--- a/README.md
+++ b/README.md
@@ -14,4 +14,15 @@ r1上层
//PC6 接球光电
-//PI6 运球光电
\ No newline at end of file
+//PI6 运球光电
+
+## 上层过程
+
++ 一个按键切换 运球 /发射
++ 👈 运球 +
++ .3
+
+
+
+
+
diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c
index ca9390a..e277254 100644
--- a/User/device/djiMotor.c
+++ b/User/device/djiMotor.c
@@ -416,7 +416,7 @@ return DEVICE_OK;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
- assert_param_rpm(&RPM);
+ //assert_param_rpm(&RPM);
data = (int32_t)(RPM);
vesc_tx_message.ExtId = id;
diff --git a/User/module/ball.cpp b/User/module/ball.cpp
index 35c69c1..4439b92 100644
--- a/User/module/ball.cpp
+++ b/User/module/ball.cpp
@@ -7,8 +7,8 @@
extern RC_ctrl_t rc_ctrl;
extern int key;
-// C键 sw[4]👆 200 中1000 👇1800
-// D键 sw[5]👆 1800 👇200
+// C键 sw[5]👆 1800 中1000 👇200
+// D键 sw[6]👆 200 👇1800
#define M_SPEED 4000
@@ -17,6 +17,8 @@ extern int key;
//添加平移3508 得跑位置吧
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;
@@ -40,6 +42,23 @@ Ball ::Ball()
speedSet[i] = 0;
}
+ //两个伸缩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;
+
//小米电机初始化
JZ_xiaomi.position = 0; //
JZ_xiaomi.speed = 20; //
@@ -55,6 +74,35 @@ Ball ::Ball()
}
+void Ball ::Extend_control(int angle)
+{
+ 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]);
+
+
+}
+
+
void Ball ::Spin(float speed)
{
@@ -78,6 +126,7 @@ void Ball::ballDown(void)
void Ball::ballStop(void)
{
+ HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
speedm=0;
}
@@ -98,77 +147,70 @@ void Ball::ballTake(void)
void Ball::ballHadling(void)
{
+ static int last_sw5 = 0; // 保存上一次拨杆状态
+ ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 有球 1 无球 0
- 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 (rc_ctrl.sw[4] > 1000||key > 0) // 检测按键是否被按下,自动回弹的
+ HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
+ // 只在拨杆从非200切到200时触发
+ if ((rc_ctrl.sw[5] == 200 && last_sw5 != 200) || key > 0)
{
- speedm=-4000;
+ speedm = -4000;
currentState1 = BALL_FORWARD; // 切换到正转状态
}
break;
case BALL_FORWARD:
-
- 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 )
+ 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);// 打开气缸
+ HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
currentState1 = BALL_DROP; // 切换到球下落状态
}
break;
case BALL_DROP:
-
- if (ball_state == 0) //读光电 有球 1 无球 0
+ if (ball_state == 0) // 读光电 有球 1 无球 0
{
osDelay(200); // 延时200ms
- speedm=3500;
+ speedm = 3500;
currentState1 = BALL_REVERSE; // 切换到反转状态
}
-
break;
case BALL_REVERSE:
-
- if (ball_state == 1)
+ if (ball_state == 1)
{
-
- speedm=0; // 停止电机
+ speedm = 0; // 停止电机
currentState1 = BALL_CLOSE; // 切换到完成状态
}
-
- break;
+ break;
case BALL_CLOSE:
-
- if(ball_state == 1)
- {
- HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
- currentState1 = BALL_FINISH; // 切换到完成状态
- }
-
- break;
+ if (ball_state == 1)
+ {
+ HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
+ currentState1 = BALL_FINISH; // 切换到完成状态
+ }
+ break;
case BALL_FINISH:
- osDelay(200); // 延时50ms
- key=0; // 重置按键状态
- speedm=0;
+ osDelay(200); // 延时200ms
+ key = 0; // 重置按键状态
+ speedm = 0;
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
- currentState1 = BALL_IDLE; // 回到空闲状态
+ currentState1 = BALL_IDLE; // 直接回到空闲状态
break;
default:
- currentState1 = BALL_IDLE; // 默认回到空闲状态
+ currentState1 = BALL_IDLE; // 默认回到空闲状态
break;
}
-
-
+ last_sw5 = rc_ctrl.sw[5]; // 更新上一次拨杆状态
}
void Ball::Send_control()
@@ -176,6 +218,9 @@ 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);
+
}
#else
diff --git a/User/module/ball.hpp b/User/module/ball.hpp
index 3919c88..4bb3c3e 100644
--- a/User/module/ball.hpp
+++ b/User/module/ball.hpp
@@ -54,6 +54,7 @@ public:
void Send_control(void);
void ballStop(void);
void ballTake(void);
+ void Extend_control(int angle);
BallState_t currentState1; // 当前状态
int flag;//暂时还没用到
@@ -62,6 +63,10 @@ public:
//小米电机伸缩
JZ_xiaomi_t JZ_xiaomi;
+ //伸缩6020
+ int16_t e_result[2];
+ motor_measure_t * Extern_Motor[2];
+
int16_t result[MOTOR_NUM];
motor_measure_t *hand_Motor[MOTOR_NUM];
//==========================公共变量==========================//
@@ -75,12 +80,17 @@ private:
static const float M3508_speed_PID[3];
static const float M3508_angle_PID[3];
+ static const float Extend_speed_PID[3];
+ static const float Extend_angle_PID[3];
//电机速度pid结构体
pid_type_def speed_pid[MOTOR_NUM];
//位置环pid
pid_type_def angle_pid[MOTOR_NUM];
+ pid_type_def extend_speed_pid[2];
+ pid_type_def extend_angle_pid[2];
+
float angleSet[2];
float speedSet[MOTOR_NUM];
};
diff --git a/User/module/shoot.cpp b/User/module/shoot.cpp
index 32e84e1..0b24d5f 100644
--- a/User/module/shoot.cpp
+++ b/User/module/shoot.cpp
@@ -8,24 +8,32 @@
#include "vofa.h"
extern RC_ctrl_t rc_ctrl;
+NUC_t nuc_v;
+
float vofa[8];
#define SHOOT_SPEED 30500
#define SHOOT_SPEED_BACK -2500
#define Error 50
-int test_speed =30500;
+int triggerspeed = -5000; // 扳机速度
+int test_speed =34700;
+float test_distance =3.6;
+float xxx;
#define STOP 0
+int Torque = -5000; // 扳机恒力
#define Trigger_Torque -5000
+#define Trigger_Slow 2000//扳机慢速 不用加负号
//sw[7]👆 1694 中 1000 👇306
//sw[2]👆 1694 👇306
-//F键 sw[0]👆 1800 中 1000 👇200
-//E键 sw[1]👆 1800 👇200
+//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
+//F键 sw[0]👆 1800 开 👇200 关
+//B键 sw[3]👆 200 开 👇1800 关
-const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
+const fp32 Shoot:: M2006_speed_PID[3] = {50, 0, 0};
int t=0;
Shoot::Shoot()
@@ -33,8 +41,8 @@ Shoot::Shoot()
//扳机初始化
trigger_Motor= get_motor_point(4);
trigger_Motor->type=M2006;
- PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,6000, 1000);//pid初始化
- speedSet = 0;
+ PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,8000, 1000);//pid初始化
+ t_speedSet = 0;
result = 0;
//发射摩擦轮
@@ -46,80 +54,84 @@ Shoot::Shoot()
void Shoot::trigger_control()
{
- ///speedSet=speed;
- //result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, speedSet);
- //result = Trigger_Torque;
+ // result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
CAN_cmd_1FF(result,0,0,0,&hcan1);
-
-}
-
-void Shoot::shootThree()
-{
- 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;
-
- }
- 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()
-{
- speed_5065=SHOOT_SPEED_BACK;
- CAN_VESC_Control(1,speed_5065,&hcan2);
-
}
void Shoot::shootStop()
{
- speed_5065=STOP;
- CAN_VESC_Control(1,speed_5065,&hcan2);
-
+ speed_5065=STOP;
+ result=STOP;//扳机慢速归位
+
+
}
-void Shoot::shootStateMachine() {
+void Shoot::errorControl()
+{
+ result=Trigger_Slow;//扳机慢速归位
+ speed_5065=SHOOT_SPEED_BACK; //摩擦归位
+
+
+}
- distance =3.5;
+
+void Shoot :: distanceGet(const NUC_t &nuc_v)
+{
+
+ test_distance=nuc_v.vision.z;
+
+}
+
+void Shoot::vesc_send()
+{
+ CAN_VESC_Control(1, speed_5065, &hcan2);
+}
+
+void Shoot::vofaWatch()
+{
+ vofa[0] = motor_5065[1]->rotor_speed;
+ vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
+ vofa[2] = speed_5065; // 发送电机速度数据
+ vofa[4] = trigger_Motor->speed_rpm; // 发送扳机电机速度数据
+ vofa_tx_main(vofa); // 发送数据到虚拟串口
+}
+
+
+void Shoot::shootStateMachine() {
+
+ xxx=polynomial(test_distance);
+ distance =test_distance;
switch (currentState) {
case SHOOT_IDLE: {
speed_5065=STOP;
- result=STOP;
+ t_speedSet=STOP;
// 空闲状态，等待遥控器输入
- if((rc_ctrl.sw[0]==1800)) {
- currentState = SHOOT_FIRE; // 切换到发射状态
+ if((rc_ctrl.sw[1]==200)) {
+ currentState = SHOOT_READY; // 切换到发射状态
}
break;
}
- case SHOOT_FIRE: {
- // 发射状态，控制电机发射
- // speed_5065 = test_speed;
- speed_5065 =distanceToSpeed(distance);
+ case SHOOT_READY:
+ {
+ // 发射状态，控制电机发射
+ speed_5065 = test_speed;
+ // speed_5065 =polynomial(distance);
if(motor_5065[0]->rotor_speed>=speed_5065-Error && motor_5065[0]->rotor_speed<=speed_5065+Error &&
motor_5065[1]->rotor_speed>=speed_5065-Error && motor_5065[1]->rotor_speed<=speed_5065+Error)
{
- result=Trigger_Torque;//恒力扳机
+ if((rc_ctrl.sw[3]==1800)) {
+ currentState = SHOOT_FIRE; // 切换到发射状态
+ }
+
}
+ break;
+ }
+
+ case SHOOT_FIRE: {
+
+ t_speedSet=triggerspeed;// 扳机速度影响？
// 检测光电传感器是否触发
if (IS_PHOTOELECTRIC_TRIGGERED()) {
currentState = SHOOT_BACK; // 切换到光电触发状态
@@ -127,11 +139,13 @@ void Shoot::shootStateMachine() {
break;
}
+
+
case SHOOT_BACK: {
// 光电触发状态，停止发射
- result=STOP;
+ t_speedSet=STOP;
speed_5065=STOP;
-
+
// 切换到返回状态
currentState = SHOOT_RETURN;
break;
@@ -140,12 +154,12 @@ void Shoot::shootStateMachine() {
case SHOOT_RETURN: {
// 自动返回状态，控制电机反转
speed_5065=SHOOT_SPEED_BACK;
- result=-Trigger_Torque;
+ t_speedSet=2000;
// 检测返回完成（可以通过时间或其他传感器判断）
- if (rc_ctrl.sw[0]==1000) { // 假设遥控器开关控制返回完成
+ if (rc_ctrl.sw[1]==1000) { // 假设遥控器开关控制返回完成
speed_5065=SHOOT_SPEED_BACK;
- result=STOP;
+ t_speedSet=STOP;
currentState = SHOOT_IDLE; // 切换回空闲状态
}
break;
@@ -157,21 +171,31 @@ 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); // 发送数据到虚拟串口
-
+ result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
+ // CAN_VESC_Control(1,speed_5065,&hcan2);
+
}
-//拟合函数
-float distanceToSpeed(float x) {
- return -2.3958f * x * x * x * x
- + 59.615f * x * x * x
- - 525.63f * x * x
- + 2136.4f * x
- + 28001.0f;
+//MATLAB拟合函数
+float polynomial(float x) {
+ return 1185.3918f * powf(x, 5.0f)
+ + -45876.846f * powf(x, 4.0f)
+ + 517061.22f * powf(x, 3.0f)
+ + -2567947.4f * powf(x, 2.0f)
+ + 5954537.4f * x
+ + -5258769.0f;
}
+
+
+float polynomial2(float x) {
+ return 49162.7402f * powf(x, 5.0f)
+ + -917344.6015f * powf(x, 4.0f)
+ + 6840516.5228f * powf(x, 3.0f)
+ + -25479457.6038f * powf(x, 2.0f)
+ + 47406744.9129f * x
+ + -35217628.5565f;
+}
+
+// float polynomial(float x) {
+// return 1185.3918*pow(x,5) + -45876.846*pow(x,4) + 517061.22*pow(x,3) + -2567947.4*pow(x,2) + 5954537.4*x + -5258769;
+// }
diff --git a/User/module/shoot.hpp b/User/module/shoot.hpp
index a034ce1..24ff431 100644
--- a/User/module/shoot.hpp
+++ b/User/module/shoot.hpp
@@ -2,10 +2,12 @@
#define SHOOT_HPP
#include "djiMotor.h"
#include "pid.h"
+#include "nuc.h"
// 定义状态枚举
typedef enum {
SHOOT_IDLE, // 空闲状态
+ SHOOT_READY,
SHOOT_FIRE, // 发射状态
SHOOT_BACK, // 光电触发状态
SHOOT_RETURN // 自动返回状态
@@ -28,16 +30,19 @@ class Shoot
{
public:
Shoot();
- void shootThree(void);
void shootStop(void);
- void shootBack(void);
void shootStateMachine(void);
void trigger_control(void);
+ void errorControl(void);
+ void vofaWatch(void);
+ void vesc_send(void);
+ void distanceGet(const NUC_t &nuc);
float speed_5065;
float speed_trigger;
ShootState_t currentState;
int16_t result;
+ float t_speedSet;
//暂时放在公共,task里使用
CAN_MotorFeedback_t *motor_5065[2];
//==========================公共变量==========================
@@ -54,7 +59,7 @@ private:
//电机速度pid结构体
pid_type_def speed_pid;
motor_measure_t *trigger_Motor;
- float speedSet;
+
};
@@ -62,4 +67,8 @@ private:
float distanceToSpeed(float x);
+float polynomial(float x);
+
+float polynomial2(float x);
+
#endif
diff --git a/User/task/ballTask.cpp b/User/task/ballTask.cpp
index 5da663c..a51e0cc 100644
--- a/User/task/ballTask.cpp
+++ b/User/task/ballTask.cpp
@@ -10,10 +10,10 @@ extern RC_ctrl_t rc_ctrl;
Ball ball;
//float vofa[8];
-// 左旋钮 sw[2] 左1800 右200
-// 右旋钮 sw[3] 左1800 右200
-// C键 sw[4]👆 200 中1000 👇1800
-// D键 sw[5]👆 1800 👇200
+
+// C键 sw[5]👆 1800 中1000 👇200
+// D键 sw[6]👆 200 外伸 👇1800 归位 //三摩擦架子
+// H键 sw[7]👆 200 👇1800 //取球
int angle1=34;
int angle2=23;
@@ -26,7 +26,7 @@ int speed_set=0;
int speed_feedback=0;
extern int speedm;
-extern int speedm1;
+
void FunctionBall(void *argument)
{
@@ -44,42 +44,46 @@ void FunctionBall(void *argument)
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
- if(rc_ctrl.sw[2]>=1200)
+ if(rc_ctrl.sw[6] == 200)
{
angle1=75;
}
- if(rc_ctrl.sw[2]<1200)
+ if(rc_ctrl.sw[6]==1800)
{
angle1=0;
}
+
//运球
- if(rc_ctrl.sw[3]>=1200)
- {
- a1=1;
- //👇
- ball.ballHadling(); // 处理摩擦轮转动
- }
+ if(rc_ctrl.sw[5]==200)
+ {
+ //👇
+ ball.ballHadling(); // 处理摩擦轮转动
+ }
//下球
- if(rc_ctrl.sw[4]==200)
+ if(rc_ctrl.sw[5]==1800)
+ {
+ //👆
+ ball.ballDown();
+ }
+ //中间
+ if(rc_ctrl.sw[7]==200)
+ {
+ if(rc_ctrl.sw[5]==1000)
{
- //👆
- ball.ballDown();
- }
- //回弹停止
- if(rc_ctrl.sw[3]<1000)
- {
- //👆
+ //中
ball.ballStop();
+ ball.ballHadling(); // 处理摩擦轮转动
}
- if(rc_ctrl.sw[7]==1800)
- {
- ball.ballTake();
+ }
- }
+ if(rc_ctrl.sw[7]==1800)
+ {
+ //ball.ballTake();
-
- CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);
+ }
+
+ // CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);
ball.Spin(speedm);
ball.Send_control();
diff --git a/User/task/shootTask.cpp b/User/task/shootTask.cpp
index 4802847..7e79eab 100644
--- a/User/task/shootTask.cpp
+++ b/User/task/shootTask.cpp
@@ -15,10 +15,8 @@ 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
+//E键 sw[0]👆 200 中 1000 👇1800
+//F键 sw[1]👆 1800 👇200
void FunctionShoot(void *argument)
{
@@ -40,35 +38,46 @@ while(1)
if(shoot.flag_thread & BALL_OK)
{
a2=2;
- // shoot.shootThree();
}
+
shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
{
- // 使用接收到的视觉数据调整云台
- //gimbal.gimbalVision(nucData);
+ //shoot.distanceGet(nucData);
}
- if(rc_ctrl.sw[1]>1000)
+
+
+ if(rc_ctrl.sw[0]>1000)
{
- shoot.shootStateMachine();
- if(rc_ctrl.sw[0]==200)
+
+ if(rc_ctrl.sw[1]==1800)
{
- shoot.shootBack();
+ shoot.errorControl();
}
- if(rc_ctrl.sw[0]==1000)
+ if(rc_ctrl.sw[1]==1000)
{
shoot.shootStop();
+ shoot.shootStateMachine();//shoot
+ }
+ if(rc_ctrl.sw[1]==200)
+ {
+ shoot.shootStateMachine();//shoot
}
}
- if(rc_ctrl.sw[1]==200)
+ if(rc_ctrl.sw[0]==200)
{
shoot.shootStop();
}
- shoot.trigger_control();
+ shoot.vesc_send(); // 发送电机速度数据
+
+ shoot.trigger_control();
+
+ shoot.vofaWatch();
+
osDelay(2);
tick += delay_tick; /* 计算下一个唤醒时刻 */