Compare commits
6 Commits
6489615e36
...
f5a40b99b1
| Author | SHA1 | Date | |
|---|---|---|---|
| f5a40b99b1 | |||
| 3654e052e7 | |||
| ffd13d2e63 | |||
| c6ded22f93 | |||
| 74d7f2ef20 | |||
| 30de35d147 |
@ -57,7 +57,6 @@ void EXTI0_IRQHandler(void);
|
||||
void EXTI4_IRQHandler(void);
|
||||
void DMA1_Stream1_IRQHandler(void);
|
||||
void CAN1_RX0_IRQHandler(void);
|
||||
void CAN1_RX1_IRQHandler(void);
|
||||
void EXTI9_5_IRQHandler(void);
|
||||
void TIM1_UP_TIM10_IRQHandler(void);
|
||||
void USART1_IRQHandler(void);
|
||||
@ -65,7 +64,6 @@ void USART3_IRQHandler(void);
|
||||
void DMA2_Stream1_IRQHandler(void);
|
||||
void DMA2_Stream2_IRQHandler(void);
|
||||
void DMA2_Stream3_IRQHandler(void);
|
||||
void CAN2_RX0_IRQHandler(void);
|
||||
void CAN2_RX1_IRQHandler(void);
|
||||
void DMA2_Stream5_IRQHandler(void);
|
||||
void DMA2_Stream6_IRQHandler(void);
|
||||
|
||||
@ -81,7 +81,7 @@ void MX_CAN2_Init(void)
|
||||
hcan2.Init.AutoWakeUp = DISABLE;
|
||||
hcan2.Init.AutoRetransmission = DISABLE;
|
||||
hcan2.Init.ReceiveFifoLocked = DISABLE;
|
||||
hcan2.Init.TransmitFifoPriority = ENABLE;
|
||||
hcan2.Init.TransmitFifoPriority = DISABLE;
|
||||
if (HAL_CAN_Init(&hcan2) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
@ -124,8 +124,6 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
|
||||
/* CAN1 interrupt Init */
|
||||
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
|
||||
HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN1_MspInit 1 */
|
||||
|
||||
/* USER CODE END CAN1_MspInit 1 */
|
||||
@ -155,8 +153,6 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
|
||||
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
|
||||
|
||||
/* CAN2 interrupt Init */
|
||||
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
|
||||
HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN2_MspInit 1 */
|
||||
@ -187,7 +183,6 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
|
||||
|
||||
/* CAN1 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
|
||||
HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN1_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END CAN1_MspDeInit 1 */
|
||||
@ -211,7 +206,6 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
|
||||
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6);
|
||||
|
||||
/* CAN2 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
|
||||
HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN2_MspDeInit 1 */
|
||||
|
||||
|
||||
@ -250,20 +250,6 @@ void CAN1_RX0_IRQHandler(void)
|
||||
/* USER CODE END CAN1_RX0_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN1 RX1 interrupt.
|
||||
*/
|
||||
void CAN1_RX1_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN CAN1_RX1_IRQn 0 */
|
||||
|
||||
/* USER CODE END CAN1_RX1_IRQn 0 */
|
||||
HAL_CAN_IRQHandler(&hcan1);
|
||||
/* USER CODE BEGIN CAN1_RX1_IRQn 1 */
|
||||
|
||||
/* USER CODE END CAN1_RX1_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles EXTI line[9:5] interrupts.
|
||||
*/
|
||||
@ -363,20 +349,6 @@ void DMA2_Stream3_IRQHandler(void)
|
||||
/* USER CODE END DMA2_Stream3_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN2 RX0 interrupts.
|
||||
*/
|
||||
void CAN2_RX0_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN CAN2_RX0_IRQn 0 */
|
||||
|
||||
/* USER CODE END CAN2_RX0_IRQn 0 */
|
||||
HAL_CAN_IRQHandler(&hcan2);
|
||||
/* USER CODE BEGIN CAN2_RX0_IRQn 1 */
|
||||
|
||||
/* USER CODE END CAN2_RX0_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN2 RX1 interrupt.
|
||||
*/
|
||||
|
||||
34
MDK-ARM/.vscode/keil-assistant.log
vendored
34
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -32,3 +32,37 @@
|
||||
|
||||
[info] Log at : 2025/6/15|11:17:22|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/16|19:01:05|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/17|19:06:14|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/19|13:29:28|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/19|17:34:46|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/19|20:07:38|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/20|09:52:08|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/20|19:02:32|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/21|01:54:17|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/21|09:59:57|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/21|17:02:34|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/22|13:36:33|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/23|15:15:24|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/23|16:29:17|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/24|12:24:51|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/24|19:22:03|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/24|21:35:17|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/25|09:51:44|GMT+0800
|
||||
|
||||
|
||||
6
MDK-ARM/.vscode/uv4.log
vendored
6
MDK-ARM/.vscode/uv4.log
vendored
@ -1,8 +1,8 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1'
|
||||
compiling calc_lib.c...
|
||||
compiling shoot.cpp...
|
||||
linking...
|
||||
Program Size: Code=30644 RO-data=1884 RW-data=5816 ZI-data=33280
|
||||
Program Size: Code=31344 RO-data=1832 RW-data=268 ZI-data=32252
|
||||
FromELF: creating hex file...
|
||||
"R1\R1.axf" - 0 Error(s), 0 Warning(s).
|
||||
Build Time Elapsed: 00:00:03
|
||||
Build Time Elapsed: 00:00:05
|
||||
|
||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/6/14 22:01:44
|
||||
2025/6/25 9:57:28
|
||||
@ -150,32 +150,15 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>ST-LINKIII-KEIL_SWO</Key>
|
||||
<Name>-U-O142 -O2254 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
<Name>-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)</Name>
|
||||
</SetRegEntry>
|
||||
</TargetDriverDllRegistry>
|
||||
<Breakpoint>
|
||||
<Bp>
|
||||
<Number>0</Number>
|
||||
<Type>0</Type>
|
||||
<LineNumber>82</LineNumber>
|
||||
<EnabledFlag>1</EnabledFlag>
|
||||
<Address>0</Address>
|
||||
<ByteObject>0</ByteObject>
|
||||
<HtxType>0</HtxType>
|
||||
<ManyObjects>0</ManyObjects>
|
||||
<SizeOfObject>0</SizeOfObject>
|
||||
<BreakByAccess>0</BreakByAccess>
|
||||
<BreakIfRCount>0</BreakIfRCount>
|
||||
<Filename>../Core/Src/main.c</Filename>
|
||||
<ExecCommand></ExecCommand>
|
||||
<Expression></Expression>
|
||||
</Bp>
|
||||
</Breakpoint>
|
||||
<Breakpoint/>
|
||||
<WatchWindow1>
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc_ctrl,0x0A</ItemText>
|
||||
<ItemText>ball,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>1</count>
|
||||
@ -185,32 +168,27 @@
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_fromnuc</ItemText>
|
||||
<ItemText>triggerCount,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nucbuf</ItemText>
|
||||
<ItemText>abc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>goangle,0x0A</ItemText>
|
||||
<ItemText>rc_ctrl,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ball,0x0A</ItemText>
|
||||
<ItemText>nucbuf</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>knob_increment</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>task_struct,0x0A</ItemText>
|
||||
<ItemText>nuc_v</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
@ -1063,7 +1041,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/module</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1086,18 +1064,6 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\module\gimbal.cpp</PathWithFileName>
|
||||
<FilenameWithoutPath>gimbal.cpp</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>64</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\module\shoot.cpp</PathWithFileName>
|
||||
<FilenameWithoutPath>shoot.cpp</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1107,13 +1073,13 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/task</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileNumber>64</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1125,7 +1091,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1137,7 +1103,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1149,7 +1115,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1161,19 +1127,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\gimbalTask.cpp</PathWithFileName>
|
||||
<FilenameWithoutPath>gimbalTask.cpp</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1185,7 +1139,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
|
||||
@ -733,11 +733,6 @@
|
||||
<FileType>8</FileType>
|
||||
<FilePath>..\User\module\ball.cpp</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>gimbal.cpp</FileName>
|
||||
<FileType>8</FileType>
|
||||
<FilePath>..\User\module\gimbal.cpp</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>shoot.cpp</FileName>
|
||||
<FileType>8</FileType>
|
||||
@ -768,11 +763,6 @@
|
||||
<FileType>8</FileType>
|
||||
<FilePath>..\User\task\encodeCan.cpp</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>gimbalTask.cpp</FileName>
|
||||
<FileType>8</FileType>
|
||||
<FilePath>..\User\task\gimbalTask.cpp</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>nucTask.cpp</FileName>
|
||||
<FileType>8</FileType>
|
||||
|
||||
37
MDK-ARM/README.md
Normal file
37
MDK-ARM/README.md
Normal file
@ -0,0 +1,37 @@
|
||||
# R1_up
|
||||
|
||||
r1上层
|
||||
|
||||
## 外设
|
||||
|
||||
+ CAN1
|
||||
+ 扳机2006 id:0x201
|
||||
+ CAN2
|
||||
+ 小米电机 id:1
|
||||
+ UART
|
||||
+ uart1 波特率4000000 id：2
|
||||
+ uart6 nuc
|
||||
+ uart3 遥控器接收
|
||||
+ GPIO
|
||||
+ PI6运球光电
|
||||
+ PE13 爪气缸
|
||||
+ PE14 砸气缸
|
||||
|
||||
## 遥控器
|
||||
|
||||
## 待解决
|
||||
|
||||
+ 用了将运球和伸缩绑定到一起 √
|
||||
+ 串口收数加个滤波 √
|
||||
|
||||
## 思路
|
||||
|
||||
+ 👆 传球档 👆 配合档
|
||||
+ 中 初始档 中 初始档
|
||||
+ 👇 发射档 👇 运球档
|
||||
+ 起步遥控档我直接蓄力准备接球
|
||||
+ 可多次的运球
|
||||
+ 缩回转移球
|
||||
+ 蓄力到位,收到掉落信号和已伸出信号
|
||||
+ 根据视觉拟合信息的动态调整
|
||||
+ 拨置👇发射清空掉落信号
|
||||
4
R1.ioc
4
R1.ioc
@ -21,7 +21,7 @@ CAN2.CalculateTimeQuantum=71.42857142857143
|
||||
CAN2.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,BS1,BS2,Prescaler,TXFP,NART
|
||||
CAN2.NART=DISABLE
|
||||
CAN2.Prescaler=3
|
||||
CAN2.TXFP=ENABLE
|
||||
CAN2.TXFP=DISABLE
|
||||
Dma.Request0=SPI1_RX
|
||||
Dma.Request1=SPI1_TX
|
||||
Dma.Request2=USART3_RX
|
||||
@ -181,8 +181,6 @@ MxCube.Version=6.14.1
|
||||
MxDb.Version=DB.6.0.141
|
||||
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN2_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
|
||||
@ -14,18 +14,15 @@
|
||||
#ifndef _AUTO
|
||||
#define _AUTO 0
|
||||
#endif
|
||||
|
||||
|
||||
#define ONE_CONTROL 1
|
||||
//是否使用大疆DT7遥控器
|
||||
#ifndef DT7
|
||||
#define DT7 0
|
||||
#endif
|
||||
//是否使用三摩擦
|
||||
#ifndef HANDLING3
|
||||
#define HANDLING3 1
|
||||
#endif
|
||||
//云台使用类型
|
||||
#ifndef GM6020ING
|
||||
#define GM6020ING 1
|
||||
#endif
|
||||
|
||||
|
||||
//=============================================
|
||||
//================任务通知,时间组================//
|
||||
//事件组
|
||||
@ -34,15 +31,14 @@
|
||||
|
||||
//================任务通知================//
|
||||
//运球
|
||||
#define BALL_OK (1<<1)
|
||||
//接到放球命令
|
||||
#define PUT_CMD (1<<2)
|
||||
//运球结束
|
||||
#define PUT_OK (1<<2)
|
||||
#define BALL_DOWN (1<<1)
|
||||
|
||||
//运球结束
|
||||
//#define PREPARE (1<<0)
|
||||
|
||||
//伸缩结束
|
||||
#define EXTEND_OK (1<<3)
|
||||
|
||||
//take任务要等待上面两个标志位
|
||||
#define TAKE_WAIT (0x0C)
|
||||
//要发送ok了
|
||||
#define BALL_SEND (1<<6)
|
||||
//能够处理放球命令
|
||||
|
||||
@ -66,17 +66,17 @@
|
||||
(ptr)->offset_ecd = (ptr)->ecd; \
|
||||
}
|
||||
|
||||
static void VescMotor_Decode(CAN_MotorFeedback_t *feedback, CAN_RxHeaderTypeDef *rx_header, const uint8_t *rx_data)
|
||||
{
|
||||
// 检查接收到的ID是否匹配我们期望的VESC状态消息ID
|
||||
// 提取数据字段
|
||||
feedback->rotor_speed = ((int32_t)(rx_data[0]) << 24) | ((int32_t)(rx_data[1]) << 16) | ((int32_t)(rx_data[2]) << 8) | ((int32_t)(rx_data[3]));
|
||||
feedback->torque_current = ((int16_t)(rx_data[5]) << 8) | (int16_t)(rx_data[4]);
|
||||
feedback->torque_current /= 1000; // 将单位从 0.1A 转换为实际电流值
|
||||
feedback->duty_cycle = ((int16_t)(rx_data[7]) << 8) | (int16_t)(rx_data[6]);
|
||||
feedback->duty_cycle /= 1000.0f; // 将单位从千分之一转换为实际的占空比值 (-1.0 到 1.0)
|
||||
// static void VescMotor_Decode(CAN_MotorFeedback_t *feedback, CAN_RxHeaderTypeDef *rx_header, const uint8_t *rx_data)
|
||||
// {
|
||||
// // 检查接收到的ID是否匹配我们期望的VESC状态消息ID
|
||||
// // 提取数据字段
|
||||
// feedback->rotor_speed = ((int32_t)(rx_data[0]) << 24) | ((int32_t)(rx_data[1]) << 16) | ((int32_t)(rx_data[2]) << 8) | ((int32_t)(rx_data[3]));
|
||||
// feedback->torque_current = ((int16_t)(rx_data[5]) << 8) | (int16_t)(rx_data[4]);
|
||||
// feedback->torque_current /= 1000; // 将单位从 0.1A 转换为实际电流值
|
||||
// feedback->duty_cycle = ((int16_t)(rx_data[7]) << 8) | (int16_t)(rx_data[6]);
|
||||
// feedback->duty_cycle /= 1000.0f; // 将单位从千分之一转换为实际的占空比值 (-1.0 到 1.0)
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
//小米
|
||||
#define ID_xiaomi 1
|
||||
@ -178,23 +178,6 @@ void djiMotorEncode()
|
||||
|
||||
void can2MotorEncode()
|
||||
{
|
||||
switch (rx_header.ExtId) {
|
||||
|
||||
case CAN_VESC5065_M1_MSG1:
|
||||
// 存储消息到对应的电机结构体中
|
||||
VescMotor_Decode(&motor_5065[0], &rx_header,rx_data);
|
||||
break;
|
||||
case CAN_VESC5065_M2_MSG1:
|
||||
// 存储消息到对应的电机结构体中
|
||||
VescMotor_Decode(&motor_5065[1], &rx_header,rx_data);
|
||||
case CAN_VESC5065_M3_MSG1:
|
||||
// 存储消息到对应的电机结构体中
|
||||
VescMotor_Decode(&motor_5065[2], &rx_header,rx_data);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
switch (rx_data[0])
|
||||
{
|
||||
@ -239,6 +222,8 @@ void Dji_Motor_CB()
|
||||
void can2_Motor_CB(void)
|
||||
{
|
||||
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
|
||||
|
||||
//osThreadFlagsSet(thread_alert, EVENT_CAN);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -249,12 +234,15 @@ 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,
|
||||
can2_Motor_CB);
|
||||
|
||||
can_filter_init();
|
||||
can_filter_init();
|
||||
}
|
||||
|
||||
/**
|
||||
@ -486,6 +474,27 @@ return DEVICE_OK;
|
||||
feedback->can_id = rx_data[0];
|
||||
}
|
||||
|
||||
void XiaomiWait_init(int id,CAN_HandleTypeDef*hcan)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
|
||||
tx_message.StdId = id;
|
||||
tx_message.IDE = CAN_ID_STD;
|
||||
tx_message.RTR = CAN_RTR_DATA;
|
||||
tx_message.DLC = 0x08;
|
||||
|
||||
can_send_data[0] = 0xFF;
|
||||
can_send_data[1] = 0xFF;
|
||||
can_send_data[2] = 0xFF;
|
||||
can_send_data[3] = 0xFF;
|
||||
can_send_data[4] = 0xFF;
|
||||
can_send_data[5] = 0xFF;
|
||||
can_send_data[6] = 0xFF;
|
||||
can_send_data[7] = 0xFC;
|
||||
|
||||
HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
|
||||
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
|
||||
{
|
||||
@ -507,12 +516,12 @@ void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
|
||||
can_send_data[2] = (speed_code >> 4) & 0xFF; // 速度高8位（取12位中的高8）
|
||||
can_send_data[3] = ((speed_code & 0x0F) << 4) | ((JZ_xiaomi->K_P >> 8) & 0x0F); // 速度低4位 + Kp高4位
|
||||
|
||||
can_send_data[4] = JZ_xiaomi->K_P & 0xFF; // Kp低8位
|
||||
can_send_data[4] = JZ_xiaomi->K_P & 0xFF; // Kp低8位 位置增益
|
||||
|
||||
can_send_data[5] = (JZ_xiaomi->K_D >> 4) & 0xFF; // Kd高8位（取12位中的高8）
|
||||
can_send_data[5] = (JZ_xiaomi->K_D >> 4) & 0xFF; // Kd高8位（取12位中的高8） 位置阻尼
|
||||
can_send_data[6] = ((JZ_xiaomi->K_D & 0x0F) << 4) | ((current_code >> 8) & 0x0F); // Kd低4位 + 电流高4位
|
||||
|
||||
can_send_data[7] = current_code & 0xFF; // 电流低8位
|
||||
can_send_data[7] = current_code & 0xFF; // 电流低8位 力矩
|
||||
|
||||
}
|
||||
else
|
||||
@ -532,3 +541,7 @@ void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
|
||||
HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
MotorFeedback_t * get_CyberGear_point()
|
||||
{
|
||||
return &MotorFeedback;
|
||||
}
|
||||
|
||||
@ -98,6 +98,29 @@ typedef struct
|
||||
|
||||
} motor_measure_t;
|
||||
|
||||
//小米
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int16_t position;
|
||||
int16_t speed;
|
||||
int16_t K_P;
|
||||
int16_t K_D;
|
||||
int16_t K_C;
|
||||
|
||||
int16_t Pmax;//决定最大角度值，+-1 -> 最大+-360°
|
||||
|
||||
}JZ_xiaomi_t;
|
||||
|
||||
typedef struct {
|
||||
|
||||
uint8_t can_id;
|
||||
float position_deg;
|
||||
float speed_rads;
|
||||
float current_A;
|
||||
|
||||
} MotorFeedback_t;
|
||||
|
||||
|
||||
//motor calc ecd to angle
|
||||
#define MOTOR_ECD_TO_ANGLE_3508 (360.0 / 8191.0 / (3591.0/187.0))
|
||||
@ -156,6 +179,8 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand
|
||||
extern motor_measure_t *get_motor_point(uint8_t i);
|
||||
|
||||
extern CAN_MotorFeedback_t *get_vesc_point(uint8_t i);
|
||||
|
||||
extern MotorFeedback_t *get_CyberGear_point(void);
|
||||
/**
|
||||
* @brief 数据处理函数
|
||||
* @param[in] none
|
||||
@ -172,33 +197,9 @@ void CAN_VESC_HEAD(uint8_t controller_id);
|
||||
int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan);
|
||||
|
||||
|
||||
//小米
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int16_t position;
|
||||
int16_t speed;
|
||||
int16_t K_P;
|
||||
int16_t K_D;
|
||||
int16_t K_C;
|
||||
|
||||
int16_t Pmax;//决定最大角度值，+-1 -> 最大+-360°
|
||||
|
||||
}JZ_xiaomi_t;
|
||||
|
||||
typedef struct {
|
||||
|
||||
uint8_t can_id;
|
||||
float position_deg;
|
||||
float speed_rads;
|
||||
float current_A;
|
||||
|
||||
} MotorFeedback_t;
|
||||
|
||||
|
||||
|
||||
void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) ;
|
||||
|
||||
void XiaomiWait_init(int id,CAN_HandleTypeDef*hcan);
|
||||
|
||||
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan);
|
||||
|
||||
|
||||
@ -68,7 +68,7 @@ int8_t NUC_SendPacket(void *data, uint16_t length) {
|
||||
|
||||
return DEVICE_OK; // 发送成功
|
||||
}
|
||||
|
||||
int wzcsb=0;
|
||||
int8_t NUC_RawParse(NUC_t *n) {
|
||||
if (n == NULL) return DEVICE_ERR_NULL;
|
||||
union {
|
||||
@ -86,7 +86,8 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
case VISION:
|
||||
/* 协议格式
|
||||
0xFF HEAD
|
||||
0x02 控制帧
|
||||
0x09
|
||||
控制帧
|
||||
0x01 相机帧
|
||||
x fp32
|
||||
y fp32
|
||||
@ -101,10 +102,10 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
instance.data[0] = nucbuf[3];
|
||||
n->vision.x = instance.x[0];
|
||||
|
||||
instance.data[7] = nucbuf[7];
|
||||
instance.data[6] = nucbuf[8];
|
||||
instance.data[5] = nucbuf[9];
|
||||
instance.data[4] = nucbuf[10];
|
||||
instance.data[7] = nucbuf[10];
|
||||
instance.data[6] = nucbuf[9];
|
||||
instance.data[5] = nucbuf[8];
|
||||
instance.data[4] = nucbuf[7];
|
||||
n->vision.y = instance.x[1];
|
||||
|
||||
instance.data[11] = nucbuf[11];
|
||||
@ -119,6 +120,7 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
|
||||
|
||||
error:
|
||||
wzcsb++;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
|
||||
@ -22,18 +22,18 @@ void user_delay_ms(uint16_t ms)
|
||||
}
|
||||
}
|
||||
|
||||
//¸¡µãÊý·¶Î§ÏÞÖÆ
|
||||
void abs_limit_fp(fp32 *num, fp32 Limit)
|
||||
{
|
||||
if (*num > Limit)
|
||||
{
|
||||
*num = Limit;
|
||||
}
|
||||
else if (*num < -Limit)
|
||||
{
|
||||
*num = -Limit;
|
||||
}
|
||||
}
|
||||
// //¸¡µãÊý·¶Î§ÏÞÖÆ
|
||||
// void abs_limit_fp(fp32 *num, fp32 Limit)
|
||||
// {
|
||||
// if (*num > Limit)
|
||||
// {
|
||||
// *num = Limit;
|
||||
// }
|
||||
// else if (*num < -Limit)
|
||||
// {
|
||||
// *num = -Limit;
|
||||
// }
|
||||
// }
|
||||
|
||||
//ÕûÊý·¶Î§ÏÞÖÆ
|
||||
void abs_limit_int(int64_t *num, int64_t Limit)
|
||||
@ -137,3 +137,33 @@ bool is_reached_multiple(float current1, float current2, float current3, float t
|
||||
}
|
||||
return false; // 未满足条件达到阈值，返回 0
|
||||
}
|
||||
|
||||
|
||||
//限幅
|
||||
void abs_limit_fp(fp32 *num, fp32 Limit)
|
||||
{
|
||||
if (*num > Limit)
|
||||
{
|
||||
*num = Limit;
|
||||
}
|
||||
else if (*num < -Limit)
|
||||
{
|
||||
*num = -Limit;
|
||||
}
|
||||
}
|
||||
//改原始值，限制最大最小
|
||||
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max)
|
||||
{
|
||||
if (*num > Limit_max)
|
||||
{
|
||||
*num = Limit_max;
|
||||
return Limit_max;
|
||||
}
|
||||
else if (*num < Limit_min)
|
||||
{
|
||||
*num = Limit_min;
|
||||
return Limit_min;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -34,6 +34,8 @@ float expo_map(float value, float expo_factor);
|
||||
|
||||
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold);
|
||||
|
||||
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -7,33 +7,10 @@
|
||||
#include "shoot.hpp"
|
||||
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
extern int key;
|
||||
extern int16_t M2006_result;
|
||||
|
||||
// C键 sw[5]👆 1800 中1000 👇200
|
||||
// D键 sw[6]👆 200 👇1800
|
||||
|
||||
//伸缩
|
||||
#define M_SPEED 4000
|
||||
#define I_ANGLE1 180
|
||||
#define I_ANGLE2 -75
|
||||
#define O_ANGLE1 340
|
||||
#define O_ANGLE2 -240
|
||||
|
||||
// 三摩擦轮电机
|
||||
#define M_SPEED1 3000
|
||||
#define M_BACK -4000
|
||||
|
||||
#if HANDLING3 == 1
|
||||
//三摩擦轮运球！！！
|
||||
//添加平移3508 得跑位置吧
|
||||
|
||||
const fp32 Ball:: M3508_speed_PID[3] = {30, 0.03, 0};
|
||||
const fp32 Ball:: Extend_speed_PID[3] = { 40, 0.0, 0.1};
|
||||
const fp32 Ball:: Extend_angle_PID[3]= { 25, 0.1, 0};
|
||||
|
||||
//摩擦轮转速
|
||||
int speedm=0;
|
||||
#define I_ANGLE 147
|
||||
#define O_ANGLE 187
|
||||
|
||||
//PE11 气缸
|
||||
|
||||
@ -41,138 +18,101 @@ Ball ::Ball()
|
||||
{
|
||||
detect_init();
|
||||
|
||||
//三摩擦轮
|
||||
for(int i = 0;i < MOTOR_NUM;i ++)
|
||||
{
|
||||
hand_Motor[i] = get_motor_point(i);
|
||||
if(i <=3)
|
||||
{
|
||||
hand_Motor[i]->type = M3508;//设置电机类型
|
||||
PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 10000);//pid初始化
|
||||
}
|
||||
result[i] = 0;
|
||||
speedSet[i] = 0;
|
||||
}
|
||||
//小米电机
|
||||
feedback=get_CyberGear_point();
|
||||
|
||||
//两个伸缩6020 左207 右208
|
||||
Extern_Motor[0] = get_motor_point(6);
|
||||
Extern_Motor[1] = get_motor_point(7);
|
||||
//小米电机初始化
|
||||
xiaomi.position = I_ANGLE; //
|
||||
xiaomi.speed = 25; //
|
||||
xiaomi.K_P = 80; // 位置增益
|
||||
xiaomi.K_D =20; // 位置阻尼
|
||||
xiaomi.K_C = 50 ; // 力矩
|
||||
xiaomi.Pmax =1; //好像没啥用
|
||||
|
||||
Extern_Motor[0]->type = GM6020;
|
||||
Extern_Motor[1]->type = GM6020;
|
||||
PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,10000, 2000);
|
||||
PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,20000, 1000);
|
||||
|
||||
PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,10000, 2000);
|
||||
PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,20000, 1000);
|
||||
|
||||
e_result[0] = 0;
|
||||
e_result[1] = 0;
|
||||
angleSet[0] = 0;
|
||||
angleSet[1] = 0;
|
||||
|
||||
//状态机状态初始化
|
||||
currentState1= BALL_IDLE;
|
||||
// //小米电机初始化
|
||||
// xiaomi.position = 0; //
|
||||
// xiaomi.speed = 0; //
|
||||
// xiaomi.K_P = 0; //
|
||||
// xiaomi.K_D =0; //
|
||||
// xiaomi.K_C = 0 ;
|
||||
// xiaomi.Pmax =0;
|
||||
|
||||
//状态机状态初始化
|
||||
currentState1= BALL_IDLE;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Ball ::Extend_mcontrol(int angle1,int angle2)
|
||||
{
|
||||
int16_t delta[2];
|
||||
angleSet[0] = angle1/2;
|
||||
angleSet[1] = angle2/2;
|
||||
|
||||
fp32 angle_get[2];
|
||||
angle_get[0] = motor_ecd_to_angle_change(Extern_Motor[0]->ecd, angleSet[0]);
|
||||
angle_get[1] = motor_ecd_to_angle_change(Extern_Motor[1]->ecd, angleSet[1]);
|
||||
|
||||
delta[0] = PID_calc(&extend_angle_pid[0], angle_get[0] , angleSet[0]);
|
||||
e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]);
|
||||
|
||||
delta[1] = PID_calc(&extend_angle_pid[1], angle_get[1] , angleSet[1]);
|
||||
e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Ball ::Spin()
|
||||
{
|
||||
|
||||
speedSet[MOTOR_1] = speed_set;
|
||||
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
|
||||
|
||||
speedSet[MOTOR_2] = speed_set;
|
||||
result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);
|
||||
|
||||
speedSet[MOTOR_3] = speed_set;
|
||||
result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);
|
||||
|
||||
}
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
|
||||
speed_set=500;
|
||||
|
||||
}
|
||||
|
||||
void Ball::Move_Extend(void)
|
||||
{
|
||||
if(rc_ctrl.sw[6] == 200) // 左拨杆上
|
||||
{
|
||||
Extend_mcontrol(I_ANGLE1, I_ANGLE2); // 内伸
|
||||
}
|
||||
else if(rc_ctrl.sw[6] == 1800) // 左拨杆下
|
||||
{
|
||||
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
|
||||
}
|
||||
else
|
||||
{
|
||||
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
|
||||
}
|
||||
}
|
||||
|
||||
// C键 sw[5]👆 200 中1000 👇1800
|
||||
// D键 sw[6]👆 200 👇1800
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800
|
||||
// G键 sw[6]👆 200 中 1000 👇1800
|
||||
// sw[5] 👆 200 👇1800
|
||||
//左旋 sw[7] 200 --1800
|
||||
void Ball::rc_mode()
|
||||
{
|
||||
if(rc_ctrl.sw[5]==200)
|
||||
if(rc_ctrl.sw[6]==200)
|
||||
{
|
||||
rc_key=UP2;
|
||||
}
|
||||
if(rc_ctrl.sw[5]==1000)
|
||||
if(rc_ctrl.sw[6]==1000)
|
||||
{
|
||||
rc_key=MIDDLE2;
|
||||
}
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
if(rc_ctrl.sw[6]==1800)
|
||||
{
|
||||
rc_key=DOWN2;
|
||||
}
|
||||
|
||||
if(rc_ctrl.sw[6]==200)
|
||||
if(rc_ctrl.sw[5]==200)
|
||||
{
|
||||
extern_key=IN;
|
||||
}
|
||||
if(rc_ctrl.sw[6]==1800)
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
{
|
||||
extern_key=OUT;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Ball::Send_control()
|
||||
{
|
||||
|
||||
CAN_XiaoMi(1,&xiaomi,&hcan2);
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
|
||||
#if ONE_CONTROL==0
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
}
|
||||
|
||||
void Ball::Move_Extend()
|
||||
{
|
||||
if(extern_key==IN)
|
||||
{
|
||||
xiaomi.position = I_ANGLE;
|
||||
}
|
||||
if(extern_key==OUT)
|
||||
{
|
||||
xiaomi.position = O_ANGLE;
|
||||
}
|
||||
}
|
||||
|
||||
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); // 读取光电状态(有球 1，无球 0)
|
||||
|
||||
Move_Extend();
|
||||
|
||||
switch (rc_key){
|
||||
case MIDDLE2:
|
||||
speed_set=0;
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (currentState1 == BALL_FINISH)
|
||||
{
|
||||
currentState1 = BALL_IDLE;
|
||||
@ -183,109 +123,37 @@ void Ball::ball_control()
|
||||
break;
|
||||
|
||||
case UP2:
|
||||
Three_Handing();
|
||||
ballDown();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
ballDown();
|
||||
ballHadling();
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
Spin();
|
||||
|
||||
Send_control();
|
||||
|
||||
}
|
||||
|
||||
void Ball::Three_Handing()
|
||||
{
|
||||
switch (currentState1){
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
|
||||
currentState1 = BALL_FORWARD; // 切换到正转状态
|
||||
|
||||
break;
|
||||
case BALL_FORWARD:
|
||||
speed_set=M_SPEED1;
|
||||
if(is_reached_multiple(hand_Motor[MOTOR_1]->speed_rpm,
|
||||
hand_Motor[MOTOR_2]->speed_rpm,
|
||||
hand_Motor[MOTOR_3]->speed_rpm,
|
||||
speed_set,
|
||||
speed_set,
|
||||
speed_set,
|
||||
50.f,50))
|
||||
{
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
|
||||
if (ball_state == 0) // 读光电 有球 1 无球 0
|
||||
{
|
||||
osDelay(200); // 延时200ms
|
||||
speed_set = M_BACK;
|
||||
currentState1 = BALL_REVERSE; // 切换到反转状态
|
||||
}
|
||||
break;
|
||||
case BALL_REVERSE:
|
||||
if (ball_state == 1)
|
||||
{
|
||||
speed_set = 0;
|
||||
currentState1 = BALL_CLOSE; // 切换到完成状态
|
||||
}
|
||||
break;
|
||||
case BALL_CLOSE:
|
||||
if(ball_state == 1)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
currentState1 = BALL_FINISH; // 切换到完成状态
|
||||
|
||||
}
|
||||
case BALL_FINISH:
|
||||
key = 0; // 重置按键状态
|
||||
speed_set = 0;
|
||||
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
|
||||
//currentState1 = BALL_IDLE; // 直接回到空闲状态
|
||||
break;
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void Ball::Send_control()
|
||||
{
|
||||
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
|
||||
osDelay(1);
|
||||
|
||||
CAN_cmd_1FF(M2006_result,0,e_result[0],e_result[1],&hcan1);
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
int ball_state = 0;
|
||||
int triggerCount = 0; // 光电传感器触发计数
|
||||
int last_ball_state = 1; // 上一次的光电状态
|
||||
//(有球 0,无球 1)
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
|
||||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态（有球 0，无球 1）
|
||||
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸闭合
|
||||
if (key > 0) // 检测按键是否被按下
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (rc_key == DOWN2) // 检测按键是否被按下
|
||||
{
|
||||
key = 0; // 重置按键状态
|
||||
|
||||
triggerCount = 0; // 重置触发计数
|
||||
currentState1 = BALL_FORWARD; // 切换到正转状态
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
break;
|
||||
|
||||
@ -324,25 +192,210 @@ void Ball::ballHadling(void)
|
||||
case BALL_CLOSE:
|
||||
if (triggerCount == 2) // 确保是第二次触发
|
||||
{
|
||||
osDelay(100); // 延时去抖
|
||||
osDelay(50);
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_REVERSE; // 切换到反转状态
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_REVERSE:
|
||||
case BALL_FINISH:
|
||||
osDelay(50); // 延时 50ms
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
key = 0; // 重置按键状态
|
||||
|
||||
triggerCount = 0; // 重置触发计数
|
||||
currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
|
||||
//currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
void Ball::ball_control()
|
||||
{
|
||||
hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0)
|
||||
|
||||
switch (rc_key){
|
||||
case MIDDLE2:
|
||||
Idle_control();
|
||||
break;
|
||||
|
||||
case UP2:
|
||||
ballDown();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
ballHadling();
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
Send_control();
|
||||
|
||||
}
|
||||
|
||||
|
||||
//任务通知来作全过程
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
switch(currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
xiaomi.position = I_ANGLE; // 保持收回
|
||||
if(feedback->position_deg >= I_ANGLE-0.8 && feedback->position_deg <= I_ANGLE+0.8)
|
||||
{
|
||||
currentState1 = EXTEND_DOWN;
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_DOWN:
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
// 检测到球自由下落一次就切换状态
|
||||
if(ball_state == 0)
|
||||
{
|
||||
currentState1 = EXTEND_OUT;
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_OUT:
|
||||
xiaomi.position = O_ANGLE; // 保持伸出
|
||||
if(feedback->position_deg >= O_ANGLE-0.2 && feedback->position_deg <= O_ANGLE+0.2)
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
|
||||
currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_FINISH:
|
||||
xiaomi.position = O_ANGLE; // 一直保持伸出
|
||||
// 等待拨杆复位（如切到MIDDLE2），Idle_control会负责回位
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Ball::Idle_control()
|
||||
{
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
osThreadFlagsClear(PREPARE);
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
|
||||
// 拨杆回到中间挡位时，回位并重置状态机
|
||||
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;
|
||||
}
|
||||
//xiaomi.position = I_ANGLE;
|
||||
|
||||
}
|
||||
|
||||
int ball_state = 0;
|
||||
int triggerCount = 0; // 光电传感器触发计数
|
||||
int last_ball_state = 1; // 上一次的光电状态
|
||||
//(有球 0,无球 1)
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (rc_key == DOWN2)
|
||||
{
|
||||
xiaomi.position = O_ANGLE;//外伸
|
||||
triggerCount = 0; // 重置触发计数
|
||||
if(feedback->position_deg>= O_ANGLE-1)// 确保伸缩电机到位
|
||||
{
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_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_state == 0 && last_ball_state == 1) // 检测到状态从无球变为有球
|
||||
{
|
||||
triggerCount++; // 增加触发计数
|
||||
if (triggerCount == 1) // 第一次触发
|
||||
{
|
||||
currentState1 = BALL_FLAG; // 切换到等待第二次触发状态
|
||||
}
|
||||
}
|
||||
last_ball_state = ball_state; // 更新上一次的状态
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
// osDelay(10); // 延时 50ms
|
||||
if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球
|
||||
{
|
||||
triggerCount++; // 增加触发计数
|
||||
currentState1 = BALL_CLOSE; // 切换到闭合气缸状态
|
||||
}
|
||||
last_ball_state = ball_state; // 更新上一次的状态
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
if (triggerCount == 2) // 确保是第二次触发
|
||||
{
|
||||
osDelay(50);
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FINISH:
|
||||
osDelay(50); // 延时 50ms
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
triggerCount = 0; // 重置触发计数
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -11,19 +11,23 @@
|
||||
#include "filter.h"
|
||||
#include "calc_lib.h"
|
||||
|
||||
|
||||
|
||||
// 定义状态枚举
|
||||
typedef enum {
|
||||
BALL_IDLE, // 空闲状态
|
||||
BALL_FORWARD, // 正转状态
|
||||
BALL_DROP, // 球下落状态
|
||||
BALL_REVERSE, // 反转状态
|
||||
BALL_FORWARD, //
|
||||
BALL_DROP, //
|
||||
BALL_FLAG,
|
||||
BALL_CLOSE, // 关闭状态
|
||||
BALL_FINISH, // 完成状态
|
||||
|
||||
//持球状态
|
||||
BALL_TAKE,
|
||||
BALL_LOSE
|
||||
EXTEND_DOWN,
|
||||
EXTEND_OUT,
|
||||
EXTEND_FINISH,
|
||||
BALL_LOSE,
|
||||
|
||||
} BallState_t;
|
||||
|
||||
@ -52,54 +56,36 @@ class Ball
|
||||
{
|
||||
public:
|
||||
Ball();
|
||||
void Spin(void);
|
||||
void ballHadling(void);
|
||||
void ballDown(void);
|
||||
void Send_control(void);
|
||||
void ballStop(void);
|
||||
void Move_Extend(void);
|
||||
void Idle_control(void);
|
||||
void rc_mode(void);
|
||||
void Extend_mcontrol(int angle1,int angle2);
|
||||
void Three_Handing(void);
|
||||
void ball_control(void);
|
||||
|
||||
BallState_t currentState1; // 当前状态
|
||||
int flag;//暂时还没用到
|
||||
BallState_t currentState1; // 运球任务状态机
|
||||
int flag_thread;//暂时还没用到
|
||||
int ball_state ;//光电检测
|
||||
|
||||
//伸缩6020
|
||||
int16_t e_result[2];
|
||||
motor_measure_t * Extern_Motor[2];
|
||||
|
||||
float speed_set;
|
||||
int16_t result[MOTOR_NUM];
|
||||
motor_measure_t *hand_Motor[MOTOR_NUM];
|
||||
//小米电机伸缩
|
||||
MotorFeedback_t *feedback;
|
||||
JZ_xiaomi_t xiaomi;
|
||||
|
||||
|
||||
//==========================公共变量==========================//
|
||||
int16_t rc_key; //遥控器按键
|
||||
int16_t extern_key;
|
||||
//用于传接球,运球后通知
|
||||
volatile BallState_t ballStatus;//是否有球
|
||||
volatile uint32_t flag_thread;//接收传回的线程通知
|
||||
volatile uint32_t hand_thread;//接收传回的线程通知
|
||||
|
||||
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];
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -20,27 +20,31 @@ NUC_t nuc_v;
|
||||
//B键 sw[3]👆 200 开 👇1800 关
|
||||
//左旋钮 sw[4] 👈 200 👉1800
|
||||
|
||||
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
|
||||
const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0, 0.1};
|
||||
//尽量别动这组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 INIT_POS -130
|
||||
#define TOP_POS -210
|
||||
#define TOP_POS -211
|
||||
#define BOTTOM_POS 0
|
||||
#define GO_ERROR 1.0f
|
||||
#define Tigger_DO -5
|
||||
#define Tigger_DO 0
|
||||
#define Tigger_ZERO 100
|
||||
#define Tigger_ERROR 3
|
||||
|
||||
//💩
|
||||
int16_t M2006_result =0;
|
||||
|
||||
float knob_increment;
|
||||
|
||||
Shoot::Shoot()
|
||||
{
|
||||
//扳机初始化
|
||||
trigger_Motor= get_motor_point(4);//id 205
|
||||
trigger_Motor= get_motor_point(0);//id 201
|
||||
trigger_Motor->type=M2006;
|
||||
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,3000, 2000);//pid初始化
|
||||
PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,3000, 2000);//pid初始化
|
||||
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;
|
||||
|
||||
@ -53,6 +57,7 @@ Shoot::Shoot()
|
||||
go1.W = 0,
|
||||
go1.T = 0,
|
||||
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
fire_pos = INIT_POS; // 发射位置
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback);//注册串口回调函数，bsp层
|
||||
@ -61,6 +66,7 @@ Shoot::Shoot()
|
||||
|
||||
currentState= SHOOT_IDLE;
|
||||
|
||||
LowPassFilter2p_Init(&distance_filter,500.0f,80.0f); //给distance 做滤波
|
||||
|
||||
}
|
||||
|
||||
@ -69,16 +75,17 @@ void Shoot::trigger_control()
|
||||
int delta = 0;
|
||||
delta = PID_calc(&angle_pid,trigger_Motor->total_angle,t_posSet); // 计算位置环PID
|
||||
result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,delta); // 计算速度环PID
|
||||
M2006_result=result;
|
||||
// result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
|
||||
// CAN_cmd_1FF(result,0,0,0,&hcan1);
|
||||
|
||||
|
||||
CAN_cmd_200(result,0,0,0,&hcan1);
|
||||
}
|
||||
|
||||
|
||||
void Shoot :: distanceGet(const NUC_t &nuc_v)
|
||||
{
|
||||
|
||||
//test_distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
distance= LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理
|
||||
//distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
|
||||
}
|
||||
|
||||
@ -115,68 +122,108 @@ int Shoot::GO_SendData(float pos,float limit)
|
||||
return 0;
|
||||
}
|
||||
|
||||
//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
|
||||
// F键 sw[0]👆 1800 👇200
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800
|
||||
// G键 sw[6]👆 1800 中 1000 👇200
|
||||
// B键 sw[3]👆 200 开 中 1000 👇1800 关
|
||||
//左旋 sw[7] 200 --1800
|
||||
void Shoot::rc_mode()
|
||||
{
|
||||
if(rc_ctrl.sw[1]==200)
|
||||
{
|
||||
rc_key=UP1;
|
||||
}
|
||||
if(rc_ctrl.sw[1]==1000)
|
||||
if(rc_ctrl.sw[1]==1800 && rc_ctrl.sw[2]==200)
|
||||
{
|
||||
rc_key=MIDDLE1;
|
||||
}
|
||||
if(rc_ctrl.sw[1]==1800)
|
||||
if(rc_ctrl.sw[2]==1800 && rc_ctrl.sw[1]==1800)
|
||||
{
|
||||
rc_key=DOWN1;
|
||||
}
|
||||
|
||||
if(rc_ctrl.sw[7]==200)
|
||||
if(rc_ctrl.sw[0]==1800)
|
||||
{
|
||||
trigger_key=WAIT;
|
||||
mode_key=TEST;
|
||||
}
|
||||
if(rc_ctrl.sw[7]==1800)
|
||||
if(rc_ctrl.sw[0]==200)
|
||||
{
|
||||
trigger_key=SHOOT;
|
||||
mode_key=VSION;
|
||||
}
|
||||
if(rc_ctrl.sw[3]==1800)
|
||||
{
|
||||
ready_key=PREPARE;
|
||||
}
|
||||
else
|
||||
{
|
||||
ready_key=0; //默认不准备
|
||||
}
|
||||
|
||||
//knob_increment=rc_ctrl.ch[2]/150;
|
||||
|
||||
//旋钮增量
|
||||
static int last_knob_value = 0; // 记录旋钮的上一次值
|
||||
int current_knob_value = rc_ctrl.sw[4]; // 获取当前旋钮值
|
||||
// if(rc_ctrl.sw[7]==200)
|
||||
// {
|
||||
// trigger_key=WAIT;
|
||||
// }
|
||||
// if(rc_ctrl.sw[7]==1800)
|
||||
// {
|
||||
// trigger_key=SHOOT;
|
||||
// }
|
||||
|
||||
// 计算旋钮增量
|
||||
if (current_knob_value >= 200 && current_knob_value <= 1800) {
|
||||
knob_increment = (current_knob_value - last_knob_value) / 15.0f; // 每80单位对应一个增量
|
||||
} else {
|
||||
knob_increment = 0; // 如果旋钮值超出范围，不产生增量
|
||||
}
|
||||
// //旋钮增量
|
||||
// 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);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#if ONE_CONTROL==0
|
||||
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
//fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
|
||||
fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
//t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= 95) { // 假设120度为发射完成角度
|
||||
if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
@ -196,9 +243,54 @@ void Shoot::shoot_control() {
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TEST:
|
||||
//实时可调蓄力位置
|
||||
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
|
||||
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;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, 5.0f);
|
||||
GO_SendData(go1.Pos,limit_speed);
|
||||
|
||||
// 控制扳机电机
|
||||
trigger_control();
|
||||
@ -211,6 +303,7 @@ void Shoot :: shoot_Current()
|
||||
case SHOOT_IDLE:
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
control_pos = TOP_POS; // 顶部位置
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
go1.Pos = control_pos;
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
if (feedback.fd_gopos <-209) {
|
||||
@ -220,7 +313,7 @@ void Shoot :: shoot_Current()
|
||||
|
||||
case SHOOT_TOP:
|
||||
t_posSet = Tigger_DO; // 扳机扣动钩住
|
||||
if (trigger_Motor->total_angle<-2)
|
||||
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f)
|
||||
{
|
||||
//判定为钩住
|
||||
is_hooked = true; // 标记钩子已钩住
|
||||
@ -232,30 +325,16 @@ 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) {
|
||||
BSP_Buzzer_Start();
|
||||
// BSP_Buzzer_Set(1,5000);
|
||||
// see_you_again();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
//currentState = SHOOT_WAIT; // 等待发射信号
|
||||
//在拨杆切换时触发了
|
||||
}
|
||||
}
|
||||
break;
|
||||
// case SHOOT_WAIT:
|
||||
// // 等待发射信号
|
||||
// if (trigger_key == SHOOT) {
|
||||
// currentState = SHOOT_FIRE; // 切换到发射状态
|
||||
// }
|
||||
// break;
|
||||
|
||||
// case SHOOT_FIRE:
|
||||
// // 发射逻辑
|
||||
// t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
// if (t_posSet <= 50) { // 假设250度为发射完成角度
|
||||
// currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
// is_hooked = false; // 重置钩子状态
|
||||
// }
|
||||
// break;
|
||||
|
||||
default:
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
@ -265,11 +344,191 @@ void Shoot :: shoot_Current()
|
||||
|
||||
void Shoot::RemoveError() {
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_POS;
|
||||
go1.Pos = control_pos;
|
||||
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)
|
||||
{
|
||||
//认为完全松开
|
||||
control_pos=INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
|
||||
}
|
||||
limit_speed=3.0f;//慢慢送上去
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试 反馈信息
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
|
||||
|
||||
shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key) {
|
||||
case MIDDLE1:
|
||||
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)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动 射出
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
//无自瞄拟合测试档
|
||||
case TEST:
|
||||
switch (rc_key) {
|
||||
case MIDDLE1:
|
||||
|
||||
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)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ball_receive(); // 执行接收球的操作
|
||||
}
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if(shoot_thread & EXTEND_OK)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动 射出
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, limit_speed);
|
||||
|
||||
// 控制扳机电机
|
||||
trigger_control();
|
||||
}
|
||||
|
||||
//配合运球到发射
|
||||
void Shoot ::ball_receive()
|
||||
{
|
||||
switch (currentState)
|
||||
{
|
||||
case SHOOT_IDLE:
|
||||
//底盘手动档直接蓄力
|
||||
if(ready_key == PREPARE){
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
control_pos = TOP_POS;
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
t_posSet = Tigger_ZERO;
|
||||
if(feedback.fd_gopos >= TOP_POS - 0.5f && feedback.fd_gopos <= TOP_POS + 0.5f)
|
||||
{
|
||||
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)
|
||||
{
|
||||
|
||||
currentState = BAKC; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
case BAKC:
|
||||
control_pos = BOTTOM_POS;
|
||||
limit_speed=TO_BOTTOM;//慢速下来
|
||||
if(feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
|
||||
{
|
||||
|
||||
//osThreadFlagsClear(PREPARE); // 清除任务通知标志位
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
//调整go电机位置
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
void Shoot::RemoveError() {
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_POS;
|
||||
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)
|
||||
{
|
||||
//认为完全松开
|
||||
control_pos=INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
|
||||
}
|
||||
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "nuc.h"
|
||||
#include "usart.h"
|
||||
#include "filter.h"
|
||||
|
||||
// 定义状态枚举
|
||||
typedef enum {
|
||||
@ -14,7 +15,10 @@ typedef enum {
|
||||
SHOOT_READY,
|
||||
SHOOT_WAIT,
|
||||
SHOOT_FIRE, // 发射状态
|
||||
SHOOT_RETURN // 自动返回状态
|
||||
SHOOT_RETURN, // 自动返回状态
|
||||
//运射配合
|
||||
GO_TOP,
|
||||
BAKC
|
||||
} ShootState_t;
|
||||
|
||||
// // 定义状态枚举
|
||||
@ -33,7 +37,10 @@ typedef enum
|
||||
MIDDLE1,
|
||||
DOWN1,
|
||||
SHOOT,
|
||||
WAIT
|
||||
WAIT,
|
||||
TEST,
|
||||
VSION,
|
||||
PREPARE
|
||||
}rc_mode;
|
||||
|
||||
// 光电传感器读取宏
|
||||
@ -50,8 +57,8 @@ public:
|
||||
void shoot_control(void);
|
||||
void shoot_Current(void);
|
||||
void RemoveError(void);
|
||||
void ball_receive(void);
|
||||
|
||||
|
||||
int GO_SendData(float pos,float limit);
|
||||
|
||||
//go电机
|
||||
@ -65,8 +72,7 @@ public:
|
||||
float fd_tpos;
|
||||
}feedback;
|
||||
|
||||
|
||||
|
||||
|
||||
//扳机
|
||||
float speed_trigger;
|
||||
int16_t result;
|
||||
@ -75,13 +81,18 @@ public:
|
||||
float fd_tpos; //扳机反馈位置
|
||||
motor_measure_t *trigger_Motor;
|
||||
|
||||
//滤波器
|
||||
LowPassFilter2p_t distance_filter; // 用于滤波视觉距离
|
||||
|
||||
//==========================公共变量==========================
|
||||
int16_t rc_key; //遥控器按键
|
||||
int16_t mode_key;
|
||||
int16_t ready_key;
|
||||
int16_t trigger_key;
|
||||
ShootState_t currentState; //状态机
|
||||
//volatile BallState_t ballStatus;//是否有球
|
||||
volatile uint32_t flag_thread;//接收传回的线程通知
|
||||
float distance; //视觉距离
|
||||
|
||||
volatile uint32_t shoot_thread;//接收传回的线程通知
|
||||
fp32 distance; //视觉距离
|
||||
|
||||
private:
|
||||
//扳机2006
|
||||
@ -95,6 +106,7 @@ private:
|
||||
|
||||
bool is_hooked;// 标记钩子是否已经钩住拉簧
|
||||
|
||||
float limit_speed;//go电机限速
|
||||
|
||||
|
||||
};
|
||||
|
||||
@ -10,17 +10,9 @@ extern RC_ctrl_t rc_ctrl;
|
||||
Ball ball;
|
||||
//float vofa[8];
|
||||
|
||||
|
||||
// C键 sw[5]👆 1800 中1000 👇200
|
||||
// D键 sw[6]👆 200 外伸 👇1800 归位 //三摩擦架子
|
||||
// H键 sw[7]👆 200 👇1800 //取球
|
||||
|
||||
int angle1=330;
|
||||
int angle2=-240;
|
||||
int test111=0;
|
||||
|
||||
//检查光电
|
||||
int abc=0;
|
||||
int aaaa=146;
|
||||
|
||||
extern int speedm;
|
||||
|
||||
@ -29,6 +21,10 @@ void FunctionBall(void *argument)
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
|
||||
|
||||
osDelay(6000);//等待极致控制板启动
|
||||
XiaomiWait_init(1,&hcan2); //小米电机初始化
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
@ -37,19 +33,17 @@ void FunctionBall(void *argument)
|
||||
task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
|
||||
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
|
||||
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
|
||||
|
||||
ball.rc_mode(); // 遥控器模式
|
||||
|
||||
ball.ball_control(); // 控制球的动作
|
||||
|
||||
// vofa[0] = -speedm; // 发送电机角度数据
|
||||
// vofa[1] = ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
|
||||
// vofa[2] = -ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
|
||||
// vofa[3] = -ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据
|
||||
// vofa_tx_main(vofa); // 发送数据到虚拟串口
|
||||
// ball.xiaomi.position = aaaa;
|
||||
// CAN_XiaoMi(1,&ball.xiaomi,&hcan2);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -23,6 +23,8 @@ 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)
|
||||
|
||||
@ -10,14 +10,7 @@ extern RC_ctrl_t rc_ctrl;
|
||||
Shoot shoot;
|
||||
NUC_t nucData; // 自瞄
|
||||
|
||||
int shoot_flag = 0;
|
||||
|
||||
int a2;
|
||||
|
||||
int goangle=0;
|
||||
|
||||
//E键 sw[0]👆 200 中 1000 👇1800
|
||||
//F键 sw[1]👆 1800 👇200
|
||||
int aaaxxx=0;
|
||||
|
||||
void FunctionShoot(void *argument)
|
||||
{
|
||||
@ -34,24 +27,18 @@ while(1)
|
||||
#endif
|
||||
|
||||
shoot.rc_mode(); //遥控器模式
|
||||
//我放的任务通知 运球成功放置过来后
|
||||
shoot.flag_thread=osThreadFlagsGet();
|
||||
|
||||
if(shoot.flag_thread & BALL_OK)
|
||||
{
|
||||
a2=2;
|
||||
}
|
||||
|
||||
|
||||
shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
|
||||
|
||||
|
||||
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
{
|
||||
shoot.distanceGet(nucData);
|
||||
}
|
||||
|
||||
//shoot.GO_SendData(goangle,5);
|
||||
shoot.shoot_control();
|
||||
|
||||
// shoot.t_posSet=aaaxxx;
|
||||
// shoot.trigger_control();
|
||||
// shoot.GO_SendData(goangle,5);
|
||||
// shoot.shoot_control();
|
||||
// shoot.t_posSet=goangle;
|
||||
// shoot.trigger_control();
|
||||
|
||||
|
||||
@ -19,7 +19,7 @@ extern "C" {
|
||||
#define TASK_FREQ_MONITOR (2u)
|
||||
#define TASK_FREQ_CAN (1500u)
|
||||
#define TASK_FREQ_AI (500u)
|
||||
#define TASK_FREQ_BALL (700u)
|
||||
#define TASK_FREQ_BALL (500u)
|
||||
|
||||
#define TASK_INIT_DELAY_INFO (500u)
|
||||
#define TASK_INIT_DELAY_MONITOR (10)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user