Compare commits
12 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 464544450b | |||
| cbbbc1a0cc | |||
| 873f653116 | |||
| 9bfe4580f8 | |||
| 7e2fb5370a | |||
| 4de285abfb | |||
| 512c223748 | |||
| 94eadcfa16 | |||
| 960778a7fb | |||
| 0abece2ee1 | |||
| 117acf4142 | |||
| 7b54a46bd3 |
20
.vscode/c_cpp_properties.json
vendored
Normal file
20
.vscode/c_cpp_properties.json
vendored
Normal file
@ -0,0 +1,20 @@
|
||||
{
|
||||
"configurations": [
|
||||
{
|
||||
"name": "Win32",
|
||||
"includePath": [
|
||||
"${workspaceFolder}/**"
|
||||
],
|
||||
"defines": [
|
||||
"_DEBUG",
|
||||
"UNICODE",
|
||||
"_UNICODE"
|
||||
],
|
||||
"compilerPath": "D:\\mingw\\MinGW\\bin\\gcc.exe",
|
||||
"cStandard": "c17",
|
||||
"cppStandard": "gnu++17",
|
||||
"intelliSenseMode": "windows-gcc-x64"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
@ -79,6 +79,9 @@ void Error_Handler(void);
|
||||
#define CLOSE_GPIO_Port GPIOE
|
||||
#define DOWN_Pin GPIO_PIN_11
|
||||
#define DOWN_GPIO_Port GPIOE
|
||||
#define TEST_Pin GPIO_PIN_12
|
||||
#define TEST_GPIO_Port GPIOB
|
||||
#define TEST_EXTI_IRQn EXTI15_10_IRQn
|
||||
|
||||
/* USER CODE BEGIN Private defines */
|
||||
|
||||
|
||||
@ -60,6 +60,7 @@ void EXTI9_5_IRQHandler(void);
|
||||
void TIM1_UP_TIM10_IRQHandler(void);
|
||||
void USART1_IRQHandler(void);
|
||||
void USART3_IRQHandler(void);
|
||||
void EXTI15_10_IRQHandler(void);
|
||||
void DMA2_Stream1_IRQHandler(void);
|
||||
void DMA2_Stream2_IRQHandler(void);
|
||||
void DMA2_Stream3_IRQHandler(void);
|
||||
|
||||
@ -64,7 +64,7 @@ void MX_GPIO_Init(void)
|
||||
/*Configure GPIO pin : BALL_Pin */
|
||||
GPIO_InitStruct.Pin = BALL_Pin;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
HAL_GPIO_Init(BALL_GPIO_Port, &GPIO_InitStruct);
|
||||
|
||||
/*Configure GPIO pin : up_ball_Pin */
|
||||
@ -93,6 +93,12 @@ void MX_GPIO_Init(void)
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
|
||||
|
||||
/*Configure GPIO pin : TEST_Pin */
|
||||
GPIO_InitStruct.Pin = TEST_Pin;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
HAL_GPIO_Init(TEST_GPIO_Port, &GPIO_InitStruct);
|
||||
|
||||
/* EXTI interrupt init*/
|
||||
HAL_NVIC_SetPriority(EXTI0_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
|
||||
@ -100,6 +106,9 @@ void MX_GPIO_Init(void)
|
||||
HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
|
||||
|
||||
HAL_NVIC_SetPriority(EXTI15_10_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
|
||||
|
||||
}
|
||||
|
||||
/* USER CODE BEGIN 2 */
|
||||
|
||||
@ -292,6 +292,20 @@ void USART1_IRQHandler(void)
|
||||
// /* USER CODE END USART3_IRQn 1 */
|
||||
//}
|
||||
|
||||
/**
|
||||
* @brief This function handles EXTI line[15:10] interrupts.
|
||||
*/
|
||||
void EXTI15_10_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN EXTI15_10_IRQn 0 */
|
||||
|
||||
/* USER CODE END EXTI15_10_IRQn 0 */
|
||||
HAL_GPIO_EXTI_IRQHandler(TEST_Pin);
|
||||
/* USER CODE BEGIN EXTI15_10_IRQn 1 */
|
||||
|
||||
/* USER CODE END EXTI15_10_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles DMA2 stream1 global interrupt.
|
||||
*/
|
||||
|
||||
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
@ -3,26 +3,26 @@
|
||||
{
|
||||
"name": "R1",
|
||||
"includePath": [
|
||||
"d:\\Desktop\\r1\\R1_up\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\r1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\r1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
|
||||
"d:\\Desktop\\r1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\r1\\R1_up\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\r1\\R1_up\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\R1_up\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\R1_up\\User\\module",
|
||||
"d:\\Desktop\\r1\\R1_up\\User\\task",
|
||||
"d:\\Desktop\\r1\\R1_up\\User\\lib",
|
||||
"d:\\Desktop\\r1\\R1_up\\User\\device",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Core\\Inc",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\User\\bsp",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\User\\module",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\User\\task",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\User\\lib",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\User\\device",
|
||||
"D:\\keil\\ARM\\ARMCC\\include",
|
||||
"D:\\keil\\ARM\\ARMCC\\include\\rw",
|
||||
"d:\\Desktop\\r1\\R1_up\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\R1_up\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\r1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\r1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
"d:\\Desktop\\运球赛\\R1_up\\MDK-ARM",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Core\\Src",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\运球赛\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
],
|
||||
"defines": [
|
||||
"USE_HAL_DRIVER",
|
||||
|
||||
94
MDK-ARM/.vscode/keil-assistant.log
vendored
94
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -74,93 +74,43 @@
|
||||
|
||||
[info] Log at : 2025/6/25|19:23:57|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/27|11:41:11|GMT+0800
|
||||
[info] Log at : 2025/6/26|03:33:46|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/27|15:32:00|GMT+0800
|
||||
<<<<<<< HEAD
|
||||
[info] Log at : 2025/6/27|10:05:14|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/27|18:43:18|GMT+0800
|
||||
[info] Log at : 2025/6/27|23:24:18|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/28|01:19:54|GMT+0800
|
||||
[info] Log at : 2025/6/28|01:53:31|GMT+0800
|
||||
=======
|
||||
[info] Log at : 2025/6/26|15:46:24|GMT+0800
|
||||
>>>>>>> 7b54a46bd3c69e44a04ffd669bb7d3b115fc9e9b
|
||||
|
||||
[info] Log at : 2025/6/28|14:29:33|GMT+0800
|
||||
[info] Log at : 2025/6/30|23:46:31|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/28|16:39:11|GMT+0800
|
||||
[info] Log at : 2025/6/30|23:48:14|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/28|16:39:57|GMT+0800
|
||||
[info] Log at : 2025/6/30|23:49:51|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/28|18:37:20|GMT+0800
|
||||
[info] Log at : 2025/7/1|00:00:07|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/29|15:33:54|GMT+0800
|
||||
[info] Log at : 2025/7/1|14:59:17|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/30|10:24:59|GMT+0800
|
||||
[info] Log at : 2025/7/2|14:47:50|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/1|10:57:51|GMT+0800
|
||||
[info] Log at : 2025/7/2|19:49:16|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/2|02:59:15|GMT+0800
|
||||
[info] Log at : 2025/7/9|09:48:19|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/2|17:15:33|GMT+0800
|
||||
[info] Log at : 2025/7/9|09:49:07|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/4|09:10:46|GMT+0800
|
||||
[info] Log at : 2025/7/10|15:36:20|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/6|22:01:26|GMT+0800
|
||||
[info] Log at : 2025/7/11|18:17:38|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/6|22:02:13|GMT+0800
|
||||
[info] Log at : 2025/7/13|01:22:22|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/6|23:12:04|GMT+0800
|
||||
[info] Log at : 2025/7/13|01:27:09|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/7|04:53:38|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/7|08:44:07|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/7|14:20:11|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/8|02:49:12|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/8|13:15:42|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/8|18:00:22|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/8|20:39:07|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/9|08:39:28|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/9|11:36:02|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/9|16:01:34|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/9|23:48:02|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/10|01:59:23|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/10|15:33:45|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/10|17:26:59|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/10|22:12:10|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/11|10:11:25|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/11|17:39:11|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/12|23:45:28|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/13|11:08:55|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/13|12:29:41|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/13|16:33:17|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/13|16:42:25|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/13|22:38:15|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/14|07:52:29|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/14|12:31:22|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/14|13:56:03|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/16|22:15:00|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/16|22:26:06|GMT+0800
|
||||
[info] Log at : 2025/7/13|08:37:10|GMT+0800
|
||||
|
||||
|
||||
7
MDK-ARM/.vscode/uv4.log
vendored
7
MDK-ARM/.vscode/uv4.log
vendored
@ -1,9 +1,8 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1'
|
||||
compiling ballTask.cpp...
|
||||
compiling shootTask.cpp...
|
||||
compiling ball.cpp...
|
||||
linking...
|
||||
Program Size: Code=31984 RO-data=1832 RW-data=272 ZI-data=32264
|
||||
Program Size: Code=29156 RO-data=1832 RW-data=300 ZI-data=32220
|
||||
FromELF: creating hex file...
|
||||
"R1\R1.axf" - 0 Error(s), 0 Warning(s).
|
||||
Build Time Elapsed: 00:00:05
|
||||
Build Time Elapsed: 00:00:04
|
||||
|
||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/7/16 22:30:59
|
||||
2025/7/13 7:05:23
|
||||
@ -154,76 +154,43 @@
|
||||
</SetRegEntry>
|
||||
</TargetDriverDllRegistry>
|
||||
<Breakpoint/>
|
||||
<<<<<<< HEAD
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
>>>>>>> 上层测试
|
||||
<WatchWindow1>
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc_ctrl,0x0A</ItemText>
|
||||
<ItemText>ttttt1,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>1</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>shoot,0x0A</ItemText>
|
||||
<ItemText>ball,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<<<<<<< HEAD
|
||||
<ItemText>ball,0x0A</ItemText>
|
||||
=======
|
||||
<ItemText>ball</ItemText>
|
||||
>>>>>>> 上层测试
|
||||
<ItemText>nucbuf</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<<<<<<< HEAD
|
||||
<ItemText>nucbuf</ItemText>
|
||||
=======
|
||||
<ItemText>and1</ItemText>
|
||||
>>>>>>> 上层测试
|
||||
<ItemText>runCount,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<<<<<<< HEAD
|
||||
<ItemText>nuc_v</ItemText>
|
||||
=======
|
||||
<ItemText>and1</ItemText>
|
||||
>>>>>>> 上层测试
|
||||
<ItemText>rc_ctrl,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<<<<<<< HEAD
|
||||
<ItemText>abc,0x0A</ItemText>
|
||||
=======
|
||||
<ItemText>nucbuf</ItemText>
|
||||
>>>>>>> 上层测试
|
||||
<ItemText>runCount,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<<<<<<< HEAD
|
||||
<ItemText>shoot_wait,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>error_code</ItemText>
|
||||
<ItemText>ball_task_active,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
=======
|
||||
>>>>>>> 上层测试
|
||||
=======
|
||||
<ItemText>drop_message,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
>>>>>>> 上层测试
|
||||
<MemoryWindow4>
|
||||
<Mm>
|
||||
<WinNumber>4</WinNumber>
|
||||
|
||||
@ -36,29 +36,10 @@ r1上层
|
||||
+ 拨置👇发射清空掉落信号
|
||||
|
||||
+ 用一个攻守方档
|
||||
+ 初始移动到最上面 更待蓄力(不管攻方守方都在最上面等待)
|
||||
+ 攻方时拨下立马蓄力并伸出(可小角度)
|
||||
+ 守方时不动并保持缩回
|
||||
+ 👇 运球档正常运球
|
||||
+ 中 初始档直接缩回
|
||||
+ 👆 配合档 完成配合并伸出才能发射
|
||||
|
||||
+ 传球模式
|
||||
+ 自动
|
||||
+ 底盘的传球对准档拨下
|
||||
+ 我的蓄力进入传球拟合
|
||||
+ 继续拨下发射
|
||||
+ 手动
|
||||
+ 目前只能打固定距离
|
||||
+ 切相同传球档 自动蓄力到传球力度
|
||||
+ 发射档发射
|
||||
+ 图传多距离
|
||||
+ 传球档
|
||||
+ 旋钮+看图传点位调整
|
||||
|
||||
+ 修复
|
||||
+ 6.29 发射误操作导致没有拟合作用就射了(已修复)
|
||||
+ 6.29 串口不稳定 重新拔插一下
|
||||
+ 6.29 nuc位置更新慢
|
||||
+ 6.29 添加光电上电保护防止跳尺(已添加)
|
||||
|
||||
+ 运球赛专属简单代码
|
||||
|
||||
24
R1.ioc
24
R1.ioc
@ -153,20 +153,21 @@ Mcu.Pin22=PD14
|
||||
Mcu.Pin23=PA0-WKUP
|
||||
Mcu.Pin24=PE13
|
||||
Mcu.Pin25=PE11
|
||||
Mcu.Pin26=PA7
|
||||
Mcu.Pin27=VP_CRC_VS_CRC
|
||||
Mcu.Pin28=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin29=VP_SYS_VS_Systick
|
||||
Mcu.Pin26=PB12
|
||||
Mcu.Pin27=PA7
|
||||
Mcu.Pin28=VP_CRC_VS_CRC
|
||||
Mcu.Pin29=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin3=PB3
|
||||
Mcu.Pin30=VP_TIM4_VS_ClockSourceINT
|
||||
Mcu.Pin31=VP_TIM10_VS_ClockSourceINT
|
||||
Mcu.Pin30=VP_SYS_VS_Systick
|
||||
Mcu.Pin31=VP_TIM4_VS_ClockSourceINT
|
||||
Mcu.Pin32=VP_TIM10_VS_ClockSourceINT
|
||||
Mcu.Pin4=PA14
|
||||
Mcu.Pin5=PA13
|
||||
Mcu.Pin6=PB7
|
||||
Mcu.Pin7=PB6
|
||||
Mcu.Pin8=PD0
|
||||
Mcu.Pin9=PC11
|
||||
Mcu.PinsNb=32
|
||||
Mcu.PinsNb=33
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
Mcu.UserName=STM32F407IGHx
|
||||
@ -184,6 +185,7 @@ NVIC.DMA2_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.EXTI0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.EXTI9_5_IRQn=true\:5\:0\:true\:false\:true\:false\:true\:true\:true
|
||||
NVIC.ForceEnableDMAVector=true
|
||||
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
@ -218,6 +220,12 @@ PA7.Signal=SPI1_MOSI
|
||||
PA9.Locked=true
|
||||
PA9.Mode=Asynchronous
|
||||
PA9.Signal=USART1_TX
|
||||
PB12.GPIOParameters=GPIO_PuPd,GPIO_Label,GPIO_ModeDefaultEXTI
|
||||
PB12.GPIO_Label=TEST
|
||||
PB12.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING
|
||||
PB12.GPIO_PuPd=GPIO_PULLDOWN
|
||||
PB12.Locked=true
|
||||
PB12.Signal=GPXTI12
|
||||
PB3.GPIOParameters=GPIO_PuPd
|
||||
PB3.GPIO_PuPd=GPIO_PULLUP
|
||||
PB3.Mode=Full_Duplex_Master
|
||||
@ -369,6 +377,8 @@ RCC.VCOOutputFreq_Value=336000000
|
||||
RCC.VcooutputI2S=192000000
|
||||
SH.GPXTI0.0=GPIO_EXTI0
|
||||
SH.GPXTI0.ConfNb=1
|
||||
SH.GPXTI12.0=GPIO_EXTI12
|
||||
SH.GPXTI12.ConfNb=1
|
||||
SH.GPXTI7.0=GPIO_EXTI7
|
||||
SH.GPXTI7.ConfNb=1
|
||||
SH.S_TIM10_CH1.0=TIM10_CH1,PWM Generation1 CH1
|
||||
|
||||
59
README.md
59
README.md
@ -5,60 +5,19 @@ r1上层
|
||||
## 外设
|
||||
|
||||
+ CAN1
|
||||
+ 扳机2006 id:0x201
|
||||
+ CAN2
|
||||
+ 小米电机 id:1
|
||||
- 扳机2006 id:0x205
|
||||
- 三摩擦 id:123
|
||||
+ UART
|
||||
+ uart1 波特率4000000 id：2
|
||||
+ uart6 nuc
|
||||
+ uart3 遥控器接收
|
||||
- uart1 波特率4000000 id：2
|
||||
- uart6 nuc
|
||||
- uart3 遥控器接收
|
||||
+ GPIO
|
||||
+ PI6运球光电
|
||||
+ PE13 爪气缸
|
||||
+ PE14 砸气缸
|
||||
- PI6运球光电
|
||||
- PE11 运球气缸
|
||||
|
||||
|
||||
|
||||
## 遥控器
|
||||
|
||||
## 待解决
|
||||
|
||||
+ 用了将运球和伸缩绑定到一起 √
|
||||
+ 串口收数加个滤波 √
|
||||
|
||||
## 思路
|
||||
|
||||
+ 👆 传球档 👆 配合档
|
||||
+ 中 初始档 中 初始档
|
||||
+ 👇 发射档 👇 运球档
|
||||
+ 起步遥控档我直接蓄力准备接球 + 可多次的运球
|
||||
+ 缩回转移球
|
||||
+ 蓄力到位,收到掉落信号和已伸出信号
|
||||
+ 根据视觉拟合信息的动态调整
|
||||
+ 拨置👇发射清空掉落信号
|
||||
|
||||
+ 用一个攻守方档
|
||||
+ 初始移动到最上面 更待蓄力(不管攻方守方都在最上面等待)
|
||||
+ 攻方时拨下立马蓄力并伸出(可小角度)
|
||||
+ 守方时不动并保持缩回
|
||||
+ 👇 运球档正常运球
|
||||
+ 中 初始档直接缩回
|
||||
+ 👆 配合档 完成配合并伸出才能发射
|
||||
|
||||
+ 传球模式
|
||||
+ 自动
|
||||
+ 底盘的传球对准档拨下
|
||||
+ 我的蓄力进入传球拟合
|
||||
+ 继续拨下发射
|
||||
+ 手动
|
||||
+ 目前只能打固定距离
|
||||
+ 切相同传球档 自动蓄力到传球力度
|
||||
+ 发射档发射
|
||||
+ 图传多距离
|
||||
+ 传球档
|
||||
+ 旋钮+看图传点位调整
|
||||
|
||||
+ 修复
|
||||
+ 6.29 发射误操作导致没有拟合作用就射了(已修复)
|
||||
+ 6.29 串口不稳定 重新拔插一下
|
||||
+ 6.29 nuc位置更新慢
|
||||
+ 6.29 添加光电上电保护防止跳尺(已添加)
|
||||
|
||||
@ -16,7 +16,10 @@
|
||||
#endif
|
||||
|
||||
|
||||
#define ONE_CONTROL 1
|
||||
#define ONE_CONTROL 0
|
||||
|
||||
|
||||
|
||||
|
||||
//是否使用大疆DT7遥控器
|
||||
#ifndef DT7
|
||||
@ -28,8 +31,7 @@
|
||||
//================任务通知,时间组================//
|
||||
//事件组
|
||||
#define EVENT_RC (1<<1)
|
||||
#define EVENT_CAN1 (1<<2)
|
||||
#define EVENT_CAN2 (1<<3)
|
||||
#define EVENT_CAN (1<<2)
|
||||
|
||||
//================任务通知================//
|
||||
//运球
|
||||
@ -41,10 +43,6 @@
|
||||
#define HANDING_FINISH (1<<2)
|
||||
//伸缩结束
|
||||
#define EXTEND_OK (1<<3)
|
||||
//等待ok
|
||||
#define WAIT_OK (1<<4)
|
||||
//可以防守收回
|
||||
#define DEF_READY (1<<5)
|
||||
|
||||
//要发送ok了
|
||||
#define BALL_SEND (1<<6)
|
||||
|
||||
@ -20,23 +20,23 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
// int8_t BSP_GPIO_RegisterCallback(uint16_t pin, void (*callback)(void)) {
|
||||
// if (callback == NULL) return BSP_ERR_NULL;
|
||||
int8_t BSP_GPIO_RegisterCallback(uint16_t pin, void (*callback)(void)) {
|
||||
if (callback == NULL) return BSP_ERR_NULL;
|
||||
|
||||
// for (uint8_t i = 0; i < 16; i++) {
|
||||
// if (pin & (1 << i)) {
|
||||
// GPIO_Callback[i] = callback;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
// return BSP_OK;
|
||||
// }
|
||||
for (uint8_t i = 0; i < 16; i++) {
|
||||
if (pin & (1 << i)) {
|
||||
GPIO_Callback[i] = callback;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
// int8_t BSP_GPIO_EnableIRQ(uint16_t pin) {
|
||||
// switch (pin) {
|
||||
|
||||
// case KEY_Pin:
|
||||
// HAL_NVIC_EnableIRQ(KEY_Pin);
|
||||
// case USER_KEY_Pin:
|
||||
// HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn);
|
||||
// break;
|
||||
|
||||
// /*
|
||||
@ -54,9 +54,9 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
|
||||
// int8_t BSP_GPIO_DisableIRQ(uint16_t pin) {
|
||||
// switch (pin) {
|
||||
|
||||
// case KEY_Pin:
|
||||
// HAL_NVIC_DisableIRQ(KEY_IRQn);
|
||||
// break;
|
||||
// // case USER_KEY_Pin:
|
||||
// // HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn);
|
||||
// // break;
|
||||
|
||||
// /*
|
||||
// case XXX_Pin:
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
#include "bsp_delay.h"
|
||||
int key=0;
|
||||
int ball_exit=0;
|
||||
int test_exit=0;
|
||||
|
||||
// 按键中断回调函数
|
||||
void detect_exit(void)
|
||||
@ -15,17 +16,35 @@ void detect_exit(void)
|
||||
void detect_ball(void)
|
||||
{
|
||||
|
||||
ball_exit++; // 按键按下时变量自增
|
||||
// ball_exit++; // 按键按下时变量自增
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(BALL_Pin); // 清除中断标志位
|
||||
}
|
||||
|
||||
void detect_test(void)
|
||||
{
|
||||
delay_ms(10); // 延时10ms
|
||||
if(HAL_GPIO_ReadPin(TEST_GPIO_Port, TEST_Pin) == GPIO_PIN_RESET)
|
||||
{
|
||||
ball_exit++;
|
||||
}
|
||||
|
||||
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(TEST_Pin); // 清除中断标志位
|
||||
}
|
||||
|
||||
void detect_init(void)
|
||||
{
|
||||
|
||||
// BSP_GPIO_RegisterCallback(KEY_Pin, detect_exit);
|
||||
// BSP_GPIO_RegisterCallback(BALL_Pin, detect_ball);
|
||||
BSP_GPIO_RegisterCallback(KEY_Pin, detect_exit);
|
||||
BSP_GPIO_RegisterCallback(BALL_Pin, detect_ball);
|
||||
BSP_GPIO_RegisterCallback(TEST_Pin, detect_test);
|
||||
|
||||
// // 启用按键中断
|
||||
// if (BSP_GPIO_EnableIRQ(KEY_Pin) != BSP_OK) {
|
||||
// // 错误处理
|
||||
// }
|
||||
// if (BSP_GPIO_EnableIRQ(BALL_Pin) != BSP_OK) {
|
||||
// // 错误处理
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
@ -215,13 +215,15 @@ static osThreadId_t thread_alert;
|
||||
void Dji_Motor_CB()
|
||||
{
|
||||
HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
|
||||
osThreadFlagsSet(thread_alert, EVENT_CAN1);
|
||||
|
||||
osThreadFlagsSet(thread_alert, EVENT_CAN);
|
||||
// osEventFlagsSet(eventReceive, EVENT_CAN);
|
||||
}
|
||||
void can2_Motor_CB(void)
|
||||
{
|
||||
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
|
||||
osThreadFlagsSet(thread_alert, EVENT_CAN2);
|
||||
|
||||
//osThreadFlagsSet(thread_alert, EVENT_CAN);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -232,6 +234,9 @@ void can2_Motor_CB(void)
|
||||
void djiInit(void)
|
||||
{
|
||||
thread_alert = osThreadGetId();
|
||||
|
||||
|
||||
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
|
||||
Dji_Motor_CB);
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
|
||||
@ -245,9 +250,11 @@ void djiInit(void)
|
||||
*/
|
||||
uint32_t waitNewDji()
|
||||
{
|
||||
// 等待CAN1或CAN2任意一个事件
|
||||
// return osEventFlagsWait(
|
||||
// eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
|
||||
return osThreadFlagsWait(
|
||||
EVENT_CAN1 | EVENT_CAN2, osFlagsWaitAny, osWaitForever);
|
||||
EVENT_CAN, osFlagsWaitAll, osWaitForever);
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@ static osThreadId_t thread_alert;
|
||||
|
||||
volatile uint32_t drop_message = 0;
|
||||
|
||||
uint8_t nucbuf[18];
|
||||
uint8_t nucbuf[6];
|
||||
uint8_t packet[32]; // 假设最大数据包长度为 32 字节
|
||||
|
||||
static void NUC_CBLTCallback(void)
|
||||
@ -150,19 +150,19 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
else
|
||||
{
|
||||
|
||||
if (nucbuf[17] != TAIL) goto error;
|
||||
if (nucbuf[5] != TAIL) goto error;
|
||||
|
||||
instance.data[3] = nucbuf[8];
|
||||
instance.data[2] = nucbuf[7];
|
||||
instance.data[1] = nucbuf[6];
|
||||
instance.data[0] = nucbuf[5];
|
||||
instance.data[3] = nucbuf[4];
|
||||
instance.data[2] = nucbuf[3];
|
||||
instance.data[1] = nucbuf[2];
|
||||
instance.data[0] = nucbuf[1];
|
||||
n->vision.x = instance.x[0];
|
||||
|
||||
instance.data[7] = nucbuf[16];
|
||||
instance.data[6] = nucbuf[15];
|
||||
instance.data[5] = nucbuf[14];
|
||||
instance.data[4] = nucbuf[13];
|
||||
n->vision.y = instance.x[1];
|
||||
// instance.data[7] = nucbuf[16];
|
||||
// instance.data[6] = nucbuf[15];
|
||||
// instance.data[5] = nucbuf[14];
|
||||
// instance.data[4] = nucbuf[13];
|
||||
// n->vision.y = instance.x[1];
|
||||
|
||||
}
|
||||
return DEVICE_OK;
|
||||
@ -247,6 +247,3 @@ int8_t NUC_HandleOffline(NUC_t *cmd)
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -165,7 +165,6 @@ fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max)
|
||||
return Limit_min;
|
||||
}
|
||||
|
||||
return *num; // 如果在范围内，直接返回原值
|
||||
return *num;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -6,10 +6,14 @@
|
||||
#include "user_math.h"
|
||||
#include "shoot.hpp"
|
||||
|
||||
NUC_t nuc_d;
|
||||
|
||||
#define HANGDING_TWO 0 // 是否使用两次运球
|
||||
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
extern int ball_exit;
|
||||
|
||||
// 外死点168 外163 中150 内127 限位124.8
|
||||
|
||||
// 伸缩
|
||||
//外死点89 外85 中75 内49 限位46
|
||||
|
||||
@ -18,18 +22,22 @@ extern int ball_exit;
|
||||
#define WAIT_POS 75
|
||||
#define HANGDING_POS 89
|
||||
|
||||
// PE11 气缸git stash apply
|
||||
uint8_t stop_flag[3] = {0XFF,0X01,0XFE};
|
||||
|
||||
// PE11 气缸
|
||||
|
||||
Ball ::Ball()
|
||||
{
|
||||
detect_init();
|
||||
|
||||
// 小米电机
|
||||
feedback = get_CyberGear_point();
|
||||
|
||||
// 小米电机初始化
|
||||
xiaomi.position = I_ANGLE; //
|
||||
xiaomi.speed = 35; //
|
||||
xiaomi.K_P = 40; // 位置增益
|
||||
xiaomi.K_D = 100; // 位置阻尼
|
||||
xiaomi.speed = 40; //
|
||||
xiaomi.K_P = 80; // 位置增益
|
||||
xiaomi.K_D = 20; // 位置阻尼
|
||||
xiaomi.K_C = 12; // 力矩
|
||||
xiaomi.Pmax = 1; // 好像没啥用
|
||||
|
||||
@ -48,7 +56,11 @@ Ball ::Ball()
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800
|
||||
// G键 sw[6]👆 200 中 1000 👇1800
|
||||
// sw[5] 👆 200 👇1800
|
||||
// H键 sw[6] 👆200 👇1800
|
||||
// 左旋 sw[7] 200 --1800
|
||||
|
||||
int last_sw5 = 1800; // 上一次拨杆状态
|
||||
|
||||
void Ball::rc_mode()
|
||||
{
|
||||
if (rc_ctrl.sw[6] == 200)
|
||||
@ -73,14 +85,15 @@ void Ball::rc_mode()
|
||||
extern_key = OUT;
|
||||
}
|
||||
|
||||
if (rc_ctrl.sw[5] == 1800)
|
||||
{
|
||||
ready_key = SIDE;
|
||||
}
|
||||
if (rc_ctrl.sw[5] == 200)
|
||||
{
|
||||
ready_key = DEF; // 默认不准备
|
||||
}
|
||||
// if (rc_ctrl.sw[5] == 200 && last_sw5 == 1800) // 只在1800->200的瞬间触发
|
||||
// {
|
||||
// rc_key = DOWN2;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// rc_key = 0; // 其他情况不触发
|
||||
// }
|
||||
// last_sw5 = rc_ctrl.sw[5];
|
||||
}
|
||||
|
||||
void Ball::Send_control()
|
||||
@ -99,6 +112,8 @@ void Ball::ballDown(void)
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Ball::Move_Extend()
|
||||
{
|
||||
if (extern_key == IN)
|
||||
@ -108,45 +123,431 @@ void Ball::Move_Extend()
|
||||
if (extern_key == OUT)
|
||||
{
|
||||
xiaomi.position = HANGDING_POS;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
int runCount = 0;
|
||||
int last_ball_state = 0;
|
||||
int step = 0;
|
||||
int ball_task_active = 0;
|
||||
int last_data = 0; // 新增
|
||||
|
||||
void Ball::data_get(const NUC_t &nuc_d)
|
||||
{
|
||||
data = nuc_d.vision.x;
|
||||
// 只在data从0变为非0时激活流程（上升沿触发）
|
||||
if (data != 0 && last_data == 0 && ball_task_active == 0) {
|
||||
ball_task_active = 1;
|
||||
runCount = 0; // 只在收到新指令时清零
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
last_data = data;
|
||||
}
|
||||
|
||||
#if HANGDING_TWO == 0
|
||||
|
||||
void Ball::ball_control()
|
||||
{
|
||||
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);
|
||||
|
||||
if (ball_task_active == 0) {
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
currentState1 = BALL_IDLE;
|
||||
return;
|
||||
}
|
||||
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_FORWARD:
|
||||
osDelay(500);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);
|
||||
osDelay(5);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);
|
||||
currentState1 = BALL_DROP;
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
osDelay(100);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
if (ball_state == 1 && last_ball_state == 0) {
|
||||
currentState1 = BALL_FLAG;
|
||||
}
|
||||
last_ball_state = ball_state;
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
osDelay(10);
|
||||
if (ball_state == 0 && last_ball_state == 1) {
|
||||
currentState1 = BALL_CLOSE;
|
||||
}
|
||||
last_ball_state = ball_state;
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
osDelay(100);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
currentState1 = BALL_FINISH;
|
||||
break;
|
||||
|
||||
case BALL_FINISH:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
runCount++;
|
||||
if (runCount < 2) {
|
||||
currentState1 = BALL_FORWARD;
|
||||
} else {
|
||||
// 运完两次，流程结束
|
||||
ball_task_active = 0;
|
||||
data = 0;
|
||||
HAL_UART_Transmit(&huart6, stop_flag, 3, 10);
|
||||
osDelay(500);
|
||||
// runCount 不要在这里清零
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// int runCount = 0; // 运球次数计数
|
||||
// int last_ball_state = 0; // 上一次的光电状态
|
||||
// int step = 0; // 0:持球 1:击球 2:反弹 3:完全离开
|
||||
// int ball_task_active = 0; // 运球流程激活标志
|
||||
// int ifyun=0;
|
||||
|
||||
// void Ball::data_get(const NUC_t &nuc_d)
|
||||
// {
|
||||
// data = nuc_d.vision.x;
|
||||
// // 收到上位机新指令且未在运球流程中，激活流程
|
||||
// if (data !=0 && ball_task_active == 0) {
|
||||
// ball_task_active = 1;
|
||||
// runCount = 0;
|
||||
// ifyun=0;
|
||||
// currentState1 = BALL_FORWARD;
|
||||
// }
|
||||
// }
|
||||
|
||||
// #if HANGDING_TWO == 0
|
||||
|
||||
// void Ball::ball_control()
|
||||
// {
|
||||
// ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
|
||||
|
||||
// if (ball_task_active == 0) {
|
||||
// // 未激活流程，保持空闲
|
||||
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
// currentState1 = BALL_IDLE;
|
||||
// return;
|
||||
// }
|
||||
// if(ifyun==0){
|
||||
// switch (currentState1)
|
||||
// {
|
||||
// case BALL_FORWARD:
|
||||
// osDelay(100);
|
||||
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);
|
||||
// osDelay(5);
|
||||
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);
|
||||
// currentState1 = BALL_DROP;
|
||||
// break;
|
||||
|
||||
// case BALL_DROP:
|
||||
// osDelay(100);
|
||||
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
// if (ball_state == 1 && last_ball_state == 0) {
|
||||
// currentState1 = BALL_FLAG;
|
||||
// }
|
||||
// last_ball_state = ball_state;
|
||||
// break;
|
||||
|
||||
// case BALL_FLAG:
|
||||
// osDelay(10);
|
||||
// if (ball_state == 0 && last_ball_state == 1) {
|
||||
// currentState1 = BALL_CLOSE;
|
||||
// }
|
||||
// last_ball_state = ball_state;
|
||||
// break;
|
||||
|
||||
// case BALL_CLOSE:
|
||||
// osDelay(100);
|
||||
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
// currentState1 = BALL_FINISH;
|
||||
// break;
|
||||
|
||||
// case BALL_FINISH:
|
||||
// //osDelay(200);
|
||||
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
// runCount++;
|
||||
// if (runCount < 2) {
|
||||
// currentState1 = BALL_FORWARD;
|
||||
|
||||
// } else {
|
||||
// // 运完两次，流程结束
|
||||
// ball_task_active = 0;
|
||||
// data = 0;
|
||||
// // 通过串口发送stop_flag数组给上位机，通知停止
|
||||
// HAL_UART_Transmit(&huart6, stop_flag, 3, 10);
|
||||
// ifyun=1;
|
||||
// currentState1 = BALL_IDLE;
|
||||
// }
|
||||
// break;
|
||||
|
||||
// default:
|
||||
// currentState1 = BALL_IDLE;
|
||||
// break;
|
||||
// }
|
||||
|
||||
// }
|
||||
// }
|
||||
#else
|
||||
|
||||
void Ball::ball_control()
|
||||
{
|
||||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0，无球 1)
|
||||
|
||||
Move_Extend();
|
||||
|
||||
switch (rc_key)
|
||||
switch (currentState1)
|
||||
{
|
||||
case MIDDLE2:
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (currentState1 == BALL_FINISH)
|
||||
if (rc_key == DOWN2) // 检测按键是否被按下
|
||||
{
|
||||
currentState1 = BALL_IDLE;
|
||||
runCount = 0; // 每次拨动重新计数
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
osDelay(5);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
osDelay(100); // 延时 100ms
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||||
// 一直检测有球（ball_state == 0），等球离开
|
||||
if (ball_state == 1 && last_ball_state == 0) // 球离开
|
||||
{
|
||||
currentState1 = BALL_FLAG;
|
||||
}
|
||||
last_ball_state = ball_state;
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
osDelay(10); // 延时 50ms
|
||||
// 等待球弹回再次检测到球
|
||||
if (ball_state == 0 && last_ball_state == 1) // 球弹回
|
||||
{
|
||||
currentState1 = BALL_CLOSE;
|
||||
}
|
||||
last_ball_state = ball_state;
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
osDelay(100);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
break;
|
||||
|
||||
case BALL_FINISH:
|
||||
osDelay(200); // 延时 50ms
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
runCount++;
|
||||
osDelay(200); // 延时 50ms
|
||||
if (runCount < 2)
|
||||
{
|
||||
currentState1 = BALL_FORWARD; // 继续下一次运球
|
||||
osDelay(50);
|
||||
}
|
||||
else
|
||||
{
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
currentState1 = BALL_IDLE; // 完成两次，回到空闲
|
||||
}
|
||||
break;
|
||||
|
||||
case UP2:
|
||||
ballDown();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
ballHadling();
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
|
||||
Send_control();
|
||||
}
|
||||
|
||||
int ball_state = 0;
|
||||
int last_ball_state = 0; // 上一次的光电状态
|
||||
#endif
|
||||
|
||||
void Ball::Close(void)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
}
|
||||
|
||||
void Ball::Hadling(void)
|
||||
{
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (rc_key == DOWN2)
|
||||
{
|
||||
ball_exit = 0; // 进入流程时清零
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
osDelay(5);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
osDelay(100); // 延时 100ms
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||||
// 只响应第一次边沿
|
||||
if (ball_exit == 1)
|
||||
{
|
||||
currentState1 = BALL_FLAG;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Ball::exit_Handling(void)
|
||||
{
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (rc_key == DOWN2)
|
||||
{
|
||||
ball_exit = 0; // 进入流程时清零
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
osDelay(5);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
osDelay(100); // 延时 100ms
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||||
// 只响应第一次边沿
|
||||
if (ball_exit == 1)
|
||||
{
|
||||
currentState1 = BALL_FLAG;
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
// osDelay(10); // 延时 10ms
|
||||
// 只响应第二次边沿
|
||||
if (ball_exit == 3)
|
||||
{
|
||||
currentState1 = BALL_CLOSE;
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
osDelay(25);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
break;
|
||||
|
||||
case BALL_FINISH:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
ball_exit = 0; // 流程完成后清零
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Ball::test_up(void)
|
||||
{
|
||||
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (rc_key == DOWN2) // 检测按键是否被按下
|
||||
{
|
||||
step = 0;
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
osDelay(5);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
osDelay(50); // 延时 100ms
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||||
|
||||
// 步骤0：等待球离开（持球->无球）
|
||||
if (step == 0 && ball_state == 1 && last_ball_state == 0)
|
||||
{
|
||||
step = 1;
|
||||
}
|
||||
// 步骤1：等待球反弹回来（无球->持球）
|
||||
else if (step == 1 && ball_state == 0 && last_ball_state == 1)
|
||||
{
|
||||
step = 2;
|
||||
}
|
||||
// 步骤2：等待球完全上升离开（持球->无球）
|
||||
else if (step == 2 && ball_state == 1 && last_ball_state == 0)
|
||||
{
|
||||
step = 3;
|
||||
currentState1 = BALL_CLOSE; // 进入闭合气缸
|
||||
}
|
||||
last_ball_state = ball_state;
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
// osDelay(25);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到完成状态
|
||||
break;
|
||||
|
||||
case BALL_FINISH:
|
||||
osDelay(50); // 延时 50ms
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
// currentState1 = BALL_IDLE; // 可选：回到空闲状态
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
@ -211,209 +612,30 @@ void Ball::ballHadling(void)
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
void Ball::ball_control()
|
||||
void Ball::ballHadling_two(void)
|
||||
{
|
||||
hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0，无球 1)
|
||||
|
||||
|
||||
// 进攻
|
||||
if (ready_key == SIDE)
|
||||
switch (currentState1)
|
||||
{
|
||||
osThreadFlagsClear(DEF_READY);
|
||||
switch (rc_key)
|
||||
{
|
||||
case MIDDLE2:
|
||||
Idle_control();
|
||||
break;
|
||||
|
||||
case UP2:
|
||||
ballDown();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
ballHadling();
|
||||
break;
|
||||
}
|
||||
|
||||
Send_control();
|
||||
}
|
||||
// 防守
|
||||
else if(ready_key == DEF)
|
||||
{
|
||||
if(hand_thread & DEF_READY)
|
||||
{
|
||||
xiaomi.position = I_ANGLE;
|
||||
|
||||
}
|
||||
// 保持收回
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||||
osThreadFlagsClear(HANDING_FINISH);
|
||||
|
||||
haveball=0;//变为空球状态
|
||||
|
||||
currentState1 = BALL_IDLE;
|
||||
|
||||
Send_control();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 任务通知来作全过程
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
osThreadFlagsClear(HANDING_FINISH);
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
xiaomi.position = I_ANGLE; // 保持收回
|
||||
if (feedback->position_deg >= I_ANGLE - 1 && feedback->position_deg <= I_ANGLE + 1)
|
||||
{
|
||||
currentState1 = EXTEND_DOWN;
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_DOWN:
|
||||
osDelay(500); // 不放太快
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
// 检测到球自由下落一次就切换状态
|
||||
if (ball_state == 1)
|
||||
{
|
||||
currentState1 = EXTEND_OUT;
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_OUT:
|
||||
xiaomi.position = O_ANGLE; // 保持伸出
|
||||
if (feedback->position_deg >= O_ANGLE - 1 && feedback->position_deg <= O_ANGLE + 1)
|
||||
{
|
||||
|
||||
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
|
||||
currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_FINISH:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
xiaomi.position = O_ANGLE; // 一直保持伸出
|
||||
// 等待拨杆复位（如切到MIDDLE2），Idle_control会负责回位
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Ball::Idle_control()
|
||||
{
|
||||
if(ball_state==1 && haveball==0)// 读取光电状态(有球 0，无球 1)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 确保爪气缸张开
|
||||
|
||||
}
|
||||
if(ball_state==0)
|
||||
{
|
||||
haveball=1;//变为持球状态
|
||||
osDelay(500);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸闭合
|
||||
}
|
||||
|
||||
|
||||
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
|
||||
if (ready_key == SIDE) // 检测是否准备好
|
||||
{
|
||||
if(hand_thread & HANDING_FINISH)
|
||||
{
|
||||
xiaomi.position=HANGDING_POS;//继续保持外伸
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
xiaomi.position = WAIT_POS;
|
||||
if (feedback->position_deg >= WAIT_POS - 3)
|
||||
{
|
||||
// 只在READY_TELL未置位时发送，防止重复
|
||||
if ((osThreadFlagsGet() & READY_TELL) == 0)
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
xiaomi.position = I_ANGLE; // 默认回到收回位置
|
||||
}
|
||||
|
||||
// 拨杆回到中间挡位时，回位并重置状态机
|
||||
if (currentState1 == EXTEND_FINISH) // 转移后
|
||||
{
|
||||
xiaomi.position = I_ANGLE;
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
if (currentState1 == BALL_FINISH) // 运球完成
|
||||
{
|
||||
xiaomi.position = O_ANGLE;
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
else
|
||||
{
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
int ball_state = 0;
|
||||
int last_ball_state = 0; // 上一次的光电状态
|
||||
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
if (rc_key == DOWN2)
|
||||
{
|
||||
xiaomi.position = HANGDING_POS; // 外伸
|
||||
if (feedback->position_deg >= HANGDING_POS - 0.5f) // 确保伸缩电机到位
|
||||
{
|
||||
runCount = 0; // 每次拨动重新计数
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
osDelay(300); // 不放太快
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
osDelay(5);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
currentState1 = BALL_DROP;
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
osDelay(100); //不要动这里
|
||||
osDelay(100);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||||
// 一直检测有球（ball_state == 0），等球离开
|
||||
if (ball_state == 1 && last_ball_state == 0) // 球离开
|
||||
{
|
||||
currentState1 = BALL_FLAG;
|
||||
@ -422,8 +644,7 @@ void Ball::ballHadling(void)
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
osDelay(10); // 延时 50ms
|
||||
// 等待球弹回再次检测到球
|
||||
osDelay(10);
|
||||
if (ball_state == 0 && last_ball_state == 1) // 球弹回
|
||||
{
|
||||
currentState1 = BALL_CLOSE;
|
||||
@ -432,21 +653,28 @@ void Ball::ballHadling(void)
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
|
||||
osDelay(100);
|
||||
osDelay(25);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
currentState1 = BALL_FINISH;
|
||||
break;
|
||||
|
||||
case BALL_FINISH:
|
||||
osDelay(50); // 延时 50ms
|
||||
osThreadFlagsSet(task_struct.thread.ball, HANDING_FINISH);
|
||||
osDelay(50);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
runCount++;
|
||||
if (runCount < 2)
|
||||
{
|
||||
currentState1 = BALL_FORWARD; // 继续下一次运球
|
||||
}
|
||||
else
|
||||
{
|
||||
currentState1 = BALL_IDLE; // 完成两次，回到空闲
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
currentState1 = BALL_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@ -10,7 +10,7 @@
|
||||
#include "pid.h"
|
||||
#include "filter.h"
|
||||
#include "calc_lib.h"
|
||||
|
||||
#include "nuc.h"
|
||||
|
||||
|
||||
// 定义状态枚举
|
||||
@ -39,8 +39,7 @@ typedef enum
|
||||
DOWN2,
|
||||
IN,
|
||||
OUT,
|
||||
SIDE,
|
||||
DEF
|
||||
SIDE
|
||||
}ball_rc_mode;
|
||||
|
||||
// 定义光电传感器检测宏
|
||||
@ -56,12 +55,18 @@ public:
|
||||
void Move_Extend(void);
|
||||
void Idle_control(void);
|
||||
void rc_mode(void);
|
||||
void test_Handling(void);
|
||||
void ballHadling_two(void);
|
||||
void ball_control(void);
|
||||
void exit_Handling(void);
|
||||
void Hadling(void);
|
||||
void Close(void);
|
||||
void test_up(void);
|
||||
void data_get(const NUC_t &nuc_d);
|
||||
|
||||
BallState_t currentState1; // 运球任务状态机
|
||||
int flag_thread;//暂时还没用到
|
||||
int ball_state ;//光电检测
|
||||
float data;
|
||||
|
||||
|
||||
//小米电机伸缩
|
||||
@ -74,10 +79,11 @@ public:
|
||||
int16_t extern_key;
|
||||
int16_t ready_key; //准备按键
|
||||
//用于传接球,运球后通知
|
||||
volatile BallState_t ballStatus;//是否有球
|
||||
volatile uint32_t hand_thread;//接收传回的线程通知
|
||||
|
||||
private:
|
||||
bool haveball;
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
113
User/module/gimbal.cpp
Normal file
113
User/module/gimbal.cpp
Normal file
@ -0,0 +1,113 @@
|
||||
#include "TopDefine.h"
|
||||
#include "gimbal.hpp"
|
||||
#include "remote_control.h"
|
||||
#include "calc_lib.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include <cmsis_os2.h>
|
||||
|
||||
#define KP 0.12
|
||||
#define KD 0.008
|
||||
//可活动角度
|
||||
#define ANGLE_ALLOW 1.0f
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
NUC_t nuc;
|
||||
|
||||
const fp32 Gimbal:: Gimbal_speed_PID[3] = {50, 0.1, 0};
|
||||
const fp32 Gimbal:: Gimbal_angle_PID[3]= { 5, 0.01, 0};
|
||||
|
||||
#if GM6020ING ==1
|
||||
Gimbal::Gimbal()
|
||||
{
|
||||
// GM6020_Motor = get_motor_point(6);
|
||||
// GM6020_Motor->type = GM6020;
|
||||
// PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000);
|
||||
// PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000);
|
||||
|
||||
// result = 0;
|
||||
// angleSet = 0;
|
||||
|
||||
}
|
||||
|
||||
void Gimbal::gimbalFlow()
|
||||
{
|
||||
int16_t delta[1];
|
||||
//angleSet = angle1;
|
||||
delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
|
||||
result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);
|
||||
|
||||
CAN_cmd_1FF(0,0,result,0,&hcan1);
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
|
||||
void Gimbal::gimbalZero()
|
||||
{
|
||||
angleSet=0;
|
||||
//gimbalFlow();
|
||||
|
||||
}
|
||||
|
||||
void Gimbal::gimbalVision(const NUC_t &nuc)
|
||||
{
|
||||
int16_t delta[1];
|
||||
angleSet = nuc.vision.x;
|
||||
delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
|
||||
result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);
|
||||
|
||||
CAN_cmd_1FF(0,0,result,0,&hcan1);
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
#else
|
||||
Gimbal::Gimbal()
|
||||
{
|
||||
|
||||
Kp = KP;
|
||||
Kd = KD;
|
||||
allowRange = ANGLE_ALLOW;
|
||||
}
|
||||
|
||||
void Gimbal::gimbalInit(void)
|
||||
{
|
||||
int i;
|
||||
GO_M8010_init();
|
||||
for(i = 0;i < GO_NUM;i ++)
|
||||
{
|
||||
goData[i] = getGoPoint(i);//获取电机数据指针
|
||||
|
||||
angleSet[i] = 0;
|
||||
offestAngle[i] = 0;
|
||||
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
|
||||
offestAngle[i] = goData[i]->Pos;
|
||||
HAL_Delay(100);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Gimbal::gimbalFlow(void)
|
||||
{
|
||||
|
||||
//angleSet[0] = map_fp32((float)rc_ctrl.ch[3],-800.0f,800.0f,-allowRange,allowRange) + offestAngle[0];
|
||||
GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
|
||||
osDelay(1);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Gimbal::gimbalZero(void)
|
||||
{
|
||||
GO_M8010_send_data(&huart6, 0,0,0,0,0,0,0);
|
||||
}
|
||||
|
||||
void Gimbal::gimbalVision(const NUC_t &nuc)
|
||||
{
|
||||
angleSet[0] = nuc.vision.x;
|
||||
GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
#endif
|
||||
57
User/module/gimbal.hpp
Normal file
57
User/module/gimbal.hpp
Normal file
@ -0,0 +1,57 @@
|
||||
#ifndef GIMBAL_HPP
|
||||
#define GIMBAL_HPP
|
||||
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "djiMotor.h"
|
||||
#include "pid.h"
|
||||
#include "nuc.h"
|
||||
|
||||
class Gimbal
|
||||
{
|
||||
public:
|
||||
Gimbal();
|
||||
void gimbalFlow(void);//云台随遥控器转动
|
||||
void gimbalZero(void);//云台零阻尼模式
|
||||
void gimbalInit(void);//go初始化
|
||||
void gimbalVision(const NUC_t &nuc); // 接收 NUC_t 数据
|
||||
|
||||
int16_t result;
|
||||
//暂存要发送的扭矩
|
||||
//float result[GO_NUM];
|
||||
// float Kp;
|
||||
// float Kd;
|
||||
private:
|
||||
|
||||
#if GM6020ING == 1
|
||||
//GM6020电机数据
|
||||
motor_measure_t *GM6020_Motor;
|
||||
|
||||
static const float Gimbal_speed_PID[3];
|
||||
static const float Gimbal_angle_PID[3];
|
||||
|
||||
//电机速度pid结构体
|
||||
pid_type_def speed_pid;
|
||||
//位置环pid
|
||||
pid_type_def angle_pid;
|
||||
|
||||
float angleSet;
|
||||
|
||||
#else
|
||||
motor_measure_t *motorData[GO_NUM];
|
||||
//视觉发送的要调的角度
|
||||
float self_angleSet;
|
||||
GO_Motorfield* goData[GO_NUM];
|
||||
//暂存目标位置
|
||||
float angleSet[GO_NUM];
|
||||
float offestAngle[GO_NUM];//go数据
|
||||
float Kp;
|
||||
float Kd;
|
||||
float allowRange;
|
||||
|
||||
#endif
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,6 +1,5 @@
|
||||
#include "TopDefine.h"
|
||||
#include "shoot.hpp"
|
||||
#include "userTask.h"
|
||||
#include "main.h"
|
||||
#include "remote_control.h"
|
||||
#include "FreeRTOS.h"
|
||||
@ -9,124 +8,88 @@
|
||||
#include "vofa.h"
|
||||
#include "buzzer.h"
|
||||
#include "bsp_buzzer.h"
|
||||
#include <math.h>
|
||||
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
NUC_t nuc_v;
|
||||
float vofa[8];
|
||||
|
||||
double test_distance=4.0;
|
||||
//sw[7]👆 1694 中 1000 👇306
|
||||
//sw[2]👆 1694 👇306
|
||||
|
||||
// sw[7]👆 1694 中 1000 👇306
|
||||
// sw[2]👆 1694 👇306
|
||||
//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
|
||||
//F键 sw[0]👆 1800 开 👇200 关
|
||||
//B键 sw[3]👆 200 开 👇1800 关
|
||||
//左旋钮 sw[4] 👈 200 👉1800
|
||||
|
||||
// E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
|
||||
// F键 sw[0]👆 1800 开 👇200 关
|
||||
// B键 sw[3]👆 200 开 👇1800 关
|
||||
// 左旋钮 sw[4] 👈 200 👉1800
|
||||
//尽量别动这组pid
|
||||
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0.01};
|
||||
const fp32 Shoot:: M2006_angle_PID[3] = { 15, 0.1, 0};
|
||||
|
||||
// 尽量别动这组pid
|
||||
const fp32 Shoot::M2006_speed_PID[3] = {5, 0, 0.01};
|
||||
const fp32 Shoot::M2006_angle_PID[3] = {15, 0.1, 0};
|
||||
#define TO_TOP 10.0f
|
||||
#define TO_BOTTOM 5.0f
|
||||
|
||||
#define TO_TOP 13.0f
|
||||
#define TO_BOTTOM 6.0f
|
||||
|
||||
#define INIT_POS -120
|
||||
#define INIT_POS -130
|
||||
#define TOP_POS -211
|
||||
#define BOTTOM_POS 0
|
||||
#define WAIT_POS -170
|
||||
#define CHANEGE_POS -205
|
||||
#define GO_ERROR 1.0f
|
||||
#define Tigger_DO -10
|
||||
#define Tigger_ZERO 125
|
||||
#define Tigger_DO 0
|
||||
#define Tigger_ZERO 100
|
||||
#define Tigger_ERROR 3
|
||||
|
||||
|
||||
float knob_increment;
|
||||
double last_distance = 4.0f; // 4米做测试吧
|
||||
double last_pass = 4.0f; // 4米做测试吧
|
||||
|
||||
Shoot::Shoot()
|
||||
{
|
||||
// 扳机初始化
|
||||
trigger_Motor = get_motor_point(0); // id 201
|
||||
trigger_Motor->type = M2006;
|
||||
PID_init(&speed_pid, PID_POSITION, M2006_speed_PID, 5000, 2000); // pid初始化
|
||||
PID_init(&angle_pid, PID_POSITION, M2006_angle_PID, 5000, 2000); // pid初始化
|
||||
//扳机初始化
|
||||
trigger_Motor= get_motor_point(0);//id 201
|
||||
trigger_Motor->type=M2006;
|
||||
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,5000, 2000);//pid初始化
|
||||
PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,5000, 2000);//pid初始化
|
||||
t_speedSet = 0;
|
||||
result = 0;
|
||||
|
||||
go1.id = 1,
|
||||
|
||||
go1.id =1,
|
||||
go1.mode = 1,
|
||||
go1.K_P = 1.0f,
|
||||
go1.K_W = 0.05,
|
||||
go1.Pos = 0, // 上电先到一个舒服的位置
|
||||
go1.Pos = 0,//上电先到一个舒服的位置
|
||||
go1.W = 0,
|
||||
go1.T = 0,
|
||||
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
fire_pos = INIT_POS; // 发射位置
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback); // 注册串口回调函数，bsp层
|
||||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback);//注册串口回调函数，bsp层
|
||||
|
||||
go1_data = get_GO_measure_point(); // go1数据
|
||||
go1_data = get_GO_measure_point();//go1数据
|
||||
|
||||
currentState = SHOOT_IDLE;
|
||||
currentState= SHOOT_IDLE;
|
||||
|
||||
LowPassFilter2p_Init(&distance_filter,500.0f,80.0f); //给distance 做滤波
|
||||
|
||||
LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波
|
||||
LowPassFilter2p_Init(&pass_filter, 500.0f, 80.0f); // 给distance 做滤波
|
||||
}
|
||||
|
||||
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
|
||||
delta = PID_calc(&angle_pid,trigger_Motor->total_angle,t_posSet); // 计算位置环PID
|
||||
result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,delta); // 计算速度环PID
|
||||
|
||||
CAN_cmd_200(result, 0, 0, 0, &hcan1);
|
||||
|
||||
CAN_cmd_200(result,0,0,0,&hcan1);
|
||||
}
|
||||
|
||||
//好使老拟合
|
||||
// float shoot_fitting(float x)
|
||||
// {
|
||||
// return 0.67076341f * x * x + 20.212423f * x + -154.53966f + 1.0f;
|
||||
// }
|
||||
|
||||
// float shoot_fitting(float x)
|
||||
// {
|
||||
// return 1.4255807f * x * x + 12.138447f * x + -136.35306f;
|
||||
// }
|
||||
|
||||
float shoot_fitting(float x)
|
||||
void Shoot :: distanceGet(const NUC_t &nuc_v)
|
||||
{
|
||||
return 1.2143736f * x * x + 14.733786f * x + -133.3627f;
|
||||
|
||||
distance= LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理
|
||||
//distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
float pass_fitting(float x)
|
||||
{
|
||||
return 0.36807548f * x * x + 25.002169f * x + -199.2727f;
|
||||
}
|
||||
|
||||
void Shoot::distanceGet(const NUC_t &nuc_v)
|
||||
{
|
||||
// 判断数据是否在合理范围
|
||||
if (nuc_v.vision.x >= 0.0f && nuc_v.vision.x <= 7.5f)
|
||||
{
|
||||
last_distance = LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x);
|
||||
}
|
||||
|
||||
if(nuc_v.vision.y >= 0.0f && nuc_v.vision.y <= 7.5f)
|
||||
{
|
||||
last_pass = LowPassFilter2p_Apply(&pass_filter, nuc_v.vision.y);
|
||||
}
|
||||
// 否则不更新，保持上一次的值
|
||||
distance = last_distance;
|
||||
pass_distance =last_pass;
|
||||
}
|
||||
|
||||
int Shoot::GO_SendData(float pos, float limit)
|
||||
int Shoot::GO_SendData(float pos,float limit)
|
||||
{
|
||||
|
||||
//// static int is_calibration=0;
|
||||
@ -145,16 +108,11 @@ int Shoot::GO_SendData(float pos, float limit)
|
||||
//// float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
|
||||
|
||||
/*限制最大输入来限制最大输出*/
|
||||
if (pos - q_current > limit)
|
||||
{
|
||||
if (pos - q_current > limit) {
|
||||
go1.Pos = q_current + limit; // 限制位置
|
||||
}
|
||||
else if (pos - q_current < -limit)
|
||||
{
|
||||
}else if (pos - q_current < -limit) {
|
||||
go1.Pos = q_current - limit; // 限制位置
|
||||
}
|
||||
else
|
||||
{
|
||||
}else {
|
||||
go1.Pos = pos; // 允许位置
|
||||
}
|
||||
|
||||
@ -166,178 +124,141 @@ int Shoot::GO_SendData(float pos, float limit)
|
||||
|
||||
// F键 sw[0]👆 1800 👇200
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800
|
||||
// G键 sw[6]👆 200 中 1000 👇1800
|
||||
// G键 sw[6]👆 1800 中 1000 👇200
|
||||
// B键 sw[3]👆 200 开 中 1000 👇1800 关
|
||||
// sw[5] 👆 200 👇1800
|
||||
// 左旋 sw[7] 200 --1800
|
||||
|
||||
|
||||
float and1=0.0f;
|
||||
|
||||
//左旋 sw[7] 200 --1800
|
||||
void Shoot::rc_mode()
|
||||
{
|
||||
// 底部光电检测（假设0为到位，1为未到位，根据实际硬件调整）
|
||||
int bottom_sensor = HAL_GPIO_ReadPin(BALL_GPIO_Port, BALL_Pin); // 0为到位
|
||||
static bool is_ready = false;
|
||||
|
||||
// 只要检测到到位就解除保护
|
||||
if (!is_ready)
|
||||
if(rc_ctrl.sw[1]==200)
|
||||
{
|
||||
if (bottom_sensor == 0)
|
||||
{
|
||||
is_ready = true;
|
||||
BSP_Buzzer_Stop();
|
||||
rc_key=UP1;
|
||||
}
|
||||
else
|
||||
if(rc_ctrl.sw[1]==1800 && rc_ctrl.sw[2]==200)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1, 5000);
|
||||
return;
|
||||
rc_key=MIDDLE1;
|
||||
}
|
||||
if(rc_ctrl.sw[2]==1800 && rc_ctrl.sw[1]==1800)
|
||||
{
|
||||
rc_key=DOWN1;
|
||||
}
|
||||
if(rc_ctrl.sw[0]==1800)
|
||||
{
|
||||
mode_key=TEST;
|
||||
}
|
||||
if(rc_ctrl.sw[0]==200)
|
||||
{
|
||||
mode_key=VSION;
|
||||
}
|
||||
// if(rc_ctrl.sw[5]==200)
|
||||
// {
|
||||
// mode_key=OUT;
|
||||
// }
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
{
|
||||
ready_key=OFFENSIVE;
|
||||
}
|
||||
else if(rc_ctrl.sw[5]==200)
|
||||
{
|
||||
ready_key=DEFENSE;
|
||||
}
|
||||
|
||||
// 以下为原有遥控按键逻辑
|
||||
if (rc_ctrl.sw[1] == 200)
|
||||
{
|
||||
rc_key = UP1;
|
||||
}
|
||||
if (rc_ctrl.sw[1] == 1800 && rc_ctrl.sw[2] == 200)
|
||||
{
|
||||
rc_key = MIDDLE1;
|
||||
}
|
||||
if (rc_ctrl.sw[2] == 1800 && rc_ctrl.sw[1] == 1800)
|
||||
{
|
||||
rc_key = DOWN1;
|
||||
}
|
||||
if (rc_ctrl.sw[0] == 1800)
|
||||
{
|
||||
mode_key = PASS;
|
||||
}
|
||||
if (rc_ctrl.sw[0] == 200)
|
||||
{
|
||||
mode_key = VSION;
|
||||
}
|
||||
if (rc_ctrl.sw[5] == 1800)
|
||||
{
|
||||
ready_key = OFFENSIVE;
|
||||
}
|
||||
else if (rc_ctrl.sw[5] == 200)
|
||||
{
|
||||
ready_key = DEFENSE;
|
||||
}
|
||||
//400--640为1 730--860为2 900到1200为3档中间 1300--1500为4
|
||||
if(rc_ctrl.sw[7]<=300)
|
||||
{
|
||||
and1=0.0f;
|
||||
}
|
||||
if(rc_ctrl.sw[7]>=400&&rc_ctrl.sw[7]<=640)
|
||||
{
|
||||
and1=-2.0f;
|
||||
}
|
||||
if(rc_ctrl.sw[7]>=730&&rc_ctrl.sw[7]<=860)
|
||||
{
|
||||
and1=-1.0f;
|
||||
}
|
||||
if(rc_ctrl.sw[7]>=900&&rc_ctrl.sw[7]<=1200)
|
||||
{
|
||||
and1=0.0f;
|
||||
}
|
||||
if(rc_ctrl.sw[7]>=1300&&rc_ctrl.sw[7]<=1500)
|
||||
{
|
||||
and1=1.0f;
|
||||
}
|
||||
// //旋钮增量
|
||||
// static int last_knob_value = 0; // 记录旋钮的上一次值
|
||||
// int current_knob_value = rc_ctrl.sw[7]; // 获取当前旋钮值
|
||||
|
||||
// // 计算旋钮增量
|
||||
// if (current_knob_value >= 200 && current_knob_value <= 1800) {
|
||||
// knob_increment = (current_knob_value - last_knob_value) / 15.0f; // 每80单位对应一个增量
|
||||
// } else {
|
||||
// knob_increment = 0; // 如果旋钮值超出范围，不产生增量
|
||||
// }
|
||||
|
||||
// 旋钮物理范围
|
||||
const int knob_min = 200;
|
||||
const int knob_max = 1800;
|
||||
|
||||
// 目标映射范围
|
||||
const float map_min = 130.0f;
|
||||
const float map_max = -60.0f;
|
||||
|
||||
int current_knob_value = rc_ctrl.sw[7];
|
||||
|
||||
// 限制范围
|
||||
if (current_knob_value < knob_min) current_knob_value = knob_min;
|
||||
if (current_knob_value > knob_max) current_knob_value = knob_max;
|
||||
|
||||
// 线性映射
|
||||
knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min);
|
||||
|
||||
|
||||
// // 旋钮映射部分不变
|
||||
// const int knob_min = 200;
|
||||
// const int knob_max = 1800;
|
||||
// const float map_min = 130.0f;
|
||||
// const float map_max = -60.0f;
|
||||
// int current_knob_value = rc_ctrl.sw[7];
|
||||
// if (current_knob_value < knob_min)
|
||||
// current_knob_value = knob_min;
|
||||
// if (current_knob_value > knob_max)
|
||||
// current_knob_value = knob_max;
|
||||
// knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min);
|
||||
}
|
||||
|
||||
#if ONE_CONTROL == 0
|
||||
|
||||
|
||||
void Shoot::shoot_control()
|
||||
{
|
||||
#if ONE_CONTROL==0
|
||||
|
||||
// 方便调试
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
//fire_pos = distance; // 视觉拟合的力
|
||||
//fire_pos =shoot_fitting(distance)+and1;
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
//t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
} else {
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
break;
|
||||
|
||||
case MIDDLE1:
|
||||
shoot_Current();
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TEST:
|
||||
//实时可调蓄力位置
|
||||
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
|
||||
switch (rc_key)
|
||||
{
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
// t_posSet = Tigger_ZERO; // 扳机松开
|
||||
//t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY)
|
||||
{
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= Tigger_ZERO - 20)
|
||||
{ // 假设120度为发射完成角度
|
||||
if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
break;
|
||||
|
||||
case MIDDLE1:
|
||||
shoot_Current();
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case PASS:
|
||||
// 实时可调蓄力位置
|
||||
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
fire_pos =pass_fitting(pass_distance);
|
||||
switch (rc_key)
|
||||
{
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
// t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY)
|
||||
{
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= Tigger_ZERO - 20)
|
||||
{ // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
break;
|
||||
@ -354,42 +275,42 @@ void Shoot::shoot_control()
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos, -210.0f, 8.0f);
|
||||
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, limit_speed);
|
||||
GO_SendData(go1.Pos,limit_speed);
|
||||
|
||||
// 控制扳机电机
|
||||
trigger_control();
|
||||
}
|
||||
|
||||
void Shoot ::shoot_Current()
|
||||
void Shoot :: shoot_Current()
|
||||
{
|
||||
|
||||
switch (currentState)
|
||||
{
|
||||
switch (currentState) {
|
||||
case SHOOT_IDLE:
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
control_pos = TOP_POS; // 顶部位置
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
go1.Pos = control_pos;
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
if (feedback.fd_gopos < -209)
|
||||
{
|
||||
if (feedback.fd_gopos <-209) {
|
||||
currentState = SHOOT_TOP; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_TOP:
|
||||
t_posSet = Tigger_DO; // 扳机扣动钩住
|
||||
if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f)
|
||||
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f)
|
||||
{
|
||||
// 判定为钩住
|
||||
//判定为钩住
|
||||
is_hooked = true; // 标记钩子已钩住
|
||||
currentState = SHOOT_READY; // 切换到准备发射状态
|
||||
}
|
||||
@ -399,39 +320,39 @@ void Shoot ::shoot_Current()
|
||||
if (is_hooked)
|
||||
{
|
||||
go1.Pos = fire_pos; // 下拉到中间挡位位置
|
||||
limit_speed = TO_BOTTOM; // 慢速下来
|
||||
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
limit_speed=TO_BOTTOM;//慢速下来
|
||||
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) {
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1, 5000);
|
||||
// currentState = SHOOT_WAIT; // 等待发射信号
|
||||
// 在拨杆切换时触发了
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
//currentState = SHOOT_WAIT; // 等待发射信号
|
||||
//在拨杆切换时触发了
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
default:
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Shoot::RemoveError()
|
||||
{
|
||||
void Shoot::RemoveError() {
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_POS + 1.0f;
|
||||
if (feedback.fd_gopos >= control_pos - 0.5f && feedback.fd_gopos <= control_pos + 0.5f)
|
||||
control_pos = TOP_POS+1.0f;
|
||||
if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
is_hooked = false;
|
||||
}
|
||||
if (trigger_Motor->total_angle >= Tigger_ZERO - 10)
|
||||
if(trigger_Motor->total_angle>=Tigger_ZERO-10)
|
||||
{
|
||||
// 认为完全松开
|
||||
control_pos = INIT_POS;
|
||||
//认为完全松开
|
||||
control_pos=INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
|
||||
}
|
||||
limit_speed = 3.0f; // 慢慢送上去
|
||||
limit_speed=3.0f;//慢慢送上去
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
@ -439,61 +360,45 @@ void Shoot::RemoveError()
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
float and2=0;
|
||||
|
||||
void Shoot::shoot_control()
|
||||
{
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
// 方便调试 反馈信息
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
//方便调试 反馈信息
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
|
||||
shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
|
||||
if (ready_key == OFFENSIVE)
|
||||
{
|
||||
switch (mode_key)
|
||||
shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
|
||||
if(ready_key==OFFENSIVE){
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = shoot_fitting(distance)+and1;
|
||||
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
|
||||
switch (rc_key)
|
||||
{
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key) {
|
||||
case MIDDLE1:
|
||||
if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK))
|
||||
{
|
||||
// 只收到READY_TELL且未收到EXTEND_OK时，顶部蓄力流程
|
||||
ball_receive();
|
||||
}
|
||||
else if (shoot_thread & EXTEND_OK)
|
||||
{
|
||||
go1.Pos = fire_pos;
|
||||
limit_speed = TO_BOTTOM;
|
||||
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
shoot_wait = 1;
|
||||
//BSP_Buzzer_Start();
|
||||
//BSP_Buzzer_Set(1, 500);
|
||||
}
|
||||
}
|
||||
ball_receive(); // 执行接收球的操作
|
||||
|
||||
if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知
|
||||
{
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if (shoot_thread & EXTEND_OK && shoot_wait == 1)
|
||||
if(shoot_thread & EXTEND_OK)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
if (feedback.fd_tpos >= Tigger_ZERO - 20)
|
||||
{
|
||||
|
||||
//BSP_Buzzer_Stop();
|
||||
currentState = SHOOT_IDLE;
|
||||
BSP_Buzzer_Stop();
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动 射出
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||||
shoot_wait = 0;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
@ -503,99 +408,70 @@ void Shoot::shoot_control()
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
break;
|
||||
|
||||
// 传球档
|
||||
case PASS:
|
||||
switch (rc_key)
|
||||
{
|
||||
//无自瞄拟合测试档
|
||||
case TEST:
|
||||
switch (rc_key) {
|
||||
case MIDDLE1:
|
||||
fire_pos = shoot_fitting(distance)+and1;
|
||||
if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK))
|
||||
|
||||
if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知
|
||||
{
|
||||
ball_receive();
|
||||
}
|
||||
else if (shoot_thread & EXTEND_OK)
|
||||
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
// 只收到EXTEND_OK时，允许调节蓄力位置
|
||||
// fire_pos = INIT_POS + knob_increment;
|
||||
go1.Pos = fire_pos;
|
||||
limit_speed = TO_BOTTOM;
|
||||
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
}
|
||||
}
|
||||
if(shoot_thread & READY_TELL) // 如果收到等待配合的通知
|
||||
{
|
||||
shoot_wait = 1;
|
||||
// BSP_Buzzer_Start();
|
||||
// BSP_Buzzer_Set(1, 500);
|
||||
}
|
||||
}
|
||||
// 没收到READY_TELL不做任何蓄力
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if (shoot_thread & EXTEND_OK)
|
||||
{
|
||||
if (shoot_wait == 1)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
if (feedback.fd_tpos >= Tigger_ZERO - 20)
|
||||
{
|
||||
|
||||
//BSP_Buzzer_Stop();
|
||||
currentState = SHOOT_IDLE;
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||||
shoot_wait = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
else if (ready_key == DEFENSE)
|
||||
{
|
||||
|
||||
control_pos = WAIT_POS; //-209
|
||||
go1.Pos = control_pos;
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
if(feedback.fd_gopos < WAIT_POS +2.0f)
|
||||
{
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
//osThreadFlagsSet(task_struct.thread.ball, DEF_READY);
|
||||
|
||||
// 只在READY_TELL未置位时发送，防止重复
|
||||
if ((osThreadFlagsGet() & DEF_READY) == 0)
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.ball, DEF_READY);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||||
osThreadFlagsClear(HANDING_FINISH);
|
||||
currentState = SHOOT_IDLE;
|
||||
|
||||
ball_receive(); // 执行接收球的操作
|
||||
}
|
||||
else
|
||||
{
|
||||
t_posSet = Tigger_DO; // 防止debug时重复
|
||||
go1.Pos = TOP_POS-3.0f; //-208
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
}
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if(shoot_thread & EXTEND_OK)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动 射出
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f);
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
else if(ready_key==DEFENSE)//攻守档总控
|
||||
{
|
||||
go1.Pos = TOP_POS-3.0f; //-208
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
}
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, limit_speed);
|
||||
|
||||
@ -603,35 +479,25 @@ void Shoot::shoot_control()
|
||||
trigger_control();
|
||||
}
|
||||
|
||||
|
||||
// 配合运球到发射
|
||||
//配合运球到发射
|
||||
void Shoot ::ball_receive()
|
||||
{
|
||||
switch (currentState)
|
||||
{
|
||||
case SHOOT_IDLE:
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
if (shoot_thread & READY_TELL) // 如果收到等待通知
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
if(trigger_Motor->total_angle > Tigger_ZERO-5 )
|
||||
{
|
||||
control_pos = TOP_POS;
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
|
||||
|
||||
}
|
||||
if (feedback.fd_gopos < -209)
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
t_posSet = Tigger_ZERO;
|
||||
if(feedback.fd_gopos <-209)
|
||||
{
|
||||
|
||||
currentState = GO_TOP; // 切换到准备发射状态
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case GO_TOP:
|
||||
t_posSet = Tigger_DO;
|
||||
if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f)
|
||||
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f)
|
||||
{
|
||||
|
||||
currentState = BAKC; // 切换到准备发射状态
|
||||
@ -639,8 +505,8 @@ void Shoot ::ball_receive()
|
||||
break;
|
||||
case BAKC:
|
||||
control_pos = BOTTOM_POS;
|
||||
limit_speed = 8.0f; // 慢速下来
|
||||
if (feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
|
||||
limit_speed=TO_BOTTOM;//慢速下来
|
||||
if(feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
|
||||
{
|
||||
|
||||
osThreadFlagsClear(READY_TELL); // 清除任务通知标志位
|
||||
@ -649,27 +515,28 @@ void Shoot ::ball_receive()
|
||||
break;
|
||||
}
|
||||
|
||||
// 调整go电机位置
|
||||
//调整go电机位置
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
void Shoot::RemoveError()
|
||||
{
|
||||
void Shoot::RemoveError() {
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_POS;
|
||||
if (feedback.fd_gopos < -209)
|
||||
if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
is_hooked = false;
|
||||
}
|
||||
if (trigger_Motor->total_angle >= Tigger_ZERO - 5)
|
||||
if(trigger_Motor->total_angle>=Tigger_ZERO-5)
|
||||
{
|
||||
// 认为完全松开
|
||||
control_pos = INIT_POS;
|
||||
//认为完全松开
|
||||
control_pos=INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
|
||||
}
|
||||
limit_speed = 5.0f; // 慢慢送上去
|
||||
limit_speed=3.0f;//慢慢送上去
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -18,8 +18,7 @@ typedef enum {
|
||||
SHOOT_RETURN, // 自动返回状态
|
||||
//运射配合
|
||||
GO_TOP,
|
||||
BAKC,
|
||||
WAIT_CHANGE
|
||||
BAKC
|
||||
} ShootState_t;
|
||||
|
||||
// // 定义状态枚举
|
||||
@ -41,10 +40,8 @@ typedef enum
|
||||
WAIT,
|
||||
TEST,
|
||||
VSION,
|
||||
PASS,
|
||||
OFFENSIVE,
|
||||
DEFENSE
|
||||
|
||||
}rc_mode;
|
||||
|
||||
// 光电传感器读取宏
|
||||
@ -87,7 +84,6 @@ public:
|
||||
|
||||
//滤波器
|
||||
LowPassFilter2p_t distance_filter; // 用于滤波视觉距离
|
||||
LowPassFilter2p_t pass_filter;
|
||||
|
||||
//==========================公共变量==========================
|
||||
int16_t rc_key; //遥控器按键
|
||||
@ -98,7 +94,6 @@ public:
|
||||
|
||||
volatile uint32_t shoot_thread;//接收传回的线程通知
|
||||
fp32 distance; //视觉距离
|
||||
fp32 pass_distance; //视觉距离
|
||||
|
||||
private:
|
||||
//扳机2006
|
||||
@ -114,8 +109,6 @@ private:
|
||||
|
||||
float limit_speed;//go电机限速
|
||||
|
||||
int shoot_wait;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -6,10 +6,14 @@
|
||||
#include "ball.hpp"
|
||||
#include "remote_control.h"
|
||||
#include "vofa.h"
|
||||
#include "nuc.h"
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
Ball ball;
|
||||
NUC_t Data;
|
||||
|
||||
|
||||
//检查光电
|
||||
int abc=0;
|
||||
//3 👆1800 中1000 👇200
|
||||
extern int speedm;
|
||||
|
||||
void FunctionBall(void *argument)
|
||||
@ -28,10 +32,23 @@ 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);
|
||||
|
||||
ball.rc_mode(); // 遥控器模式
|
||||
|
||||
|
||||
if (osMessageQueueGet(task_struct.msgq.nuc, &Data, NULL, 0) == osOK)
|
||||
{
|
||||
ball.data_get(Data);
|
||||
}
|
||||
|
||||
ball.Move_Extend();
|
||||
ball.ball_control(); // 控制球的动作
|
||||
|
||||
ball.Send_control();
|
||||
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
|
||||
|
||||
@ -26,7 +26,7 @@ void FunctionCan(void *argument)
|
||||
task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());
|
||||
|
||||
#endif
|
||||
//waitNewDji();
|
||||
waitNewDji();
|
||||
djiMotorEncode();
|
||||
can2MotorEncode();
|
||||
|
||||
|
||||
47
User/task/gimbalTask.cpp
Normal file
47
User/task/gimbalTask.cpp
Normal file
@ -0,0 +1,47 @@
|
||||
#include "TopDefine.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "userTask.h"
|
||||
#include <cmsis_os2.h>
|
||||
#include "gimbalTask.hpp"
|
||||
#include "gimbal.hpp"
|
||||
#include "main.h"
|
||||
#include "remote_control.h"
|
||||
#include "nuc.h"
|
||||
Gimbal gimbal;
|
||||
// NUC_t nucData; // 用于存储从队列接收的数据
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
int cnt1=0;
|
||||
|
||||
void FunctionGimbal(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
|
||||
|
||||
HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
|
||||
//cnt1++;
|
||||
|
||||
// gimbal.gimbalFlow();
|
||||
// 从消息队列接收视觉数据
|
||||
// if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
// {
|
||||
// // 使用接收到的视觉数据调整云台
|
||||
// //gimbal.gimbalVision(nucData);
|
||||
// }
|
||||
|
||||
osDelay(1);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
}
|
||||
}
|
||||
|
||||
5
User/task/gimbalTask.hpp
Normal file
5
User/task/gimbalTask.hpp
Normal file
@ -0,0 +1,5 @@
|
||||
#ifndef GIMBALTASK_HPP
|
||||
#define GIMBALTASK_HPP
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,15 +1,17 @@
|
||||
#include "nucTask.hpp"
|
||||
#include "nuc.h"
|
||||
#include "userTask.h"
|
||||
#include "TopDefine.h" //事件组的一些东西
|
||||
#include "TopDefine.h"//事件组的一些东西
|
||||
#include "FreeRTOS.h"
|
||||
#include <cmsis_os2.h>
|
||||
#include <stdio.h>
|
||||
#include "bsp_buzzer.h"
|
||||
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
NUC_t cmd_fromnuc;
|
||||
//int send[3]={1,2,3};
|
||||
float send[3]={1,2,3};
|
||||
|
||||
#endif
|
||||
|
||||
@ -21,18 +23,20 @@ void Function_nuc(void *argument)
|
||||
|
||||
NUC_Init(&cmd_fromnuc);
|
||||
|
||||
HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while (1)
|
||||
while(1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
task_struct.stack_water_mark.nuc = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
NUC_StartReceiving();
|
||||
|
||||
// NUC_RawParse(&cmd_fromnuc);
|
||||
// NUC_SendPacket(&send, sizeof(send)); // 发送数据包
|
||||
// 掉线处理有空写
|
||||
NUC_RawParse(&cmd_fromnuc);
|
||||
//NUC_SendPacket(&send, sizeof(send)); // 发送数据包
|
||||
//掉线处理有空写
|
||||
// if(NUC_WaitDmaCplt())
|
||||
// {
|
||||
// NUC_RawParse(&cmd_fromnuc);
|
||||
@ -43,25 +47,12 @@ void Function_nuc(void *argument)
|
||||
// NUC_HandleOffline(&cmd_fromnuc);
|
||||
// }
|
||||
|
||||
if (NUC_RawParse(&cmd_fromnuc) == DEVICE_OK)
|
||||
{
|
||||
//BSP_Buzzer_Stop();
|
||||
HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);
|
||||
osMessageQueueReset(task_struct.msgq.nuc); // 清空消息队列
|
||||
osMessageQueuePut(task_struct.msgq.nuc, &(cmd_fromnuc), 0, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
// BSP_Buzzer_Start();
|
||||
// BSP_Buzzer_Set(1, 5000);
|
||||
HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET);
|
||||
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_SET);
|
||||
NUC_HandleOffline(&cmd_fromnuc);
|
||||
}
|
||||
osMessageQueuePut(task_struct.msgq.nuc,&(cmd_fromnuc),0,0);
|
||||
osDelay(2);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
|
||||
osDelayUntil(tick);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -10,14 +10,14 @@ extern RC_ctrl_t rc_ctrl;
|
||||
Shoot shoot;
|
||||
NUC_t nucData; // 自瞄
|
||||
|
||||
|
||||
int aaaxxx=0;
|
||||
|
||||
void FunctionShoot(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_SHOOT;
|
||||
osDelay(3000);//等待M2006电机启动
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_SHOOT;
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
@ -28,12 +28,12 @@ while(1)
|
||||
|
||||
shoot.rc_mode(); //遥控器模式
|
||||
|
||||
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
{
|
||||
shoot.distanceGet(nucData);
|
||||
}
|
||||
// if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
// {
|
||||
// shoot.distanceGet(nucData);
|
||||
// }
|
||||
|
||||
shoot.shoot_control();
|
||||
// shoot.shoot_control();
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
|
||||
@ -13,8 +13,11 @@ extern "C" {
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
|
||||
/* 所有任务都要define一个“任务运行频率”和“初始化延时” */
|
||||
#define TASK_FREQ_SHOOT (500u)
|
||||
#define TASK_FREQ_CAN (500u)
|
||||
#define TASK_FREQ_CTRL_GIMBAL (250u)
|
||||
#define TASK_FREQ_CTRL_SHOOT (500u)
|
||||
#define TASK_FREQ_CTRL_COMMAND (500u)
|
||||
#define TASK_FREQ_MONITOR (2u)
|
||||
#define TASK_FREQ_CAN (1500u)
|
||||
#define TASK_FREQ_AI (500u)
|
||||
#define TASK_FREQ_BALL (500u)
|
||||
|
||||
@ -39,6 +42,8 @@ typedef struct
|
||||
//osThreadId_t rc;
|
||||
osThreadId_t nuc;
|
||||
osThreadId_t shoot;
|
||||
osThreadId_t gimbal;
|
||||
//osThreadId_t handling;
|
||||
osThreadId_t ball;
|
||||
|
||||
}thread;
|
||||
@ -59,23 +64,32 @@ typedef struct
|
||||
#ifdef DEBUG
|
||||
struct {
|
||||
UBaseType_t can;
|
||||
//UBaseType_t rc;
|
||||
UBaseType_t nuc;
|
||||
UBaseType_t shoot;
|
||||
UBaseType_t gimbal;
|
||||
//UBaseType_t handling;
|
||||
UBaseType_t ball;
|
||||
|
||||
} stack_water_mark; /* stack使用 */
|
||||
|
||||
struct {
|
||||
float can;
|
||||
//float rc;
|
||||
float nuc;
|
||||
float shoot;
|
||||
float gimbal;
|
||||
//float handling;
|
||||
float ball;
|
||||
} freq; /* 任务运行频率 */
|
||||
|
||||
struct {
|
||||
float can;
|
||||
//float rc;
|
||||
float nuc;
|
||||
float shoot;
|
||||
float gimbal;
|
||||
//float handling;
|
||||
float ball;
|
||||
} last_up_time; /* 任务最近运行时 */
|
||||
#endif
|
||||
@ -92,6 +106,7 @@ extern const osEventFlagsAttr_t attr_event;
|
||||
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
void FunctionTake(void *argument);
|
||||
void FunctionShoot(void *argument);
|
||||
void FunctionCan(void *argument);
|
||||
void FunctionRc(void *argument);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user