hehehe/engineer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加了一堆东西

Browse Source
This commit is contained in:
shentou 2025-12-22 19:46:54 +08:00
parent 2ebe359343
commit 0f6cf39492
38 changed files with 4785 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitattributes vendored
View File

@ -1 +1 @@
* text=auto * text=auto eol=lf

24
engineer/.mxproject
View File

File diff suppressed because one or more lines are too long

9
engineer/CMakeLists.txt
View File

@ -50,6 +50,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/bsp/mm.c User/bsp/mm.c
User/bsp/time.c User/bsp/time.c
User/bsp/dwt.c User/bsp/dwt.c
./User/bsp/uart.c
# User/component sources # User/component sources
User/component/ahrs.c User/component/ahrs.c
@ -58,6 +59,14 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/component/pid.c User/component/pid.c
User/component/user_math.c User/component/user_math.c
./User/component/mixer.c ./User/component/mixer.c
./User/component/cmd.c
./User/component/crc16.c
./User/component/crc8.c
./User/component/limiter.c
./User/component/matrix.cpp
./User/component/robotics.cpp
./User/component/utils.cpp
# User/device sources # User/device sources
User/device/motor.c User/device/motor.c

52
engineer/Core/Inc/dma.h Normal file
View File

@ -0,0 +1,52 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file dma.h
* @brief This file contains all the function prototypes for
* the dma.c file
******************************************************************************
* @attention
*
* Copyright (c) 2025 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __DMA_H__
#define __DMA_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* DMA memory to memory transfer handles -------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_DMA_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __DMA_H__ */

2
engineer/Core/Inc/stm32f4xx_hal_conf.h
View File

@ -64,7 +64,7 @@
/* #define HAL_MMC_MODULE_ENABLED */ /* #define HAL_MMC_MODULE_ENABLED */
/* #define HAL_SPI_MODULE_ENABLED */ /* #define HAL_SPI_MODULE_ENABLED */
#define HAL_TIM_MODULE_ENABLED #define HAL_TIM_MODULE_ENABLED
/* #define HAL_UART_MODULE_ENABLED */ #define HAL_UART_MODULE_ENABLED
/* #define HAL_USART_MODULE_ENABLED */ /* #define HAL_USART_MODULE_ENABLED */
/* #define HAL_IRDA_MODULE_ENABLED */ /* #define HAL_IRDA_MODULE_ENABLED */
/* #define HAL_SMARTCARD_MODULE_ENABLED */ /* #define HAL_SMARTCARD_MODULE_ENABLED */

2
engineer/Core/Inc/stm32f4xx_it.h
View File

@ -55,6 +55,8 @@ void DebugMon_Handler(void);
void CAN1_TX_IRQHandler(void); void CAN1_TX_IRQHandler(void);
void CAN1_RX0_IRQHandler(void); void CAN1_RX0_IRQHandler(void);
void CAN1_RX1_IRQHandler(void); void CAN1_RX1_IRQHandler(void);
void USART1_IRQHandler(void);
void USART2_IRQHandler(void);
void TIM8_TRG_COM_TIM14_IRQHandler(void); void TIM8_TRG_COM_TIM14_IRQHandler(void);
/* USER CODE BEGIN EFP */ /* USER CODE BEGIN EFP */

55
engineer/Core/Inc/usart.h Normal file
View File

@ -0,0 +1,55 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file usart.h
* @brief This file contains all the function prototypes for
* the usart.c file
******************************************************************************
* @attention
*
* Copyright (c) 2025 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USART_H__
#define __USART_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_USART1_UART_Init(void);
void MX_USART2_UART_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __USART_H__ */

65
engineer/Core/Src/dma.c Normal file
View File

@ -0,0 +1,65 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file dma.c
* @brief This file provides code for the configuration
* of all the requested memory to memory DMA transfers.
******************************************************************************
* @attention
*
* Copyright (c) 2025 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "dma.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/*----------------------------------------------------------------------------*/
/* Configure DMA */
/*----------------------------------------------------------------------------*/
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/**
* Enable DMA controller clock
*/
void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMA1_CLK_ENABLE();
__HAL_RCC_DMA2_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Stream5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
/* DMA1_Stream6_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn);
/* DMA2_Stream2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
/* DMA2_Stream7_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn);
}
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */

1
engineer/Core/Src/gpio.c
View File

@ -44,6 +44,7 @@ void MX_GPIO_Init(void)
/* GPIO Ports Clock Enable */ /* GPIO Ports Clock Enable */
__HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_GPIOH_CLK_ENABLE(); __HAL_RCC_GPIOH_CLK_ENABLE();

3
engineer/Core/Src/main.c
View File

@ -20,6 +20,7 @@
#include "main.h" #include "main.h"
#include "cmsis_os.h" #include "cmsis_os.h"
#include "can.h" #include "can.h"
#include "usart.h"
#include "gpio.h" #include "gpio.h"
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
@ -90,6 +91,8 @@ int main(void)
/* Initialize all configured peripherals */ /* Initialize all configured peripherals */
MX_GPIO_Init(); MX_GPIO_Init();
MX_CAN1_Init(); MX_CAN1_Init();
MX_USART1_UART_Init();
MX_USART2_UART_Init();
/* USER CODE BEGIN 2 */ /* USER CODE BEGIN 2 */
/* USER CODE END 2 */ /* USER CODE END 2 */

30
engineer/Core/Src/stm32f4xx_it.c
View File

@ -56,6 +56,8 @@
/* External variables --------------------------------------------------------*/ /* External variables --------------------------------------------------------*/
extern CAN_HandleTypeDef hcan1; extern CAN_HandleTypeDef hcan1;
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
extern TIM_HandleTypeDef htim14; extern TIM_HandleTypeDef htim14;
/* USER CODE BEGIN EV */ /* USER CODE BEGIN EV */
@ -202,6 +204,34 @@ void CAN1_RX1_IRQHandler(void)
/* USER CODE END CAN1_RX1_IRQn 1 */ /* USER CODE END CAN1_RX1_IRQn 1 */
} }
/**
* @brief This function handles USART1 global interrupt.
*/
void USART1_IRQHandler(void)
{
/* USER CODE BEGIN USART1_IRQn 0 */
/* USER CODE END USART1_IRQn 0 */
HAL_UART_IRQHandler(&huart1);
/* USER CODE BEGIN USART1_IRQn 1 */
/* USER CODE END USART1_IRQn 1 */
}
/**
* @brief This function handles USART2 global interrupt.
*/
void USART2_IRQHandler(void)
{
/* USER CODE BEGIN USART2_IRQn 0 */
/* USER CODE END USART2_IRQn 0 */
HAL_UART_IRQHandler(&huart2);
/* USER CODE BEGIN USART2_IRQn 1 */
/* USER CODE END USART2_IRQn 1 */
}
/** /**
* @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt. * @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt.
*/ */

196
engineer/Core/Src/usart.c Normal file
View File

@ -0,0 +1,196 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file usart.c
* @brief This file provides code for the configuration
* of the USART instances.
******************************************************************************
* @attention
*
* Copyright (c) 2025 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "usart.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;
/* USART1 init function */
void MX_USART1_UART_Init(void)
{
/* USER CODE BEGIN USART1_Init 0 */
/* USER CODE END USART1_Init 0 */
/* USER CODE BEGIN USART1_Init 1 */
/* USER CODE END USART1_Init 1 */
huart1.Instance = USART1;
huart1.Init.BaudRate = 115200;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX_RX;
huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart1.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART1_Init 2 */
/* USER CODE END USART1_Init 2 */
}
/* USART2 init function */
void MX_USART2_UART_Init(void)
{
/* USER CODE BEGIN USART2_Init 0 */
/* USER CODE END USART2_Init 0 */
/* USER CODE BEGIN USART2_Init 1 */
/* USER CODE END USART2_Init 1 */
huart2.Instance = USART2;
huart2.Init.BaudRate = 115200;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART2_Init 2 */
/* USER CODE END USART2_Init 2 */
}
void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(uartHandle->Instance==USART1)
{
/* USER CODE BEGIN USART1_MspInit 0 */
/* USER CODE END USART1_MspInit 0 */
/* USART1 clock enable */
__HAL_RCC_USART1_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/**USART1 GPIO Configuration
PB7 ------> USART1_RX
PB6 ------> USART1_TX
*/
GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_6;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USART1 interrupt Init */
HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspInit 1 */
/* USER CODE END USART1_MspInit 1 */
}
else if(uartHandle->Instance==USART2)
{
/* USER CODE BEGIN USART2_MspInit 0 */
/* USER CODE END USART2_MspInit 0 */
/* USART2 clock enable */
__HAL_RCC_USART2_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE();
/**USART2 GPIO Configuration
PD6 ------> USART2_RX
PD5 ------> USART2_TX
*/
GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_5;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF7_USART2;
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* USART2 interrupt Init */
HAL_NVIC_SetPriority(USART2_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART2_IRQn);
/* USER CODE BEGIN USART2_MspInit 1 */
/* USER CODE END USART2_MspInit 1 */
}
}
void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
{
if(uartHandle->Instance==USART1)
{
/* USER CODE BEGIN USART1_MspDeInit 0 */
/* USER CODE END USART1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_USART1_CLK_DISABLE();
/**USART1 GPIO Configuration
PB7 ------> USART1_RX
PB6 ------> USART1_TX
*/
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_7|GPIO_PIN_6);
/* USART1 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspDeInit 1 */
/* USER CODE END USART1_MspDeInit 1 */
}
else if(uartHandle->Instance==USART2)
{
/* USER CODE BEGIN USART2_MspDeInit 0 */
/* USER CODE END USART2_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_USART2_CLK_DISABLE();
/**USART2 GPIO Configuration
PD6 ------> USART2_RX
PD5 ------> USART2_TX
*/
HAL_GPIO_DeInit(GPIOD, GPIO_PIN_6|GPIO_PIN_5);
/* USART2 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART2_IRQn);
/* USER CODE BEGIN USART2_MspDeInit 1 */
/* USER CODE END USART2_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

36
engineer/DM.ioc
View File

@ -22,18 +22,24 @@ Mcu.IP1=FREERTOS
Mcu.IP2=NVIC Mcu.IP2=NVIC
Mcu.IP3=RCC Mcu.IP3=RCC
Mcu.IP4=SYS Mcu.IP4=SYS
Mcu.IPNb=5 Mcu.IP5=USART1
Mcu.IP6=USART2
Mcu.IPNb=7
Mcu.Name=STM32F407I(E-G)Hx Mcu.Name=STM32F407I(E-G)Hx
Mcu.Package=UFBGA176 Mcu.Package=UFBGA176
Mcu.Pin0=PA14 Mcu.Pin0=PA14
Mcu.Pin1=PA13 Mcu.Pin1=PA13
Mcu.Pin2=PD0 Mcu.Pin10=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin3=PD1 Mcu.Pin11=VP_SYS_VS_tim14
Mcu.Pin4=PH0-OSC_IN Mcu.Pin2=PB7
Mcu.Pin5=PH1-OSC_OUT Mcu.Pin3=PB6
Mcu.Pin6=VP_FREERTOS_VS_CMSIS_V2 Mcu.Pin4=PD6
Mcu.Pin7=VP_SYS_VS_tim14 Mcu.Pin5=PD0
Mcu.PinsNb=8 Mcu.Pin6=PD5
Mcu.Pin7=PD1
Mcu.Pin8=PH0-OSC_IN
Mcu.Pin9=PH1-OSC_OUT
Mcu.PinsNb=12
Mcu.ThirdPartyNb=0 Mcu.ThirdPartyNb=0
Mcu.UserConstants= Mcu.UserConstants=
Mcu.UserName=STM32F407IGHx Mcu.UserName=STM32F407IGHx
@ -58,17 +64,27 @@ NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false
NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn
NVIC.TimeBaseIP=TIM14 NVIC.TimeBaseIP=TIM14
NVIC.USART1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.USART2_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
PA13.Mode=Serial_Wire PA13.Mode=Serial_Wire
PA13.Signal=SYS_JTMS-SWDIO PA13.Signal=SYS_JTMS-SWDIO
PA14.Mode=Serial_Wire PA14.Mode=Serial_Wire
PA14.Signal=SYS_JTCK-SWCLK PA14.Signal=SYS_JTCK-SWCLK
PB6.Mode=Asynchronous
PB6.Signal=USART1_TX
PB7.Mode=Asynchronous
PB7.Signal=USART1_RX
PD0.Locked=true PD0.Locked=true
PD0.Mode=CAN_Activate PD0.Mode=CAN_Activate
PD0.Signal=CAN1_RX PD0.Signal=CAN1_RX
PD1.Locked=true PD1.Locked=true
PD1.Mode=CAN_Activate PD1.Mode=CAN_Activate
PD1.Signal=CAN1_TX PD1.Signal=CAN1_TX
PD5.Mode=Asynchronous
PD5.Signal=USART2_TX
PD6.Mode=Asynchronous
PD6.Signal=USART2_RX
PH0-OSC_IN.Mode=HSE-External-Oscillator PH0-OSC_IN.Mode=HSE-External-Oscillator
PH0-OSC_IN.Signal=RCC_OSC_IN PH0-OSC_IN.Signal=RCC_OSC_IN
PH1-OSC_OUT.Mode=HSE-External-Oscillator PH1-OSC_OUT.Mode=HSE-External-Oscillator
@ -140,6 +156,10 @@ RCC.VCOI2SOutputFreq_Value=384000000
RCC.VCOInputFreq_Value=2000000 RCC.VCOInputFreq_Value=2000000
RCC.VCOOutputFreq_Value=336000000 RCC.VCOOutputFreq_Value=336000000
RCC.VcooutputI2S=192000000 RCC.VcooutputI2S=192000000
USART1.IPParameters=VirtualMode
USART1.VirtualMode=VM_ASYNC
USART2.IPParameters=VirtualMode
USART2.VirtualMode=VM_ASYNC
VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2 VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
VP_SYS_VS_tim14.Mode=TIM14 VP_SYS_VS_tim14.Mode=TIM14

159
engineer/User/bsp/uart.c Normal file
View File

@ -0,0 +1,159 @@
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#include "bsp/uart.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
/* Private function -------------------------------------------------------- */
static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
if (huart->Instance == USART1)
return BSP_UART_1;
else if (huart->Instance == USART2)
return BSP_UART_2;
else
return BSP_UART_ERR;
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]();
}
}
}
void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]();
}
}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]();
}
}
}
void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]();
}
}
}
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) {
UART_Callback[bsp_uart][BSP_UART_ERROR_CB]();
}
}
}
void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]();
}
}
}
void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]();
}
}
}
void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]();
}
}
}
/* Exported functions ------------------------------------------------------- */
void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) {
if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) {
__HAL_UART_CLEAR_IDLEFLAG(huart);
if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) {
UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]();
}
}
}
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
switch (uart) {
case BSP_UART_1:
return &huart1;
case BSP_UART_2:
return &huart2;
default:
return NULL;
}
}
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR;
UART_Callback[uart][type] = callback;
return BSP_OK;
}
int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
if (uart >= BSP_UART_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
if (dma) {
return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size);
} else {
return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size);
}
}
int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
if (uart >= BSP_UART_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
if (dma) {
return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size);
} else {
return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size);
}
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

69
engineer/User/bsp/uart.h Normal file
View File

@ -0,0 +1,69 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#include <stdint.h>
#include <stdbool.h>
#include "bsp/bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */
/* UART实体枚举，与设备对应 */
typedef enum {
BSP_UART_1,
BSP_UART_2,
BSP_UART_NUM,
BSP_UART_ERR,
} BSP_UART_t;
/* UART支持的中断回调函数类型，具体参考HAL中定义 */
typedef enum {
BSP_UART_TX_HALF_CPLT_CB,
BSP_UART_TX_CPLT_CB,
BSP_UART_RX_HALF_CPLT_CB,
BSP_UART_RX_CPLT_CB,
BSP_UART_ERROR_CB,
BSP_UART_ABORT_CPLT_CB,
BSP_UART_ABORT_TX_CPLT_CB,
BSP_UART_ABORT_RX_CPLT_CB,
BSP_UART_IDLE_LINE_CB,
BSP_UART_CB_NUM,
} BSP_UART_Callback_t;
/* Exported functions prototypes -------------------------------------------- */
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart);
void BSP_UART_IRQHandler(UART_HandleTypeDef *huart);
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void));
int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

387
engineer/User/component/cmd.c Normal file
View File

@ -0,0 +1,387 @@
/*
控制命令
*/
#include "cmd.h"
#include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/**
* @brief 行为转换为对应按键
*
* @param cmd 主结构体
* @param behavior 行为
* @return uint16_t 行为对应的按键
*/
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
CMD_Behavior_t behavior) {
return cmd->param->map.key_map[behavior].key;
}
static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
CMD_Behavior_t behavior) {
return cmd->param->map.key_map[behavior].active;
}
/**
* @brief 检查按键是否按下
*
* @param rc 遥控器数据
* @param key 按键名称
* @param stateful 是否为状态切换按键
* @return true 按下
* @return false 未按下
*/
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
/* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK) {
return rc->mouse.l_click;
}
if (key == CMD_R_CLICK) {
return rc->mouse.r_click;
}
return rc->key & (1u << key);
}
static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
CMD_Behavior_t behavior) {
CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
bool now_key_pressed, last_key_pressed;
/* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK) {
now_key_pressed = rc->mouse.l_click;
last_key_pressed = cmd->mouse_last.l_click;
} else if (key == CMD_R_CLICK) {
now_key_pressed = rc->mouse.r_click;
last_key_pressed = cmd->mouse_last.r_click;
} else {
now_key_pressed = rc->key & (1u << key);
last_key_pressed = cmd->key_last & (1u << key);
}
switch (active) {
case CMD_ACTIVE_PRESSING:
return now_key_pressed && !last_key_pressed;
case CMD_ACTIVE_RASING:
return !now_key_pressed && last_key_pressed;
case CMD_ACTIVE_PRESSED:
return now_key_pressed;
}
}
/**
* @brief 解析pc行为逻辑
*
* @param rc 遥控器数据
* @param cmd 主结构体
* @param dt_sec 两次解析的间隔
*/
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
/* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
cmd->gimbal.delta_eulr.yaw =
(float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
cmd->gimbal.delta_eulr.pit =
(float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
cmd->shoot.reverse_trig = false;
/* 按键行为映射相关逻辑 */
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
/* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true;
} else {
/* 切换至准备模式，停止射击 */
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = false;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
/* 每按一次依次切换开火下一个模式 */
cmd->shoot.fire_mode++;
cmd->shoot.fire_mode %= FIRE_MODE_NUM;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
/* 切换到小陀螺模式 */
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
/* 每按一次开、关弹舱盖 */
cmd->shoot.cover_open = !cmd->shoot.cover_open;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
if (cmd->ai_status == AI_STATUS_HITSWITCH) {
/* 停止ai的打符模式，停用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
} else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
/* 自瞄模式中切换失败提醒 */
} else {
/* ai切换至打符模式，启用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
cmd->ai_status = AI_STATUS_HITSWITCH;
cmd->host_overwrite = true;
}
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
if (cmd->ai_status == AI_STATUS_AUTOAIM) {
/* 停止ai的自瞄模式，停用host控制 */
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
} else {
/* ai切换至自瞄模式，启用host控制 */
cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
}
} else {
cmd->host_overwrite = false;
// TODO: 修复逻辑
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
/* 按下拨弹反转 */
cmd->shoot.reverse_trig = true;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
}
/* 保存当前按下的键位状态 */
cmd->key_last = rc->key;
memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last));
}
/**
* @brief 解析rc行为逻辑
*
* @param rc 遥控器数据
* @param cmd 主结构体
* @param dt_sec 两次解析的间隔
*/
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
switch (rc->sw_l) {
/* 左拨杆相应行为选择和解析 */
case CMD_SW_UP:
cmd->chassis.mode = CHASSIS_MODE_BREAK;
break;
case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break;
case CMD_SW_DOWN:
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
break;
case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX;
break;
}
switch (rc->sw_r) {
/* 右拨杆相应行为选择和解析*/
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_SAFE;
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;
case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true;
break;
/*
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_SAFE;
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;
case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true;
break;
*/
case CMD_SW_ERR:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX;
}
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
}
/**
* @brief rc失控时机器人恢复放松模式
*
* @param cmd 主结构体
*/
static void CMD_RcLostLogic(CMD_t *cmd) {
/* 机器人底盘、云台、射击运行模式恢复至放松模式 */
cmd->chassis.mode = CHASSIS_MODE_RELAX;
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX;
}
/**
* @brief 初始化命令解析
*
* @param cmd 主结构体
* @param param 参数
* @return int8_t 0对应没有错误
*/
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
/* 指针检测 */
if (cmd == NULL) return -1;
if (param == NULL) return -1;
/* 设置机器人的命令参数，初始化控制方式为rc控制 */
cmd->pc_ctrl = false;
cmd->param = param;
return 0;
}
/**
* @brief 检查是否启用上位机控制指令覆盖
*
* @param cmd 主结构体
* @return true 启用
* @return false 不启用
*/
inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
/**
* @brief 解析命令
*
* @param rc 遥控器数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
/* 指针检测 */
if (rc == NULL) return -1;
if (cmd == NULL) return -1;
/* 在pc控制和rc控制间切换 */
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q))
cmd->pc_ctrl = true;
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
cmd->pc_ctrl = false;
/*c当rc丢控时，恢复机器人至默认状态 */
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd);
} else {
if (cmd->pc_ctrl) {
CMD_PcLogic(rc, cmd, dt_sec);
} else {
CMD_RcLogic(rc, cmd, dt_sec);
}
}
return 0;
}
/**
* @brief 解析上位机命令
*
* @param host host数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
(void)dt_sec; /* 未使用dt_sec，消除警告 */
/* 指针检测 */
if (host == NULL) return -1;
if (cmd == NULL) return -1;
/* 云台欧拉角设置为host相应的变化的欧拉角 */
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
/* host射击命令，设置不同的射击频率和弹丸初速度 */
if (host->fire) {
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true;
} else {
cmd->shoot.mode = SHOOT_MODE_SAFE;
}
return 0;
}
/**
* @brief 添加向Referee发送的命令
*
* @param ref 命令队列
* @param cmd 要添加的命令
* @return int8_t 0对应没有错误
*/
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
/* 指针检测 */
if (ref == NULL) return -1;
/* 越界检测 */
if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
/* 添加机器人当前行为状态到画图的命令队列中 */
ref->cmd[ref->counter] = cmd;
ref->counter++;
return 0;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

342
engineer/User/component/cmd.h Normal file
View File

@ -0,0 +1,342 @@
/*
控制命令
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
#include "component/ahrs.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define CMD_REFEREE_MAX_NUM (3) /* Lines 16 omitted */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 机器人型号 */
typedef enum {
ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */
ROBOT_MODEL_HERO, /* 英雄机器人 */
ROBOT_MODEL_ENGINEER, /* 工程机器人 */
ROBOT_MODEL_DRONE, /* 空中机器人 */
ROBOT_MODEL_SENTRY, /* 哨兵机器人 */
ROBOT_MODEL_NUM, /* 型号数量 */
} CMD_RobotModel_t;
/* 底盘运行模式 */
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */
CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
} CMD_ChassisMode_t;
typedef enum{
SET_ARM_MODE_RELAX, /*放松模式*/
SET_ARM_MODE_FOLLOW, /*控制器跟踪模式*/
SET_ARM_MODE_POSITION,/*关节位置指令模式*/
SET_ARM_MODE_STORAGE,
SET_ARM_MODE_TAKE
}CMD_ArmMode_t;
/* 云台运行模式 */
typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
} CMD_GimbalMode_t;
/* 射击运行模式 */
typedef enum {
SHOOT_MODE_RELAX, /* 放松模式，电机不输出 */
SHOOT_MODE_SAFE, /* 保险模式，电机闭环控制保持静止 */
SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */
} CMD_ShootMode_t;
typedef enum {
FIRE_MODE_SINGLE, /* 单发开火模式 */
FIRE_MODE_BURST, /* N连发开火模式 */
FIRE_MODE_CONT, /* 持续开火模式 */
FIRE_MODE_NUM,
} CMD_FireMode_t;
/* 小陀螺转动模式 */
typedef enum {
ROTOR_MODE_CW, /* 顺时针转动 */
ROTOR_MODE_CCW, /* 逆时针转动 */
ROTOR_MODE_RAND, /* 随机转动 */
} CMD_RotorMode_t;
/* 机械臂控制命令*/
typedef struct {
CMD_ArmMode_t mode;
uint8_t cmd_prepare;
struct{
float ref_angle[6];
float set_angle[6];
}data;
}CMD_Arm_t;
/* 底盘控制命令 */
typedef struct {
CMD_ChassisMode_t mode; /* 底盘运行模式 */
CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
MoveVector_t ctrl_vec; /* 底盘控制向量 */
} CMD_ChassisCmd_t;
/* 云台控制命令 */
typedef struct {
CMD_GimbalMode_t mode; /* 云台运行模式 */
AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */
} CMD_GimbalCmd_t;
/* 射击控制命令 */
typedef struct {
CMD_ShootMode_t mode; /* 射击运行模式 */
CMD_FireMode_t fire_mode; /* 开火模式 */
bool fire; /*开火*/
bool cover_open; /* 弹舱盖开关 */
bool reverse_trig; /* 拨弹电机状态 */
} CMD_ShootCmd_t;
/* 拨杆位置 */
typedef enum {
CMD_SW_ERR = 0,
CMD_SW_UP = 1,
CMD_SW_MID = 3,
CMD_SW_DOWN = 2,
} CMD_SwitchPos_t;
/* 键盘按键值 */
typedef enum {
CMD_KEY_W = 0,
CMD_KEY_S,
CMD_KEY_A,
CMD_KEY_D,
CMD_KEY_SHIFT,
CMD_KEY_CTRL,
CMD_KEY_Q,
CMD_KEY_E,
CMD_KEY_R,
CMD_KEY_F,
CMD_KEY_G,
CMD_KEY_Z,
CMD_KEY_X,
CMD_KEY_C,
CMD_KEY_V,
CMD_KEY_B,
CMD_L_CLICK,
CMD_R_CLICK,
CMD_KEY_NUM,
} CMD_KeyValue_t;
/* 行为值序列 */
typedef enum {
CMD_BEHAVIOR_FORE = 0, /* 向前 */
CMD_BEHAVIOR_BACK, /* 向后 */
CMD_BEHAVIOR_LEFT, /* 向左 */
CMD_BEHAVIOR_RIGHT, /* 向右 */
CMD_BEHAVIOR_ACCELERATE, /* 加速 */
CMD_BEHAVIOR_DECELEBRATE, /* 减速 */
CMD_BEHAVIOR_FIRE, /* 开火 */
CMD_BEHAVIOR_FIRE_MODE, /* 切换开火模式 */
CMD_BEHAVIOR_BUFF, /* 打符模式 */
CMD_BEHAVIOR_AUTOAIM, /* 自瞄模式 */
CMD_BEHAVIOR_OPENCOVER, /* 弹舱盖开关 */
CMD_BEHAVIOR_ROTOR, /* 小陀螺模式 */
CMD_BEHAVIOR_REVTRIG, /* 反转拨弹 */
CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */
CMD_BEHAVIOR_NUM,
} CMD_Behavior_t;
typedef enum {
CMD_ACTIVE_PRESSING, /* 按下时触发 */
CMD_ACTIVE_RASING, /* 抬起时触发 */
CMD_ACTIVE_PRESSED, /* 按住时触发 */
} CMD_ActiveType_t;
typedef struct {
CMD_ActiveType_t active;
CMD_KeyValue_t key;
} CMD_KeyMapItem_t;
/* 行为映射的对应按键数组 */
typedef struct {
CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM];
} CMD_KeyMap_Params_t;
/* 位移灵敏度参数 */
typedef struct {
float move_sense; /* 移动灵敏度 */
float move_fast_sense; /* 加速灵敏度 */
float move_slow_sense; /* 减速灵敏度 */
} CMD_Move_Params_t;
typedef struct {
uint16_t width;
uint16_t height;
} CMD_Screen_t;
/* 命令参数 */
typedef struct {
float sens_mouse; /* 鼠标灵敏度 */
float sens_rc; /* 遥控器摇杆灵敏度 */
CMD_KeyMap_Params_t map; /* 按键映射行为命令 */
CMD_Move_Params_t move; /* 位移灵敏度参数 */
CMD_Screen_t screen; /* 屏幕分辨率参数 */
} CMD_Params_t;
/* AI行为状态 */
typedef enum {
AI_STATUS_STOP, /* 停止状态 */
AI_STATUS_AUTOAIM, /* 自瞄状态 */
AI_STATUS_HITSWITCH, /* 打符状态 */
AI_STATUS_AUTOMATIC /* 自动状态 */
} CMD_AI_Status_t;
/* UI所用行为状态 */
typedef enum {
CMD_UI_NOTHING, /* 当前无状态 */
CMD_UI_AUTO_AIM_START, /* 自瞄状态开启 */
CMD_UI_AUTO_AIM_STOP, /* 自瞄状态关闭 */
CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */
CMD_UI_HIT_SWITCH_STOP /* 打符状态关闭 */
} CMD_UI_t;
/*裁判系统发送的命令*/
typedef struct {
CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */
uint8_t counter; /* 命令计数 */
} CMD_RefereeCmd_t;
typedef struct {
bool pc_ctrl; /* 是否使用键鼠控制 */
bool host_overwrite; /* 是否Host控制 */
uint16_t key_last; /* 上次按键键值 */
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse_last; /* 鼠标值 */
CMD_AI_Status_t ai_status; /* AI状态 */
const CMD_Params_t *param; /* 命令参数 */
CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
CMD_GimbalCmd_t gimbal; /* 云台控制命令 */
CMD_ShootCmd_t shoot; /* 射击控制命令 */
CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */
} CMD_t;
typedef struct {
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
float ch_res; /* 第五通道值 */
CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse; /* 鼠标值 */
uint16_t key; /* 按键值 */
uint16_t res; /* 保留，未启用 */
} CMD_RC_t;
typedef struct {
AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */
struct {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
bool fire; /* 开火状态 */
} CMD_Host_t;
/**
* @brief 解析行为命令
*
* @param rc 遥控器数据
* @param cmd 主结构体
*/
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param);
/**
* @brief 检查是否启用上位机控制指令覆盖
*
* @param cmd 主结构体
* @return true 启用
* @return false 不启用
*/
bool CMD_CheckHostOverwrite(CMD_t *cmd);
/**
* @brief 解析命令
*
* @param rc 遥控器数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec);
/**
* @brief 解析上位机命令
*
* @param host host数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec);
/**
* @brief 添加向Referee发送的命令
*
* @param ref 命令队列
* @param cmd 要添加的命令
* @return int8_t 0对应没有错误
*/
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

62
engineer/User/component/crc16.c Normal file
View File

@ -0,0 +1,62 @@
#include "crc16.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint16_t crc16_tab[256] = {
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) {
return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff];
}
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
while (len--) crc = CRC16_Byte(crc, *buf++);
return crc;
}
bool CRC16_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT);
return expected ==
((const uint16_t *)((const uint8_t *)buf +
(len % 2)))[len / sizeof(uint16_t) - 1];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
engineer/User/component/crc16.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC16_INIT 0XFFFF
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
bool CRC16_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

52
engineer/User/component/crc8.c Normal file
View File

@ -0,0 +1,52 @@
#include "crc8.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint8_t crc8_tab[256] = {
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1,
0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e,
0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39,
0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45,
0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e,
0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31,
0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0,
0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea,
0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b,
0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54,
0xd7, 0x89, 0x6b, 0x35,
};
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) {
/* loop over the buffer data */
while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff];
return crc;
}
bool CRC8_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT);
return expected == buf[len - sizeof(uint8_t)];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
engineer/User/component/crc8.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC8_INIT 0xFF
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc);
bool CRC8_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

107
engineer/User/component/limiter.c Normal file
View File

@ -0,0 +1,107 @@
/*
限制器
*/
#include "limiter.h"
#include <math.h>
#include <stddef.h>
#define POWER_BUFF_THRESHOLD 20
#define CHASSIS_POWER_CHECK_FREQ 10
#define CHASSIS_POWER_FACTOR_PASS 0.9f
#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len) {
/* power_limit小于0时不进行限制 */
if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
float sum_motor_out = 0.0f;
for (uint32_t i = 0; i < len; i++) {
/* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
sum_motor_out +=
fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
}
/* 保持每个电机输出值缩小时比例不变 */
if (sum_motor_out > power_limit) {
for (uint32_t i = 0; i < len; i++) {
motor_out[i] *= power_limit / sum_motor_out;
}
}
return 0;
}
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer) {
float target_power = 0.0f;
/* 计算下一个检测周期的剩余缓冲能量 */
float heat_buff = power_buffer - (float)(power_in - power_limit) /
(float)CHASSIS_POWER_CHECK_FREQ;
if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
} else {
target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
}
return target_power;
}
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer) {
float target_power = 0.0f;
/* 根据剩余缓冲能量计算输出功率 */
target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
if (target_power < 0.0f) target_power = 0.0f;
return target_power;
}
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big) {
float heat_percent = heat / heat_limit;
float stable_freq = cooling_rate / heat_increase;
if (is_big)
return stable_freq;
else
return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
}

63
engineer/User/component/limiter.h Normal file
View File

@ -0,0 +1,63 @@
/*
限制器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len);
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer);
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer);
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big);

24
engineer/User/component/matrix.cpp Normal file
View File

@ -0,0 +1,24 @@
/**
******************************************************************************
* @file matrix.cpp/h
* @brief Matrix/vector calculation. 矩阵/向量运算
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#include "matrix.hpp"
// hat of vector
Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) {
float hat[9] = {0, -vec[2][0], vec[1][0], vec[2][0], 0,
-vec[0][0], -vec[1][0], vec[0][0], 0};
return Matrixf<3, 3>(hat);
}
// cross product
Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) {
return vector3f::hat(vec1) * vec2;
}

259
engineer/User/component/matrix.hpp Normal file
View File

@ -0,0 +1,259 @@
/**
******************************************************************************
* @file matrix.cpp/h
* @brief Matrix/vector calculation. 矩阵/向量运算
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#ifndef MATRIX_H
#define MATRIX_H
#include "arm_math.h"
// Matrix class
template <int _rows, int _cols>
class Matrixf {
public:
// Constructor without input data
Matrixf(void) : rows_(_rows), cols_(_cols) {
arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_);
}
// Constructor with input data
Matrixf(float data[_rows * _cols]) : Matrixf() {
memcpy(this->data_, data, _rows * _cols * sizeof(float));
}
// Copy constructor
Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() {
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
}
// Destructor
~Matrixf(void) {}
// Row size
int rows(void) { return _rows; }
// Column size
int cols(void) { return _cols; }
// Element
float* operator[](const int& row) { return &this->data_[row * _cols]; }
// Operators
Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) {
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
return *this;
}
Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) {
arm_status s;
s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) {
arm_status s;
s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator*=(const float& val) {
arm_status s;
s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator/=(const float& val) {
arm_status s;
s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator*(const float& val) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_);
return res;
}
friend Matrixf<_rows, _cols> operator*(const float& val,
const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator/(const float& val) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_);
return res;
}
// Matrix multiplication
template <int cols>
friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1,
const Matrixf<_cols, cols>& mat2) {
arm_status s;
Matrixf<_rows, cols> res;
s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_);
return res;
}
// Submatrix
template <int rows, int cols>
Matrixf<rows, cols> block(const int& start_row, const int& start_col) {
Matrixf<rows, cols> res;
for (int row = start_row; row < start_row + rows; row++) {
memcpy((float*)res[0] + (row - start_row) * cols,
(float*)this->data_ + row * _cols + start_col,
cols * sizeof(float));
}
return res;
}
// Specific row
Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); }
// Specific column
Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); }
// Transpose
Matrixf<_cols, _rows> trans(void) {
Matrixf<_cols, _rows> res;
arm_mat_trans_f32(&arm_mat_, &res.arm_mat_);
return res;
}
// Trace
float trace(void) {
float res = 0;
for (int i = 0; i < fmin(_rows, _cols); i++) {
res += (*this)[i][i];
}
return res;
}
// Norm
float norm(void) { return sqrtf((this->trans() * *this)[0][0]); }
public:
// arm matrix instance
arm_matrix_instance_f32 arm_mat_;
protected:
// size
int rows_, cols_;
// data
float data_[_rows * _cols];
};
// Matrix funtions
namespace matrixf {
// Special Matrices
// Zero matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> zeros(void) {
float data[_rows * _cols] = {0};
return Matrixf<_rows, _cols>(data);
}
// Ones matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> ones(void) {
float data[_rows * _cols] = {0};
for (int i = 0; i < _rows * _cols; i++) {
data[i] = 1;
}
return Matrixf<_rows, _cols>(data);
}
// Identity matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> eye(void) {
float data[_rows * _cols] = {0};
for (int i = 0; i < fmin(_rows, _cols); i++) {
data[i * _cols + i] = 1;
}
return Matrixf<_rows, _cols>(data);
}
// Diagonal matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) {
Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>();
for (int i = 0; i < fmin(_rows, _cols); i++) {
res[i][i] = vec[i][0];
}
return res;
}
// Inverse
template <int _dim>
Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) {
arm_status s;
// extended matrix [A|I]
Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>();
for (int i = 0; i < _dim; i++) {
memcpy(ext_mat[i], mat[i], _dim * sizeof(float));
ext_mat[i][_dim + i] = 1;
}
// elimination
for (int i = 0; i < _dim; i++) {
// find maximum absolute value in the first column in lower right block
float abs_max = fabs(ext_mat[i][i]);
int abs_max_row = i;
for (int row = i; row < _dim; row++) {
if (abs_max < fabs(ext_mat[row][i])) {
abs_max = fabs(ext_mat[row][i]);
abs_max_row = row;
}
}
if (abs_max < 1e-12f) { // singular
return matrixf::zeros<_dim, _dim>();
s = ARM_MATH_SINGULAR;
}
if (abs_max_row != i) { // row exchange
float tmp;
Matrixf<1, 2 * _dim> row_i = ext_mat.row(i);
Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row);
memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float));
memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float));
}
float k = 1.f / ext_mat[i][i];
for (int col = i; col < 2 * _dim; col++) {
ext_mat[i][col] *= k;
}
for (int row = 0; row < _dim; row++) {
if (row == i) {
continue;
}
k = ext_mat[row][i];
for (int j = i; j < 2 * _dim; j++) {
ext_mat[row][j] -= k * ext_mat[i][j];
}
}
}
// inv = ext_mat(:,n+1:2n)
s = ARM_MATH_SUCCESS;
Matrixf<_dim, _dim> res;
for (int i = 0; i < _dim; i++) {
memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float));
}
return res;
}
} // namespace matrixf
namespace vector3f {
// hat of vector
Matrixf<3, 3> hat(Matrixf<3, 1> vec);
// cross product
Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2);
} // namespace vector3f
#endif // MATRIX_H

340
engineer/User/component/robotics.cpp Normal file
View File

@ -0,0 +1,340 @@
/**
******************************************************************************
* @file robotics.cpp/h
* @brief Robotic toolbox on STM32. STM32机器人学库
* @author Spoon Guan
* @ref [1] SJTU ME385-2, Robotics, Y.Ding
* [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and
* Control, Springer, 2010.
* [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction
* to Robotic Manipulation, CRC Press, 1994.
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#include "robotics.hpp"
Matrixf<3, 1> robotics::r2rpy(Matrixf<3, 3> R) {
float rpy[3] = {
atan2f(R[1][0], R[0][0]), // yaw
atan2f(-R[2][0], sqrtf(R[2][1] * R[2][1] + R[2][2] * R[2][2])), // pitch
atan2f(R[2][1], R[2][2]) // roll
};
return Matrixf<3, 1>(rpy);
}
Matrixf<3, 3> robotics::rpy2r(Matrixf<3, 1> rpy) {
float c[3] = {cosf(rpy[0][0]), cosf(rpy[1][0]), cosf(rpy[2][0])};
float s[3] = {sinf(rpy[0][0]), sinf(rpy[1][0]), sinf(rpy[2][0])};
float R[9] = {
c[0] * c[1], // R11
c[0] * s[1] * s[2] - s[0] * c[2], // R12
c[0] * s[1] * c[2] + s[0] * s[2], // R13
s[0] * c[1], // R21
s[0] * s[1] * s[2] + c[0] * c[2], // R22
s[0] * s[1] * c[2] - c[0] * s[2], // R23
-s[1], // R31
c[1] * s[2], // R32
c[1] * c[2] // R33
};
return Matrixf<3, 3>(R);
}
Matrixf<4, 1> robotics::r2angvec(Matrixf<3, 3> R) {
float theta = acosf(math::limit(0.5f * (R.trace() - 1), -1, 1));
if (theta == 0 || theta == PI) {
float angvec[4] = {
(1 + R[0][0] - R[1][1] - R[2][2]) / 4.0f, // rx=(1+R11-R22-R33)/4
(1 - R[0][0] + R[1][1] - R[2][2]) / 4.0f, // ry=(1-R11+R22-R33)/4
(1 - R[0][0] - R[1][1] + R[2][2]) / 4.0f, // rz=(1-R11-R22+R33)/4
theta // theta
};
return Matrixf<4, 1>(angvec);
}
float angvec[4] = {
(R[2][1] - R[1][2]) / (2.0f * sinf(theta)), // rx=(R32-R23)/2sinθ
(R[0][2] - R[2][0]) / (2.0f * sinf(theta)), // ry=(R13-R31)/2sinθ
(R[1][0] - R[0][1]) / (2.0f * sinf(theta)), // rz=(R21-R12)/2sinθ
theta // theta
};
return Matrixf<4, 1>(angvec);
}
Matrixf<3, 3> robotics::angvec2r(Matrixf<4, 1> angvec) {
float theta = angvec[3][0];
Matrixf<3, 1> r = angvec.block<3, 1>(0, 0);
Matrixf<3, 3> R;
// Rodrigues formula: R=I+ω^sinθ+ω^^2(1-cosθ)
R = matrixf::eye<3, 3>() + vector3f::hat(r) * sinf(theta) +
vector3f::hat(r) * vector3f::hat(r) * (1 - cosf(theta));
return R;
}
Matrixf<4, 1> robotics::r2quat(Matrixf<3, 3> R) {
float q[4] = {
0.5f * sqrtf(math::limit(R.trace(), -1, 1) + 1), // q0=cos(θ/2)
math::sign(R[2][1] - R[1][2]) * 0.5f *
sqrtf(math::limit(R[0][0] - R[1][1] - R[2][2], -1, 1) +
1), // q1=rx*sin(θ/2)
math::sign(R[0][2] - R[2][0]) * 0.5f *
sqrtf(math::limit(-R[0][0] + R[1][1] - R[2][2], -1, 1) +
1), // q2=ry*sin(θ/2)
math::sign(R[1][0] - R[0][1]) * 0.5f *
sqrtf(math::limit(-R[0][0] - R[1][1] + R[2][2], -1, 1) +
1), // q3=rz*sin(θ/2)
};
return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm());
}
Matrixf<3, 3> robotics::quat2r(Matrixf<4, 1> q) {
float R[9] = {
1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0]), // R11
2.0f * (q[1][0] * q[2][0] - q[0][0] * q[3][0]), // R12
2.0f * (q[1][0] * q[3][0] + q[0][0] * q[2][0]), // R13
2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), // R21
1 - 2.0f * (q[1][0] * q[1][0] + q[3][0] * q[3][0]), // R22
2.0f * (q[2][0] * q[3][0] - q[0][0] * q[1][0]), // R23
2.0f * (q[1][0] * q[3][0] - q[0][0] * q[2][0]), // R31
2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), // R32
1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0]) // R33
};
return Matrixf<3, 3>(R);
}
Matrixf<3, 1> robotics::quat2rpy(Matrixf<4, 1> q) {
float rpy[3] = {
atan2f(2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]),
1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0])), // yaw
asinf(2.0f * (q[0][0] * q[2][0] - q[1][0] * q[3][0])), // pitch
atan2f(2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]),
1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0])) // rol
};
return Matrixf<3, 1>(rpy);
}
Matrixf<4, 1> robotics::rpy2quat(Matrixf<3, 1> rpy) {
float c[3] = {cosf(0.5f * rpy[0][0]), cosf(0.5f * rpy[1][0]),
cosf(0.5f * rpy[2][0])}; // cos(*/2)
float s[3] = {sinf(0.5f * rpy[0][0]), sinf(0.5f * rpy[1][0]),
sinf(0.5f * rpy[2][0])}; // sin(*/2)
float q[4] = {
c[0] * c[1] * c[2] + s[0] * s[1] * s[2], // q0=cos(θ/2)
c[0] * c[1] * s[2] - s[0] * s[1] * c[2], // q1=rx*sin(θ/2)
c[0] * s[1] * c[2] + s[0] * c[1] * s[2], // q2=ry*sin(θ/2)
s[0] * c[1] * c[2] - c[0] * s[1] * s[2] // q3=rz*sin(θ/2)
};
return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm());
}
Matrixf<4, 1> robotics::quat2angvec(Matrixf<4, 1> q) {
float cosq0;
float theta = 2.0f * acosf(math::limit(q[0][0], -1, 1));
if (theta == 0 || theta == PI) {
float angvec[4] = {0, 0, 0, theta}; // θ=0||PI, return[0;θ]
return Matrixf<4, 1>(angvec);
}
Matrixf<3, 1> vec = q.block<3, 1>(1, 0);
float angvec[4] = {
vec[0][0] / vec.norm(), // rx
vec[1][0] / vec.norm(), // ry
vec[2][0] / vec.norm(), // rz
theta // theta
};
return Matrixf<4, 1>(angvec);
}
Matrixf<4, 1> robotics::angvec2quat(Matrixf<4, 1> angvec) {
float c = cosf(0.5f * angvec[3][0]), s = sinf(0.5f * angvec[3][0]);
float q[4] = {
c, // q0=cos(θ/2)
s * angvec[0][0], // q1=rx*sin(θ/2)
s * angvec[1][0], // q2=ry*sin(θ/2)
s * angvec[2][0] // q3=rz*sin(θ/2)
};
return Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm();
}
Matrixf<3, 3> robotics::t2r(Matrixf<4, 4> T) {
return T.block<3, 3>(0, 0); // R=T(1:3,1:3)
}
Matrixf<4, 4> robotics::r2t(Matrixf<3, 3> R) {
// T=[R,0;0,1]
float T[16] = {R[0][0], R[0][1], R[0][2], 0, R[1][0], R[1][1], R[1][2], 0,
R[2][0], R[2][1], R[2][2], 0, 0, 0, 0, 1};
return Matrixf<4, 4>(T);
}
Matrixf<3, 1> robotics::t2p(Matrixf<4, 4> T) {
return T.block<3, 1>(0, 3); // p=T(1:3,4)
}
Matrixf<4, 4> robotics::p2t(Matrixf<3, 1> p) {
// T=[I,P;0,1]
float T[16] = {1, 0, 0, p[0][0], 0, 1, 0, p[1][0],
0, 0, 1, p[2][0], 0, 0, 0, 1};
return Matrixf<4, 4>(T);
}
Matrixf<4, 4> robotics::rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p) {
// T=[R,P;0,1]
float T[16] = {R[0][0], R[0][1], R[0][2], p[0][0], R[1][0], R[1][1],
R[1][2], p[1][0], R[2][0], R[2][1], R[2][2], p[2][0],
0, 0, 0, 1};
return Matrixf<4, 4>(T);
}
Matrixf<4, 4> robotics::invT(Matrixf<4, 4> T) {
Matrixf<3, 3> RT = t2r(T).trans();
Matrixf<3, 1> p_ = -1.0f * RT * t2p(T);
float invT[16] = {RT[0][0], RT[0][1], RT[0][2], p_[0][0], RT[1][0], RT[1][1],
RT[1][2], p_[1][0], RT[2][0], RT[2][1], RT[2][2], p_[2][0],
0, 0, 0, 1};
return Matrixf<4, 4>(invT);
}
Matrixf<3, 1> robotics::t2rpy(Matrixf<4, 4> T) {
return r2rpy(t2r(T));
}
Matrixf<4, 4> robotics::rpy2t(Matrixf<3, 1> rpy) {
return r2t(rpy2r(rpy));
}
Matrixf<4, 1> robotics::t2angvec(Matrixf<4, 4> T) {
return r2angvec(t2r(T));
}
Matrixf<4, 4> robotics::angvec2t(Matrixf<4, 1> angvec) {
return r2t(angvec2r(angvec));
}
Matrixf<4, 1> robotics::t2quat(Matrixf<4, 4> T) {
return r2quat(t2r(T));
}
Matrixf<4, 4> robotics::quat2t(Matrixf<4, 1> quat) {
return r2t(quat2r(quat));
}
Matrixf<6, 1> robotics::t2twist(Matrixf<4, 4> T) {
Matrixf<3, 1> p = t2p(T);
Matrixf<4, 1> angvec = t2angvec(T);
Matrixf<3, 1> phi = angvec.block<3, 1>(0, 0) * angvec[3][0];
float twist[6] = {p[0][0], p[1][0], p[2][0], phi[0][0], phi[1][0], phi[2][0]};
return Matrixf<6, 1>(twist);
}
Matrixf<4, 4> robotics::twist2t(Matrixf<6, 1> twist) {
Matrixf<3, 1> p = twist.block<3, 1>(0, 0);
float theta = twist.block<3, 1>(3, 0).norm();
float angvec[4] = {0, 0, 0, theta};
if (theta != 0) {
angvec[0] = twist[3][0] / theta;
angvec[1] = twist[4][0] / theta;
angvec[2] = twist[5][0] / theta;
}
return rp2t(angvec2r(angvec), p);
}
Matrixf<4, 4> robotics::DH_t::fkine() {
float ct = cosf(theta), st = sinf(theta); // cosθ, sinθ
float ca = cosf(alpha), sa = sinf(alpha); // cosα, sinα
// T =
// | cθ -sθcα sθsα acθ |
// | sθ cθcα -cθsα asθ |
// | 0 sα cα d |
// | 0 0 0 1 |
T[0][0] = ct;
T[0][1] = -st * ca;
T[0][2] = st * sa;
T[0][3] = a * ct;
T[1][0] = st;
T[1][1] = ct * ca;
T[1][2] = -ct * sa;
T[1][3] = a * st;
T[2][0] = 0;
T[2][1] = sa;
T[2][2] = ca;
T[2][3] = d;
T[3][0] = 0;
T[3][1] = 0;
T[3][2] = 0;
T[3][3] = 1;
return T;
}
robotics::Link::Link(float theta,
float d,
float a,
float alpha,
robotics::Joint_Type_e type,
float offset,
float qmin,
float qmax,
float m,
Matrixf<3, 1> rc,
Matrixf<3, 3> I) {
dh_.theta = theta;
dh_.d = d;
dh_.alpha = alpha;
dh_.a = a;
type_ = type;
offset_ = offset;
qmin_ = qmin;
qmax_ = qmax;
m_ = m;
rc_ = rc;
I_ = I;
}
robotics::Link::Link(const Link& link) {
dh_.theta = link.dh_.theta;
dh_.d = link.dh_.d;
dh_.alpha = link.dh_.alpha;
dh_.a = link.dh_.a;
type_ = link.type_;
offset_ = link.offset_;
qmin_ = link.qmin_;
qmax_ = link.qmax_;
m_ = link.m_;
rc_ = link.rc_;
I_ = link.I_;
}
robotics::Link& robotics::Link::operator=(Link link) {
dh_.theta = link.dh_.theta;
dh_.d = link.dh_.d;
dh_.alpha = link.dh_.alpha;
dh_.a = link.dh_.a;
type_ = link.type_;
offset_ = link.offset_;
qmin_ = link.qmin_;
qmax_ = link.qmax_;
m_ = link.m_;
rc_ = link.rc_;
I_ = link.I_;
return *this;
}
Matrixf<4, 4> robotics::Link::T(float q) {
if (type_ == R) {
if (qmin_ >= qmax_)
dh_.theta = q + offset_;
else
dh_.theta = math::limit(q + offset_, qmin_, qmax_);
} else {
if (qmin_ >= qmax_)
dh_.d = q + offset_;
else
dh_.d = math::limit(q + offset_, qmin_, qmax_);
}
return dh_.fkine();
}

407
engineer/User/component/robotics.hpp Normal file
View File

@ -0,0 +1,407 @@
/**
******************************************************************************
* @file robotics.cpp/h
* @brief Robotic toolbox on STM32. STM32机器人学库
* @author Spoon Guan
* @ref [1] SJTU ME385-2, Robotics, Y.Ding
* [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and
* Control, Springer, 2010.
* [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction
* to Robotic Manipulation, CRC Press, 1994.
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#ifndef ROBOTICS_H
#define ROBOTICS_H
#include "utils.hpp"
#include "matrix.hpp"
namespace robotics {
// rotation matrix(R) -> RPY([yaw;pitch;roll])
Matrixf<3, 1> r2rpy(Matrixf<3, 3> R);
// RPY([yaw;pitch;roll]) -> rotation matrix(R)
Matrixf<3, 3> rpy2r(Matrixf<3, 1> rpy);
// rotation matrix(R) -> angle vector([r;θ])
Matrixf<4, 1> r2angvec(Matrixf<3, 3> R);
// angle vector([r;θ]) -> rotation matrix(R)
Matrixf<3, 3> angvec2r(Matrixf<4, 1> angvec);
// rotation matrix(R) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
Matrixf<4, 1> r2quat(Matrixf<3, 3> R);
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> rotation matrix(R)
Matrixf<3, 3> quat2r(Matrixf<4, 1> quat);
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> RPY([yaw;pitch;roll])
Matrixf<3, 1> quat2rpy(Matrixf<4, 1> q);
// RPY([yaw;pitch;roll]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
Matrixf<4, 1> rpy2quat(Matrixf<3, 1> rpy);
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> angle vector([r;θ])
Matrixf<4, 1> quat2angvec(Matrixf<4, 1> q);
// angle vector([r;θ]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
Matrixf<4, 1> angvec2quat(Matrixf<4, 1> angvec);
// homogeneous transformation matrix(T) -> rotation matrix(R)
Matrixf<3, 3> t2r(Matrixf<4, 4> T);
// rotation matrix(R) -> homogeneous transformation matrix(T)
Matrixf<4, 4> r2t(Matrixf<3, 3> R);
// homogeneous transformation matrix(T) -> translation vector(p)
Matrixf<3, 1> t2p(Matrixf<4, 4> T);
// translation vector(p) -> homogeneous transformation matrix(T)
Matrixf<4, 4> p2t(Matrixf<3, 1> p);
// rotation matrix(R) & translation vector(p) -> homogeneous transformation
// matrix(T)
Matrixf<4, 4> rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p);
// homogeneous transformation matrix(T) -> RPY([yaw;pitch;roll])
Matrixf<3, 1> t2rpy(Matrixf<4, 4> T);
// inverse of homogeneous transformation matrix(T^-1=[R',-R'P;0,1])
Matrixf<4, 4> invT(Matrixf<4, 4> T);
// RPY([yaw;pitch;roll]) -> homogeneous transformation matrix(T)
Matrixf<4, 4> rpy2t(Matrixf<3, 1> rpy);
// homogeneous transformation matrix(T) -> angle vector([r;θ])
Matrixf<4, 1> t2angvec(Matrixf<4, 4> T);
// angle vector([r;θ]) -> homogeneous transformation matrix(T)
Matrixf<4, 4> angvec2t(Matrixf<4, 1> angvec);
// homogeneous transformation matrix(T) -> quaternion,
// [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
Matrixf<4, 1> t2quat(Matrixf<4, 4> T);
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> homogeneous transformation
// matrix(T)
Matrixf<4, 4> quat2t(Matrixf<4, 1> quat);
// homogeneous transformation matrix(T) -> twist coordinates vector ([p;rθ])
Matrixf<6, 1> t2twist(Matrixf<4, 4> T);
// twist coordinates vector ([p;rθ]) -> homogeneous transformation matrix(T)
Matrixf<4, 4> twist2t(Matrixf<6, 1> twist);
// joint type: R-revolute joint, P-prismatic joint
typedef enum joint_type {
R = 0,
P = 1,
} Joint_Type_e;
// Denavit–Hartenberg(DH) method
struct DH_t {
// forward kinematic
Matrixf<4, 4> fkine();
// DH parameter
float theta;
float d;
float a;
float alpha;
Matrixf<4, 4> T;
};
class Link {
public:
Link(){};
Link(float theta, float d, float a, float alpha, Joint_Type_e type = R,
float offset = 0, float qmin = 0, float qmax = 0, float m = 1,
Matrixf<3, 1> rc = matrixf::zeros<3, 1>(),
Matrixf<3, 3> I = matrixf::zeros<3, 3>());
Link(const Link& link);
Link& operator=(Link link);
float qmin() { return qmin_; }
float qmax() { return qmax_; }
Joint_Type_e type() { return type_; }
float m() { return m_; }
Matrixf<3, 1> rc() { return rc_; }
Matrixf<3, 3> I() { return I_; }
Matrixf<4, 4> T(float q); // forward kinematic
public:
// kinematic parameter
DH_t dh_;
float offset_;
// limit(qmin,qmax), no limit if qmin<=qmax
float qmin_;
float qmax_;
// joint type
Joint_Type_e type_;
// dynamic parameter
float m_; // mass
Matrixf<3, 1> rc_; // centroid(link coordinate)
Matrixf<3, 3> I_; // inertia tensor(3*3)
};
template <uint16_t _n = 1>
class Serial_Link {
public:
Serial_Link(Link links[_n]) {
for (int i = 0; i < _n; i++)
links_[i] = links[i];
gravity_ = matrixf::zeros<3, 1>();
gravity_[2][0] = -9.81f;
}
Serial_Link(Link links[_n], Matrixf<3, 1> gravity) {
for (int i = 0; i < _n; i++)
links_[i] = links[i];
gravity_ = gravity;
}
// forward kinematic: T_n^0
// param[in] q: joint variable vector
// param[out] T_n^0
Matrixf<4, 4> fkine(Matrixf<_n, 1> q) {
T_ = matrixf::eye<4, 4>();
for (int iminus1 = 0; iminus1 < _n; iminus1++)
T_ = T_ * links_[iminus1].T(q[iminus1][0]);
return T_;
}
// forward kinematic: T_k^0
// param[in] q: joint variable vector
// param[in] k: joint number
// param[out] T_k^0
Matrixf<4, 4> fkine(Matrixf<_n, 1> q, uint16_t k) {
if (k > _n)
k = _n;
Matrixf<4, 4> T = matrixf::eye<4, 4>();
for (int iminus1 = 0; iminus1 < k; iminus1++)
T = T * links_[iminus1].T(q[iminus1][0]);
return T;
}
// T_k^k-1: homogeneous transformation matrix of link k
// param[in] q: joint variable vector
// param[in] kminus: joint number k, input k-1
// param[out] T_k^k-1
Matrixf<4, 4> T(Matrixf<_n, 1> q, uint16_t kminus1) {
if (kminus1 >= _n)
kminus1 = _n - 1;
return links_[kminus1].T(q[kminus1][0]);
}
// jacobian matrix, J_i = [J_pi;j_oi]
// param[in] q: joint variable vector
// param[out] jacobian matix J_6*n
Matrixf<6, _n> jacob(Matrixf<_n, 1> q) {
Matrixf<3, 1> p_e = t2p(fkine(q)); // p_e
Matrixf<4, 4> T_iminus1 = matrixf::eye<4, 4>(); // T_i-1^0
Matrixf<3, 1> z_iminus1; // z_i-1^0
Matrixf<3, 1> p_iminus1; // p_i-1^0
Matrixf<3, 1> J_pi;
Matrixf<3, 1> J_oi;
for (int iminus1 = 0; iminus1 < _n; iminus1++) {
// revolute joint: J_pi = z_i-1x(p_e-p_i-1), J_oi = z_i-1
if (links_[iminus1].type() == R) {
z_iminus1 = T_iminus1.block<3, 1>(0, 2);
p_iminus1 = t2p(T_iminus1);
T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]);
J_pi = vector3f::cross(z_iminus1, p_e - p_iminus1);
J_oi = z_iminus1;
}
// prismatic joint: J_pi = z_i-1, J_oi = 0
else {
z_iminus1 = T_iminus1.block<3, 1>(0, 2);
T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]);
J_pi = z_iminus1;
J_oi = matrixf::zeros<3, 1>();
}
J_[0][iminus1] = J_pi[0][0];
J_[1][iminus1] = J_pi[1][0];
J_[2][iminus1] = J_pi[2][0];
J_[3][iminus1] = J_oi[0][0];
J_[4][iminus1] = J_oi[1][0];
J_[5][iminus1] = J_oi[2][0];
}
return J_;
}
// inverse kinematic, numerical solution(Newton method)
// param[in] T: homogeneous transformation matrix of end effector
// param[in] q: initial joint variable vector(q0) for Newton method's
// iteration
// param[in] tol: tolerance of error (norm(error of twist vector))
// param[in] max_iter: maximum iterations, default 30
// param[out] q: joint variable vector
Matrixf<_n, 1> ikine(Matrixf<4, 4> Td,
Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(),
float tol = 1e-4f, uint16_t max_iter = 50) {
Matrixf<4, 4> T;
Matrixf<3, 1> pe, we;
Matrixf<6, 1> err, new_err;
Matrixf<_n, 1> dq;
float step = 1;
for (int i = 0; i < max_iter; i++) {
T = fkine(q);
pe = t2p(Td) - t2p(T);
// angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
for (int i = 0; i < 3; i++) {
err[i][0] = pe[i][0];
err[i + 3][0] = we[i][0];
}
if (err.norm() < tol)
return q;
// adjust iteration step
Matrixf<6, _n> J = jacob(q);
for (int j = 0; j < 5; j++) {
dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step;
if (dq[0][0] == INFINITY) // J'*J singular
{
dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) *
J.trans() * err * step;
// SVD<6, _n> JTJ_svd(J.trans() * J);
// dq = JTJ_svd.solve(err) * step * 5e-2f;
q += dq;
for (int i = 0; i < _n; i++) {
if (links_[i].type() == R)
q[i][0] = math::loopLimit(q[i][0], -PI, PI);
}
break;
}
T = fkine(q + dq);
pe = t2p(Td) - t2p(T);
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
for (int i = 0; i < 3; i++) {
new_err[i][0] = pe[i][0];
new_err[i + 3][0] = we[i][0];
}
if (new_err.norm() < err.norm()) {
q += dq;
for (int i = 0; i < _n; i++) {
if (links_[i].type() == robotics::Joint_Type_e::R) {
q[i][0] = math::loopLimit(q[i][0], -PI, PI);
}
}
break;
} else {
step /= 2.0f;
}
}
if (step < 1e-3f)
return q;
}
return q;
}
// (Reserved function) inverse kinematic, analytic solution(geometric method)
Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T);
// inverse dynamic, Newton-Euler method
// param[in] q: joint variable vector
// param[in] qv: dq/dt
// param[in] qa: d^2q/dt^2
// param[in] he: load on end effector [f;μ], default 0
Matrixf<_n, 1> rne(Matrixf<_n, 1> q,
Matrixf<_n, 1> qv = matrixf::zeros<_n, 1>(),
Matrixf<_n, 1> qa = matrixf::zeros<_n, 1>(),
Matrixf<6, 1> he = matrixf::zeros<6, 1>()) {
// forward propagation
// record each links' motion state in matrices
// [ωi] angular velocity
Matrixf<3, _n + 1> w = matrixf::zeros<3, _n + 1>();
// [βi] angular acceleration
Matrixf<3, _n + 1> b = matrixf::zeros<3, _n + 1>();
// [pi] position of joint
Matrixf<3, _n + 1> p = matrixf::zeros<3, _n + 1>();
// [vi] velocity of joint
Matrixf<3, _n + 1> v = matrixf::zeros<3, _n + 1>();
// [ai] acceleration of joint
Matrixf<3, _n + 1> a = matrixf::zeros<3, _n + 1>();
// [aci] acceleration of mass center
Matrixf<3, _n + 1> ac = matrixf::zeros<3, _n + 1>();
// temperary vectors
Matrixf<3, 1> w_i, b_i, p_i, v_i, ai, ac_i;
// i & i-1 coordinate convert to 0 coordinate
Matrixf<4, 4> T_0i = matrixf::eye<4, 4>();
Matrixf<4, 4> T_0iminus1 = matrixf::eye<4, 4>();
Matrixf<3, 3> R_0i = matrixf::eye<3, 3>();
Matrixf<3, 3> R_0iminus1 = matrixf::eye<3, 3>();
// unit vector of z-axis
Matrixf<3, 1> ez = matrixf::zeros<3, 1>();
ez[2][0] = 1;
for (int i = 1; i <= _n; i++) {
T_0i = T_0i * T(q, i - 1); // T_i^0
R_0i = t2r(T_0i); // R_i^0
R_0iminus1 = t2r(T_0iminus1); // R_i-1^0
// ω_i = ω_i-1+qv_i*R_i-1^0*ez
w_i = w.col(i - 1) + qv[i - 1][0] * R_0iminus1 * ez;
// β_i = β_i-1+ω_i-1x(qv_i*R_i-1^0*ez)+qa_i*R_i-1^0*ez
b_i = b.col(i - 1) +
vector3f::cross(w.col(i - 1), qv[i - 1][0] * R_0iminus1 * ez) +
qa[i - 1][0] * R_0iminus1 * ez;
p_i = t2p(T_0i); // p_i = T_i^0(1:3,4)
// v_i = v_i-1+ω_ix(p_i-p_i-1)
v_i = v.col(i - 1) + vector3f::cross(w_i, p_i - p.col(i - 1));
// a_i = a_i-1+β_ix(p_i-p_i-1)+ω_ix(ω_ix(p_i-p_i-1))
ai = a.col(i - 1) + vector3f::cross(b_i, p_i - p.col(i - 1)) +
vector3f::cross(w_i, vector3f::cross(w_i, p_i - p.col(i - 1)));
// ac_i = a_i+β_ix(R_0^i*rc_i^i)+ω_ix(ω_ix(R_0^i*rc_i^i))
ac_i =
ai + vector3f::cross(b_i, R_0i * links_[i - 1].rc()) +
vector3f::cross(w_i, vector3f::cross(w_i, R_0i * links_[i - 1].rc()));
for (int row = 0; row < 3; row++) {
w[row][i] = w_i[row][0];
b[row][i] = b_i[row][0];
p[row][i] = p_i[row][0];
v[row][i] = v_i[row][0];
a[row][i] = ai[row][0];
ac[row][i] = ac_i[row][0];
}
T_0iminus1 = T_0i; // T_i-1^0
}
// backward propagation
// record each links' force
Matrixf<3, _n + 1> f = matrixf::zeros<3, _n + 1>(); // joint force
Matrixf<3, _n + 1> mu = matrixf::zeros<3, _n + 1>(); // joint moment
// temperary vector
Matrixf<3, 1> f_iminus1, mu_iminus1;
// {T,R',P}_i^i-1
Matrixf<4, 4> T_iminus1i;
Matrixf<3, 3> RT_iminus1i;
Matrixf<3, 1> P_iminus1i;
// I_i-1(in 0 coordinate)
Matrixf<3, 3> I_i;
// joint torque
Matrixf<_n, 1> torq;
// load on end effector
for (int row = 0; row < 3; row++) {
f[row][_n] = he.block<3, 1>(0, 0)[row][0];
mu[row][_n] = he.block<3, 1>(3, 0)[row][0];
}
for (int i = _n; i > 0; i--) {
T_iminus1i = T(q, i - 1); // T_i^i-1
P_iminus1i = t2p(T_iminus1i); // P_i^i-1
RT_iminus1i = t2r(T_iminus1i).trans(); // R_i^i-1'
R_0iminus1 = R_0i * RT_iminus1i; // R_i-1^0
// I_i^0 = R_i^0*I_i^i*(R_i^0)'
I_i = R_0i * links_[i - 1].I() * R_0i.trans();
// f_i-1 = f_i+m_i*ac_i-m_i*g
f_iminus1 = f.col(i) + links_[i - 1].m() * ac.col(i) -
links_[i - 1].m() * gravity_;
// μ_i-1 = μ_i+f_ixrc_i-f_i-1xrc_i-1->ci+I_i*b_i+ω_ix(I_i*ω_i)
mu_iminus1 = mu.col(i) +
vector3f::cross(f.col(i), R_0i * links_[i - 1].rc()) -
vector3f::cross(f_iminus1, R_0i * (RT_iminus1i * P_iminus1i +
links_[i - 1].rc())) +
I_i * b.col(i) + vector3f::cross(w.col(i), I_i * w.col(i));
// τ_i = μ_i-1'*(R_i-1^0*ez)
torq[i - 1][0] = (mu_iminus1.trans() * R_0iminus1 * ez)[0][0];
for (int row = 0; row < 3; row++) {
f[row][i - 1] = f_iminus1[row][0];
mu[row][i - 1] = mu_iminus1[row][0];
}
R_0i = R_0iminus1;
}
return torq;
}
private:
Link links_[_n];
Matrixf<3, 1> gravity_;
Matrixf<4, 4> T_;
Matrixf<6, _n> J_;
};
}; // namespace robotics
#endif // ROBOTICS_H

301
engineer/User/component/ui.c Normal file
View File

@ -0,0 +1,301 @@
/*
UI相关命令
*/
#include "component/ui.h"
#include <stdio.h>
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end) {
if (grapic_line == NULL) return -1;
snprintf((char *)grapic_line->name, 2, "%s", name);
grapic_line->layer = layer;
grapic_line->type_op = type_op;
grapic_line->type_ele = 0;
grapic_line->color = color;
grapic_line->width = width;
grapic_line->x_start = x_start;
grapic_line->y_start = y_start;
grapic_line->x_end = x_end;
grapic_line->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end) {
if (grapic_rectangle == NULL) return -1;
snprintf((char *)grapic_rectangle->name, 2, "%s", name);
grapic_rectangle->type_op = type_op;
grapic_rectangle->type_ele = 1;
grapic_rectangle->layer = layer;
grapic_rectangle->color = color;
grapic_rectangle->width = width;
grapic_rectangle->x_start = x_start;
grapic_rectangle->y_start = y_start;
grapic_rectangle->x_end = x_end;
grapic_rectangle->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius) {
if (grapic_cycle == NULL) return -1;
snprintf((char *)grapic_cycle->name, 2, "%s", name);
grapic_cycle->type_op = type_op;
grapic_cycle->layer = layer;
grapic_cycle->type_ele = 2;
grapic_cycle->color = color;
grapic_cycle->width = width;
grapic_cycle->x_start = x_center;
grapic_cycle->y_start = y_center;
grapic_cycle->radius = radius;
return 0;
}
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis) {
if (grapic_oval == NULL) return -1;
snprintf((char *)grapic_oval->name, 2, "%s", name);
grapic_oval->type_op = type_op;
grapic_oval->type_ele = 3;
grapic_oval->layer = layer;
grapic_oval->color = color;
grapic_oval->width = width;
grapic_oval->x_start = x_center;
grapic_oval->y_start = y_center;
grapic_oval->x_end = x_semiaxis;
grapic_oval->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis) {
if (grapic_arc == NULL) return -1;
snprintf((char *)grapic_arc->name, 2, "%s", name);
grapic_arc->type_op = type_op;
grapic_arc->type_ele = 4;
grapic_arc->layer = layer;
grapic_arc->color = color;
grapic_arc->angle_start = angle_start;
grapic_arc->angle_end = angle_end;
grapic_arc->width = width;
grapic_arc->x_start = x_center;
grapic_arc->y_start = y_center;
grapic_arc->x_end = x_semiaxis;
grapic_arc->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low) {
if (grapic_floating == NULL) return -1;
snprintf((char *)grapic_floating->name, 2, "%s", name);
grapic_floating->type_op = type_op;
grapic_floating->type_ele = 5;
grapic_floating->layer = layer;
grapic_floating->color = color;
grapic_floating->angle_start = font_size;
grapic_floating->angle_end = digits;
grapic_floating->width = width;
grapic_floating->x_start = x_start;
grapic_floating->y_start = y_start;
grapic_floating->radius = float_high;
grapic_floating->x_end = float_middle;
grapic_floating->y_end = float_low;
return 0;
}
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low) {
if (grapic_integer == NULL) return -1;
snprintf((char *)grapic_integer->name, 2, "%s", name);
grapic_integer->type_op = type_op;
grapic_integer->type_ele = 6;
grapic_integer->layer = layer;
grapic_integer->color = color;
grapic_integer->angle_start = font_size;
grapic_integer->width = width;
grapic_integer->x_start = x_start;
grapic_integer->y_start = y_start;
grapic_integer->radius = int32_t_high;
grapic_integer->x_end = int32_t_middle;
grapic_integer->y_end = int32_t_low;
return 0;
}
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character) {
if (grapic_character == NULL) return -1;
snprintf((char *)grapic_character->grapic.name, 2, "%s", name);
grapic_character->grapic.type_op = type_op;
grapic_character->grapic.type_ele = 7;
grapic_character->grapic.layer = layer;
grapic_character->grapic.color = color;
grapic_character->grapic.angle_start = font_size;
grapic_character->grapic.angle_end = length;
grapic_character->grapic.width = width;
grapic_character->grapic.x_start = x_start;
grapic_character->grapic.y_start = y_start;
snprintf((char *)grapic_character->character, 29, "%s", character);
return 0;
}
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer) {
if (del == NULL) return -1;
del->del_operation = opt;
del->layer = layer;
return 0;
}

284
engineer/User/component/ui.h Normal file
View File

@ -0,0 +1,284 @@
/*
UI相关命令
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define UI_DEL_OPERATION_NOTHING (0)
#define UI_DEL_OPERATION_DEL (1)
#define UI_DEL_OPERATION_DEL_ALL (2)
#define UI_GRAPIC_OPERATION_NOTHING (0)
#define UI_GRAPIC_OPERATION_ADD (1)
#define UI_GRAPIC_OPERATION_REWRITE (2)
#define UI_GRAPIC_OPERATION_DEL (3)
#define UI_GRAPIC_LAYER_CONST (0)
#define UI_GRAPIC_LAYER_AUTOAIM (1)
#define UI_GRAPIC_LAYER_CHASSIS (2)
#define UI_GRAPIC_LAYER_CAP (3)
#define UI_GRAPIC_LAYER_GIMBAL (4)
#define UI_GRAPIC_LAYER_SHOOT (5)
#define UI_GRAPIC_LAYER_CMD (6)
#define UI_DEFAULT_WIDTH (0x01)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define UI_CHAR_DEFAULT_WIDTH (0x02)
typedef enum {
RED_BLUE,
YELLOW,
GREEN,
ORANGE,
PURPLISH_RED,
PINK,
CYAN,
BLACK,
WHITE
} UI_Color_t;
typedef struct __packed {
uint8_t op;
uint8_t num_layer;
} UI_InterStudent_UIDel_t;
typedef struct __packed {
uint8_t name[3];
uint8_t type_op : 3;
uint8_t type_ele : 3;
uint8_t layer : 4;
uint8_t color : 4;
uint16_t angle_start : 9;
uint16_t angle_end : 9;
uint16_t width : 10;
uint16_t x_start : 11;
uint16_t y_start : 11;
uint16_t radius : 10;
uint16_t x_end : 11;
uint16_t y_end : 11;
} UI_Ele_t;
typedef struct __packed {
UI_Ele_t grapic;
} UI_Drawgrapic_1_t;
typedef struct __packed {
UI_Ele_t grapic[2];
} UI_Drawgrapic_2_t;
typedef struct __packed {
UI_Ele_t grapic[5];
} UI_Drawgrapic_5_t;
typedef struct __packed {
UI_Ele_t grapic[7];
} UI_Drawgrapic_7_t;
typedef struct __packed {
UI_Ele_t grapic;
uint8_t character[30];
} UI_Drawcharacter_t;
typedef struct __packed {
uint8_t del_operation;
uint8_t layer;
} UI_Del_t;
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end);
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end);
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius);
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis);
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis);
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low);
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low);
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character);
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer);
#ifdef __cplusplus
}
#endif

56
engineer/User/component/utils.cpp Normal file
View File

@ -0,0 +1,56 @@
/**
******************************************************************************
* @file utils.cpp/h
* @brief General math utils. 常用数学工具函数
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#include "utils.hpp"
float math::limit(float val, const float& min, const float& max) {
if (min > max)
return val;
else if (val < min)
val = min;
else if (val > max)
val = max;
return val;
}
float math::limitMin(float val, const float& min) {
if (val < min)
val = min;
return val;
}
float math::limitMax(float val, const float& max) {
if (val > max)
val = max;
return val;
}
float math::loopLimit(float val, const float& min, const float& max) {
if (min >= max)
return val;
if (val > max) {
while (val > max)
val -= (max - min);
} else if (val < min) {
while (val < min)
val += (max - min);
}
return val;
}
float math::sign(const float& val) {
if (val > 0)
return 1;
else if (val < 0)
return -1;
return 0;
}

27
engineer/User/component/utils.hpp Normal file
View File

@ -0,0 +1,27 @@
/**
******************************************************************************
* @file utils.cpp/h
* @brief General math utils. 常用数学工具函数
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#ifndef UTILS_H
#define UTILS_H
#include <stdint.h>
namespace math {
float limit(float val, const float& min, const float& max);
float limitMin(float val, const float& min);
float limitMax(float val, const float& max);
float loopLimit(float val, const float& min, const float& max);
float sign(const float& val);
} // namespace math
#endif // UTILS_H

788
engineer/User/device/referee.c Normal file
View File

@ -0,0 +1,788 @@
/*
裁判系统抽象。
*/
/* Includes ----------------------------------------------------------------- */
#include "referee.h"
#include <string.h>
#include "bsp/uart.h"
#include "component/crc16.h"
#include "component/crc8.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
#define REF_HEADER_SOF (0xA5)
#define REF_LEN_RX_BUFF (0xFF)
#define REF_UI_FAST_REFRESH_FREQ (50)
#define REF_UI_SLOW_REFRESH_FREQ (0.2f)
#define REF_UI_BOX_UP_OFFSET (4)
#define REF_UI_BOX_BOT_OFFSET (-14)
#define REF_UI_RIGHT_START_POS (0.85f)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;
static uint8_t rxbuf[REF_LEN_RX_BUFF];
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void Referee_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_RAW_REDY);
}
static void Referee_IdleLineCallback(void) {
HAL_UART_AbortReceive_IT(BSP_UART_GetHandle(BSP_UART_REF));
}
static void Referee_AbortRxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_RAW_REDY);
}
static void RefereeFastRefreshTimerCallback(void *arg) {
(void)arg;
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_FAST_REFRESH_UI);
}
static void RefereeSlowRefreshTimerCallback(void *arg) {
(void)arg;
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_SLOW_REFRESH_UI);
}
/* Exported functions ------------------------------------------------------- */
int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui,
const CMD_Screen_t *screen) {
if (ref == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
ui->screen = screen;
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_RX_CPLT_CB,
Referee_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_ABORT_RX_CPLT_CB,
Referee_AbortRxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_IDLE_LINE_CB,
Referee_IdleLineCallback);
uint32_t fast_period_ms = (uint32_t)(1000.0f / REF_UI_FAST_REFRESH_FREQ);
uint32_t slow_period_ms = (uint32_t)(1000.0f / REF_UI_SLOW_REFRESH_FREQ);
ref->ui_fast_timer_id =
osTimerNew(RefereeFastRefreshTimerCallback, osTimerPeriodic, NULL, NULL);
ref->ui_slow_timer_id =
osTimerNew(RefereeSlowRefreshTimerCallback, osTimerPeriodic, NULL, NULL);
osTimerStart(ref->ui_fast_timer_id, fast_period_ms);
osTimerStart(ref->ui_slow_timer_id, slow_period_ms);
__HAL_UART_ENABLE_IT(BSP_UART_GetHandle(BSP_UART_REF), UART_IT_IDLE);
inited = true;
return 0;
}
int8_t Referee_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_REF));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REF));
return 0;
}
int8_t Referee_StartReceiving(Referee_t *ref) {
(void)ref;
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_REF), rxbuf,
REF_LEN_RX_BUFF) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool Referee_CheckTXReady() {
return BSP_UART_GetHandle(BSP_UART_REF)->gState == HAL_UART_STATE_READY;
}
void Referee_HandleOffline(Referee_t *referee) {
referee->ref_status = REF_STATUS_OFFLINE;
}
int8_t Referee_Parse(Referee_t *ref) {
REF_SWITCH_STATUS(*ref, REF_STATUS_RUNNING);
uint32_t data_length =
REF_LEN_RX_BUFF -
__HAL_DMA_GET_COUNTER(BSP_UART_GetHandle(BSP_UART_REF)->hdmarx);
uint8_t index = 0;
uint8_t packet_shift;
uint8_t packet_length;
while (index < data_length && rxbuf[index] == REF_HEADER_SOF) {
packet_shift = index;
Referee_Header_t *header = (Referee_Header_t *)(rxbuf + index);
index += sizeof(*header);
if (index - packet_shift >= data_length) goto error;
if (!CRC8_Verify((uint8_t *)header, sizeof(*header))) goto error;
if (header->sof != REF_HEADER_SOF) goto error;
Referee_CMDID_t *cmd_id = (Referee_CMDID_t *)(rxbuf + index);
index += sizeof(*cmd_id);
if (index - packet_shift >= data_length) goto error;
void *target = (rxbuf + index);
void *origin;
size_t size;
switch (*cmd_id) {
case REF_CMD_ID_GAME_STATUS:
origin = &(ref->game_status);
size = sizeof(ref->game_status);
break;
case REF_CMD_ID_GAME_RESULT:
origin = &(ref->game_result);
size = sizeof(ref->game_result);
break;
case REF_CMD_ID_GAME_ROBOT_HP:
origin = &(ref->game_robot_hp);
size = sizeof(ref->game_robot_hp);
break;
case REF_CMD_ID_DART_STATUS:
origin = &(ref->dart_status);
size = sizeof(ref->dart_status);
break;
case REF_CMD_ID_ICRA_ZONE_STATUS:
origin = &(ref->icra_zone);
size = sizeof(ref->icra_zone);
break;
case REF_CMD_ID_FIELD_EVENTS:
origin = &(ref->field_event);
size = sizeof(ref->field_event);
break;
case REF_CMD_ID_SUPPLY_ACTION:
origin = &(ref->supply_action);
size = sizeof(ref->supply_action);
break;
case REF_CMD_ID_REQUEST_SUPPLY:
origin = &(ref->request_supply);
size = sizeof(ref->request_supply);
case REF_CMD_ID_WARNING:
origin = &(ref->warning);
size = sizeof(ref->warning);
break;
case REF_CMD_ID_DART_COUNTDOWN:
origin = &(ref->dart_countdown);
size = sizeof(ref->dart_countdown);
break;
case REF_CMD_ID_ROBOT_STATUS:
origin = &(ref->robot_status);
size = sizeof(ref->robot_status);
break;
case REF_CMD_ID_POWER_HEAT_DATA:
origin = &(ref->power_heat);
size = sizeof(ref->power_heat);
break;
case REF_CMD_ID_ROBOT_POS:
origin = &(ref->robot_pos);
size = sizeof(ref->robot_pos);
break;
case REF_CMD_ID_ROBOT_BUFF:
origin = &(ref->robot_buff);
size = sizeof(ref->robot_buff);
break;
case REF_CMD_ID_DRONE_ENERGY:
origin = &(ref->drone_energy);
size = sizeof(ref->drone_energy);
break;
case REF_CMD_ID_ROBOT_DMG:
origin = &(ref->robot_danage);
size = sizeof(ref->robot_danage);
break;
case REF_CMD_ID_SHOOT_DATA:
origin = &(ref->shoot_data);
size = sizeof(ref->shoot_data);
break;
case REF_CMD_ID_BULLET_REMAINING:
origin = &(ref->bullet_remain);
size = sizeof(ref->bullet_remain);
break;
case REF_CMD_ID_RFID:
origin = &(ref->rfid);
size = sizeof(ref->rfid);
break;
case REF_CMD_ID_DART_CLIENT:
origin = &(ref->dart_client);
size = sizeof(ref->dart_client);
break;
case REF_CMD_ID_INTER_STUDENT_CUSTOM:
origin = &(ref->custom);
size = sizeof(ref->custom);
case REF_CMD_ID_CLIENT_MAP:
origin = &(ref->client_map);
size = sizeof(ref->client_map);
case REF_CMD_ID_KEYBOARD_MOUSE:
origin = &(ref->keyboard_mouse);
size = sizeof(ref->keyboard_mouse);
default:
return DEVICE_ERR;
}
packet_length = sizeof(Referee_Header_t) + sizeof(Referee_CMDID_t) + size +
sizeof(Referee_Tail_t);
index += size;
if (index - packet_shift >= data_length) goto error;
index += sizeof(Referee_Tail_t);
if (index - packet_shift != packet_length) goto error;
if (CRC16_Verify((uint8_t *)(rxbuf + packet_shift), packet_length))
memcpy(origin, target, size);
else
goto error;
}
return DEVICE_OK;
error:
drop_message++;
return DEVICE_ERR;
}
int8_t Referee_StartSend(uint8_t *data, uint32_t len) {
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_REF), data,
(size_t)len) == HAL_OK) {
return DEVICE_OK;
} else
return DEVICE_ERR;
}
int8_t Referee_MoveData(void *data, void *tmp, uint32_t len) {
if (len <= 0 || data == NULL || tmp == NULL) return DEVICE_ERR;
memcpy(tmp, (const void *)data, (size_t)len);
memset(data, 0, (size_t)len);
return DEVICE_OK;
}
int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
Referee_StudentCMDID_t cmd_id, uint8_t sender_id) {
header->data_cmd_id = cmd_id;
if (sender_id <= REF_BOT_RED_RADER) switch (sender_id) {
case REF_BOT_RED_HERO:
header->sender_ID = REF_BOT_RED_HERO;
header->receiver_ID = REF_CL_RED_HERO;
break;
case REF_BOT_RED_ENGINEER:
header->sender_ID = REF_BOT_RED_ENGINEER;
header->receiver_ID = REF_CL_RED_ENGINEER;
break;
case REF_BOT_RED_INFANTRY_1:
header->sender_ID = REF_BOT_RED_INFANTRY_1;
header->receiver_ID = REF_CL_RED_INFANTRY_1;
break;
case REF_BOT_RED_INFANTRY_2:
header->sender_ID = REF_BOT_RED_INFANTRY_2;
header->receiver_ID = REF_CL_RED_INFANTRY_2;
break;
case REF_BOT_RED_INFANTRY_3:
header->sender_ID = REF_BOT_RED_INFANTRY_3;
header->receiver_ID = REF_CL_RED_INFANTRY_3;
break;
case REF_BOT_RED_DRONE:
header->sender_ID = REF_BOT_RED_DRONE;
header->receiver_ID = REF_CL_RED_DRONE;
break;
case REF_BOT_RED_SENTRY:
header->sender_ID = REF_BOT_RED_SENTRY;
break;
case REF_BOT_RED_RADER:
header->sender_ID = REF_BOT_RED_RADER;
break;
default:
return -1;
}
else
switch (sender_id) {
case REF_BOT_BLU_HERO:
header->sender_ID = REF_BOT_BLU_HERO;
header->receiver_ID = REF_CL_BLU_HERO;
break;
case REF_BOT_BLU_ENGINEER:
header->sender_ID = REF_BOT_BLU_ENGINEER;
header->receiver_ID = REF_CL_BLU_ENGINEER;
break;
case REF_BOT_BLU_INFANTRY_1:
header->sender_ID = REF_BOT_BLU_INFANTRY_1;
header->receiver_ID = REF_CL_BLU_INFANTRY_1;
break;
case REF_BOT_BLU_INFANTRY_2:
header->sender_ID = REF_BOT_BLU_INFANTRY_2;
header->receiver_ID = REF_CL_BLU_INFANTRY_2;
break;
case REF_BOT_BLU_INFANTRY_3:
header->sender_ID = REF_BOT_BLU_INFANTRY_3;
header->receiver_ID = REF_CL_BLU_INFANTRY_3;
break;
case REF_BOT_BLU_DRONE:
header->sender_ID = REF_BOT_BLU_DRONE;
header->receiver_ID = REF_CL_BLU_DRONE;
break;
case REF_BOT_BLU_SENTRY:
header->sender_ID = REF_BOT_BLU_SENTRY;
break;
case REF_BOT_BLU_RADER:
header->sender_ID = REF_BOT_BLU_RADER;
break;
default:
return -1;
}
return 0;
}
int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref) {
if (!Referee_CheckTXReady()) return 0;
if (ui->character_counter == 0 && ui->grapic_counter == 0 &&
ui->del_counter == 0)
return 0;
static uint8_t send_data[sizeof(Referee_UI_Drawgrapic_7_t)] = {0};
uint16_t size;
if (ui->del_counter != 0) {
if (ui->del_counter < 0 || ui->del_counter > REF_UI_MAX_STRING_NUM)
return DEVICE_ERR;
Referee_UI_Del_t *address = (Referee_UI_Del_t *)send_data;
address->header.sof = REF_HEADER_SOF;
address->header.data_length =
sizeof(UI_Del_t) + sizeof(Referee_Interactive_Header_t);
address->header.crc8 =
CRC8_Calc((const uint8_t *)&(address->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address->IA_header), REF_STDNT_CMD_ID_UI_DEL,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->del[--ui->del_counter]), &(address->data),
sizeof(UI_Del_t));
address->crc16 =
CRC16_Calc((const uint8_t *)address,
sizeof(Referee_UI_Del_t) - sizeof(uint16_t), CRC16_INIT);
size = sizeof(Referee_UI_Del_t);
Referee_StartSend(send_data, size);
return DEVICE_OK;
} else if (ui->grapic_counter != 0) {
switch (ui->grapic_counter) {
case 1:
size = sizeof(Referee_UI_Drawgrapic_1_t);
Referee_UI_Drawgrapic_1_t *address_1 =
(Referee_UI_Drawgrapic_1_t *)send_data;
address_1->header.sof = REF_HEADER_SOF;
address_1->header.data_length =
sizeof(UI_Drawgrapic_1_t) + sizeof(Referee_Interactive_Header_t);
address_1->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_1->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_1->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_1->IA_header), REF_STDNT_CMD_ID_UI_DRAW1,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_1->data.grapic),
sizeof(UI_Drawgrapic_1_t));
address_1->crc16 = CRC16_Calc(
(const uint8_t *)address_1,
sizeof(Referee_UI_Drawgrapic_1_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 2:
size = sizeof(Referee_UI_Drawgrapic_2_t);
Referee_UI_Drawgrapic_2_t *address_2 =
(Referee_UI_Drawgrapic_2_t *)send_data;
address_2->header.sof = REF_HEADER_SOF;
address_2->header.data_length =
sizeof(UI_Drawgrapic_2_t) + sizeof(Referee_Interactive_Header_t);
address_2->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_2->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_2->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_2->IA_header), REF_STDNT_CMD_ID_UI_DRAW2,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_2->data.grapic),
sizeof(UI_Drawgrapic_2_t));
address_2->crc16 = CRC16_Calc(
(const uint8_t *)address_2,
sizeof(Referee_UI_Drawgrapic_2_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 3:
case 4:
case 5:
size = sizeof(Referee_UI_Drawgrapic_5_t);
Referee_UI_Drawgrapic_5_t *address_5 =
(Referee_UI_Drawgrapic_5_t *)send_data;
address_5->header.sof = REF_HEADER_SOF;
address_5->header.data_length =
sizeof(UI_Drawgrapic_5_t) + sizeof(Referee_Interactive_Header_t);
address_5->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_5->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_5->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_5->IA_header), REF_STDNT_CMD_ID_UI_DRAW5,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_5->data.grapic),
sizeof(UI_Drawgrapic_5_t));
address_5->crc16 = CRC16_Calc(
(const uint8_t *)address_5,
sizeof(Referee_UI_Drawgrapic_5_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 6:
case 7:
size = sizeof(Referee_UI_Drawgrapic_7_t);
Referee_UI_Drawgrapic_7_t *address_7 =
(Referee_UI_Drawgrapic_7_t *)send_data;
address_7->header.sof = REF_HEADER_SOF;
address_7->header.data_length =
sizeof(UI_Drawgrapic_7_t) + sizeof(Referee_Interactive_Header_t);
address_7->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_7->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_7->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_7->IA_header), REF_STDNT_CMD_ID_UI_DRAW7,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_7->data.grapic),
sizeof(UI_Drawgrapic_7_t));
address_7->crc16 = CRC16_Calc(
(const uint8_t *)address_7,
sizeof(Referee_UI_Drawgrapic_7_t) - sizeof(uint16_t), CRC16_INIT);
break;
default:
return DEVICE_ERR;
}
if (Referee_StartSend(send_data, size) == HAL_OK) {
ui->grapic_counter = 0;
return DEVICE_OK;
}
} else if (ui->character_counter != 0) {
if (ui->character_counter < 0 ||
ui->character_counter > REF_UI_MAX_STRING_NUM)
return DEVICE_ERR;
Referee_UI_Drawcharacter_t *address =
(Referee_UI_Drawcharacter_t *)send_data;
address->header.sof = REF_HEADER_SOF;
address->header.data_length =
sizeof(UI_Drawcharacter_t) + sizeof(Referee_Interactive_Header_t);
address->header.crc8 =
CRC8_Calc((const uint8_t *)&(address->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address->IA_header), REF_STDNT_CMD_ID_UI_STR,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->character_data[--ui->character_counter]),
&(address->data.grapic), sizeof(UI_Drawcharacter_t));
address->crc16 = CRC16_Calc(
(const uint8_t *)address,
sizeof(Referee_UI_Drawcharacter_t) - sizeof(uint16_t), CRC16_INIT);
size = sizeof(Referee_UI_Drawcharacter_t);
Referee_StartSend(send_data, size);
return DEVICE_OK;
}
return DEVICE_ERR_NULL;
}
UI_Ele_t *Referee_GetGrapicAdd(Referee_UI_t *ref_ui) {
if (ref_ui->grapic_counter >= REF_UI_MAX_GRAPIC_NUM ||
ref_ui->grapic_counter < 0)
return NULL;
else
return &(ref_ui->grapic[ref_ui->grapic_counter++]);
}
UI_Drawcharacter_t *Referee_GetCharacterAdd(Referee_UI_t *ref_ui) {
if (ref_ui->character_counter >= REF_UI_MAX_STRING_NUM ||
ref_ui->character_counter < 0)
return NULL;
else
return &(ref_ui->character_data[ref_ui->character_counter++]);
}
UI_Del_t *Referee_GetDelAdd(Referee_UI_t *ref_ui) {
if (ref_ui->del_counter >= REF_UI_MAX_DEL_NUM || ref_ui->del_counter < 0)
return NULL;
else
return &(ref_ui->del[ref_ui->del_counter++]);
}
uint8_t Referee_PraseCmd(Referee_UI_t *ref_ui, CMD_UI_t cmd) {
switch (cmd) {
/* Demo */
case CMD_UI_NOTHING:
/* 字符 */
UI_DrawCharacter(Referee_GetCharacterAdd(ref_ui), "0",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_AUTOAIM,
RED_BLUE, UI_DEFAULT_WIDTH, 100, 100, 200, 200, "Demo");
/* 直线 */
UI_DrawLine(Referee_GetGrapicAdd(ref_ui), "2", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_AUTOAIM, RED_BLUE, UI_DEFAULT_WIDTH, 960, 540,
960, 240);
/* 圆形 */
UI_DrawCycle(Referee_GetGrapicAdd(ref_ui), "1", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_AUTOAIM, RED_BLUE, UI_DEFAULT_WIDTH, 900,
500, 10);
/* 删除 */
UI_DelLayer(Referee_GetDelAdd(ref_ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_AUTOAIM);
break;
case CMD_UI_AUTO_AIM_START:
UI_DrawCharacter(Referee_GetCharacterAdd(ref_ui), "1",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_AUTOAIM,
RED_BLUE, UI_DEFAULT_WIDTH * 10, 50, UI_DEFAULT_WIDTH,
ref_ui->screen->width * 0.8,
ref_ui->screen->height * 0.5, "AUTO");
break;
case CMD_UI_AUTO_AIM_STOP:
UI_DelLayer(Referee_GetDelAdd(ref_ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_AUTOAIM);
default:
return -1;
}
return 0;
}
uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref) {
cap->chassis_power_limit = ref->robot_status.chassis_power_limit;
cap->chassis_pwr_buff = ref->power_heat.chassis_pwr_buff;
cap->chassis_watt = ref->power_heat.chassis_watt;
cap->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackAI(Referee_ForAI_t *ai, const Referee_t *ref) {
ai->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackChassis(Referee_ForChassis_t *chassis,
const Referee_t *ref) {
chassis->chassis_power_limit = ref->robot_status.chassis_power_limit;
chassis->chassis_pwr_buff = ref->power_heat.chassis_pwr_buff;
chassis->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackShoot(Referee_ForShoot_t *shoot, Referee_t *ref) {
memcpy(&(shoot->power_heat), &(ref->power_heat), sizeof(shoot->power_heat));
memcpy(&(shoot->robot_status), &(ref->robot_status),
sizeof(shoot->robot_status));
memcpy(&(shoot->shoot_data), &(ref->shoot_data), sizeof(shoot->shoot_data));
shoot->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_UIRefresh(Referee_UI_t *ui) {
static uint8_t fsm = 0;
if (osThreadFlagsGet() & SIGNAL_REFEREE_FAST_REFRESH_UI) {
osThreadFlagsClear(SIGNAL_REFEREE_FAST_REFRESH_UI);
switch (fsm) {
case 0: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CHASSIS);
UI_DrawLine(Referee_GetGrapicAdd(ui), "6", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12,
ui->screen->width * 0.4, ui->screen->height * 0.2,
ui->screen->width * 0.4 + sin(ui->chassis_ui.angle) * 46,
ui->screen->height * 0.2 + cos(ui->chassis_ui.angle) * 46);
float start_pos_h = 0.0f;
switch (ui->chassis_ui.mode) {
case CHASSIS_MODE_FOLLOW_GIMBAL:
start_pos_h = 0.68f;
break;
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
start_pos_h = 0.66f;
break;
case CHASSIS_MODE_ROTOR:
start_pos_h = 0.64f;
break;
default:
break;
}
if (start_pos_h != 0.0f)
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "8", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS - 6,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 44,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 1:
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CAP);
// switch (ui->cap_ui.status) {
// case CAN_CAP_STATUS_OFFLINE:
// UI_DrawArc(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD,
// UI_GRAPIC_LAYER_CAP, YELLOW, 0, 360,
// UI_DEFAULT_WIDTH * 5, ui->screen->width * 0.6,
// ui->screen->height * 0.2, 50, 50);
// break;
// break;
// case CAN_CAP_STATUS_RUNNING:
// UI_DrawArc(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD,
// UI_GRAPIC_LAYER_CAP, GREEN, 0,
// ui->cap_ui.percentage * 360, UI_DEFAULT_WIDTH * 5,
// ui->screen->width * 0.6, ui->screen->height * 0.2, 50,
// 50);
// break;
// }
break;
case 2: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_GIMBAL);
float start_pos_h = 0.0f;
switch (ui->gimbal_ui.mode) {
case GIMBAL_MODE_RELAX:
start_pos_h = 0.68f;
break;
case GIMBAL_MODE_RELATIVE:
start_pos_h = 0.66f;
break;
case GIMBAL_MODE_ABSOLUTE:
start_pos_h = 0.64f;
break;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "a", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_GIMBAL, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 54,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 102,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 3: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_SHOOT);
float start_pos_h = 0.0f;
switch (ui->shoot_ui.mode) {
case SHOOT_MODE_RELAX:
start_pos_h = 0.68f;
break;
case SHOOT_MODE_SAFE:
start_pos_h = 0.66f;
break;
case SHOOT_MODE_LOADED:
start_pos_h = 0.64f;
break;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "b", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_SHOOT, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 114,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 162,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
switch (ui->shoot_ui.fire) {
case FIRE_MODE_SINGLE:
start_pos_h = 0.68f;
break;
case FIRE_MODE_BURST:
start_pos_h = 0.66f;
break;
case FIRE_MODE_CONT:
start_pos_h = 0.64f;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "f", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_SHOOT, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 174,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 222,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 4:
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CMD);
if (ui->cmd_pc) {
UI_DrawRectangle(Referee_GetGrapicAdd(ui), "c",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_CMD, GREEN,
UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 96,
ui->screen->height * 0.4 + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 120,
ui->screen->height * 0.4 + REF_UI_BOX_BOT_OFFSET);
} else {
UI_DrawRectangle(Referee_GetGrapicAdd(ui), "c",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_CMD, GREEN,
UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 56,
ui->screen->height * 0.4 + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 80,
ui->screen->height * 0.4 + REF_UI_BOX_BOT_OFFSET);
}
break;
default:
fsm = 0;
if (ui->del_counter >= REF_UI_MAX_DEL_NUM ||
ui->character_counter > REF_UI_MAX_STRING_NUM ||
ui->grapic_counter > REF_UI_MAX_GRAPIC_NUM)
BSP_UART_GetHandle(BSP_UART_REF)->gState = HAL_UART_STATE_READY;
}
}
if (osThreadFlagsGet() & SIGNAL_REFEREE_SLOW_REFRESH_UI) {
osThreadFlagsClear(SIGNAL_REFEREE_SLOW_REFRESH_UI);
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CONST);
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "1", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.7, "CHAS GMBL SHOT FIRE");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "2", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.68, "FLLW RELX RELX SNGL");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "3", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.66, "FL35 ABSL SAFE BRST");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "4", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.64, "ROTR RLTV LOAD CONT");
UI_DrawLine(Referee_GetGrapicAdd(ui), "5", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 3,
ui->screen->width * 0.4, ui->screen->height * 0.2,
ui->screen->width * 0.4, ui->screen->height * 0.2 + 50);
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "d", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.4, "CTRL RC PC");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "e", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 20, 80,
UI_CHAR_DEFAULT_WIDTH * 2, ui->screen->width * 0.6 - 26,
ui->screen->height * 0.2 + 10, "CAP");
}
return 0;
}

508
engineer/User/device/referee.h Normal file
View File

@ -0,0 +1,508 @@
/*
裁判系统抽象。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include "component/cmd.h"
#include "component/ui.h"
#include "component/user_math.h"
#include "bsp/can.h"
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
#define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat))
#define CHASSIS_POWER_MAX_WITHOUT_REF 50.0f /* 裁判系统离线底盘最大功率 */
#define REF_UI_MAX_GRAPIC_NUM (7)
#define REF_UI_MAX_STRING_NUM (7)
#define REF_UI_MAX_DEL_NUM (3)
#define BSP_UART_REF BSP_UART_1
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint8_t sof;
uint16_t data_length;
uint8_t seq;
uint8_t crc8;
} Referee_Header_t;
typedef enum { REF_STATUS_OFFLINE = 0, REF_STATUS_RUNNING } Referee_Status_t;
typedef enum {
REF_CMD_ID_GAME_STATUS = 0x0001,
REF_CMD_ID_GAME_RESULT = 0x0002,
REF_CMD_ID_GAME_ROBOT_HP = 0x0003,
REF_CMD_ID_DART_STATUS = 0x0004,
REF_CMD_ID_ICRA_ZONE_STATUS = 0x0005,
REF_CMD_ID_FIELD_EVENTS = 0x0101,
REF_CMD_ID_SUPPLY_ACTION = 0x0102,
REF_CMD_ID_REQUEST_SUPPLY = 0x0103,
REF_CMD_ID_WARNING = 0x0104,
REF_CMD_ID_DART_COUNTDOWN = 0x0105,
REF_CMD_ID_ROBOT_STATUS = 0x0201,
REF_CMD_ID_POWER_HEAT_DATA = 0x0202,
REF_CMD_ID_ROBOT_POS = 0x0203,
REF_CMD_ID_ROBOT_BUFF = 0x0204,
REF_CMD_ID_DRONE_ENERGY = 0x0205,
REF_CMD_ID_ROBOT_DMG = 0x0206,
REF_CMD_ID_SHOOT_DATA = 0x0207,
REF_CMD_ID_BULLET_REMAINING = 0x0208,
REF_CMD_ID_RFID = 0x0209,
REF_CMD_ID_DART_CLIENT = 0x020A,
REF_CMD_ID_INTER_STUDENT = 0x0301,
REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302,
REF_CMD_ID_CLIENT_MAP = 0x0303,
REF_CMD_ID_KEYBOARD_MOUSE = 0x0304,
} Referee_CMDID_t;
typedef struct __packed {
uint8_t game_type : 4;
uint8_t game_progress : 4;
uint16_t stage_remain_time;
uint64_t sync_time_stamp;
} Referee_GameStatus_t;
typedef struct __packed {
uint8_t winner;
} Referee_GameResult_t;
typedef struct __packed {
uint16_t red_1;
uint16_t red_2;
uint16_t red_3;
uint16_t red_4;
uint16_t red_5;
uint16_t red_6;
uint16_t red_7;
uint16_t red_outpose;
uint16_t red_base;
uint16_t blue_1;
uint16_t blue_2;
uint16_t blue_3;
uint16_t blue_4;
uint16_t blue_5;
uint16_t blue_6;
uint16_t blue_7;
uint16_t blue_outpose;
uint16_t blue_base;
} Referee_GameRobotHP_t;
typedef struct __packed {
uint8_t dart_belong;
uint16_t stage_remain_time;
} Referee_DartStatus_t;
typedef struct __packed {
uint8_t f1_status : 1;
uint8_t f1_buff_status : 3;
uint8_t f2_status : 1;
uint8_t f2_buff_status : 3;
uint8_t f3_status : 1;
uint8_t f3_buff_status : 3;
uint8_t f4_status : 1;
uint8_t f4_buff_status : 3;
uint8_t f5_status : 1;
uint8_t f5_buff_status : 3;
uint8_t f6_status : 1;
uint8_t f6_buff_status : 3;
uint16_t red1_bullet_remain;
uint16_t red2_bullet_remain;
uint16_t blue1_bullet_remain;
uint16_t blue2_bullet_remain;
} Referee_ICRAZoneStatus_t;
typedef struct __packed {
uint8_t copter_pad : 2;
uint8_t energy_mech : 2;
uint8_t virtual_shield : 1;
uint32_t res : 27;
} Referee_FieldEvents_t;
typedef struct __packed {
uint8_t supply_id;
uint8_t robot_id;
uint8_t supply_step;
uint8_t supply_sum;
} Referee_SupplyAction_t;
typedef struct __packed {
uint8_t place_holder; /* TODO */
} Referee_RequestSupply_t;
typedef struct __packed {
uint8_t level;
uint8_t robot_id;
} Referee_Warning_t;
typedef struct __packed {
uint8_t countdown;
} Referee_DartCountdown_t;
// typedef struct __packed {
// uint8_t robot_id;
// uint8_t robot_level;
// uint16_t remain_hp;
// uint16_t max_hp;
// uint16_t shoot_id1_17_cooling_rate;
// uint16_t shoot_id1_17_heat_limit;
// uint16_t shoot_id1_17_speed_limit;
// uint16_t shoot_id2_17_cooling_rate;
// uint16_t shoot_id2_17_heat_limit;
// uint16_t shoot_id2_17_speed_limit;
// uint16_t shoot_42_cooling_rate;
// uint16_t shoot_42_heat_limit;
// uint16_t shoot_42_speed_limit;
// uint16_t chassis_power_limit;
// uint8_t power_gimbal_output : 1;
// uint8_t power_chassis_output : 1;
// uint8_t power_shoot_output : 1;
// } Referee_RobotStatus_t;
typedef struct __packed {
uint8_t robot_id;
uint8_t robot_level;
uint16_t remain_hp;
uint16_t max_hp;
uint16_t shooter_cooling_value;
uint16_t shooter_heat_limit;
uint16_t chassis_power_limit;
uint8_t power_gimbal_output : 1;
uint8_t power_chassis_output : 1;
uint8_t power_shoot_output : 1;
} Referee_RobotStatus_t; /* 0x0201 */
typedef struct __packed {
uint16_t chassis_volt;
uint16_t chassis_amp;
float chassis_watt;
uint16_t chassis_pwr_buff;
uint16_t shoot_id1_17_heat;
uint16_t shoot_id2_17_heat;
uint16_t shoot_42_heat;
} Referee_PowerHeat_t;
typedef struct __packed {
float x;
float y;
float z;
float yaw;
} Referee_RobotPos_t;
// typedef struct __packed {
// uint8_t healing : 1;
// uint8_t cooling_acc : 1;
// uint8_t defense_buff : 1;
// uint8_t attack_buff : 1;
// uint8_t res : 4;
// } Referee_RobotBuff_t;
typedef struct __packed {
uint8_t healing_buff;
uint8_t cooling_acc;
uint8_t defense_buff;
uint8_t vulnerability_buff;
uint16_t attack_buff;
} Referee_RobotBuff_t;
typedef struct __packed {
uint8_t attack_countdown;
} Referee_DroneEnergy_t;
typedef struct __packed {
uint8_t armor_id : 4;
uint8_t damage_type : 4;
} Referee_RobotDamage_t;
typedef struct __packed {
uint8_t bullet_type;
uint8_t shooter_id;
uint8_t bullet_freq;
float bullet_speed;
} Referee_ShootData_t;/* 0x0207 */
typedef struct __packed {
uint16_t bullet_17_remain;
uint16_t bullet_42_remain;
uint16_t coin_remain;
} Referee_BulletRemain_t;
typedef struct __packed {
uint8_t base : 1;
uint8_t high_ground : 1;
uint8_t energy_mech : 1;
uint8_t slope : 1;
uint8_t outpose : 1;
uint8_t resource : 1;
uint8_t healing_card : 1;
uint32_t res : 24;
} Referee_RFID_t;
typedef struct __packed {
uint8_t opening;
uint8_t target;
uint8_t target_changable_countdown;
uint8_t dart1_speed;
uint8_t dart2_speed;
uint8_t dart3_speed;
uint8_t dart4_speed;
uint16_t last_dart_launch_time;
uint16_t operator_cmd_launch_time;
} Referee_DartClient_t;
typedef struct __packed {
float position_x;
float position_y;
float position_z;
uint8_t commd_keyboard;
uint16_t robot_id;
} Referee_ClientMap_t;
typedef struct __packed {
int16_t mouse_x;
int16_t mouse_y;
int16_t mouse_wheel;
int8_t button_l;
int8_t button_r;
uint16_t keyboard_value;
uint16_t res;
} Referee_KeyboardMouse_t;
typedef uint16_t Referee_Tail_t;
typedef enum {
REF_BOT_RED_HERO = 1,
REF_BOT_RED_ENGINEER = 2,
REF_BOT_RED_INFANTRY_1 = 3,
REF_BOT_RED_INFANTRY_2 = 4,
REF_BOT_RED_INFANTRY_3 = 5,
REF_BOT_RED_DRONE = 6,
REF_BOT_RED_SENTRY = 7,
REF_BOT_RED_RADER = 9,
REF_BOT_BLU_HERO = 101,
REF_BOT_BLU_ENGINEER = 102,
REF_BOT_BLU_INFANTRY_1 = 103,
REF_BOT_BLU_INFANTRY_2 = 104,
REF_BOT_BLU_INFANTRY_3 = 105,
REF_BOT_BLU_DRONE = 106,
REF_BOT_BLU_SENTRY = 107,
REF_BOT_BLU_RADER = 109,
} Referee_RobotID_t;
typedef enum {
REF_CL_RED_HERO = 0x0101,
REF_CL_RED_ENGINEER = 0x0102,
REF_CL_RED_INFANTRY_1 = 0x0103,
REF_CL_RED_INFANTRY_2 = 0x0104,
REF_CL_RED_INFANTRY_3 = 0x0105,
REF_CL_RED_DRONE = 0x0106,
REF_CL_BLU_HERO = 0x0165,
REF_CL_BLU_ENGINEER = 0x0166,
REF_CL_BLU_INFANTRY_1 = 0x0167,
REF_CL_BLU_INFANTRY_2 = 0x0168,
REF_CL_BLU_INFANTRY_3 = 0x0169,
REF_CL_BLU_DRONE = 0x016A,
} Referee_ClientID_t;
typedef enum {
REF_STDNT_CMD_ID_UI_DEL = 0x0100,
REF_STDNT_CMD_ID_UI_DRAW1 = 0x0101,
REF_STDNT_CMD_ID_UI_DRAW2 = 0x0102,
REF_STDNT_CMD_ID_UI_DRAW5 = 0x0103,
REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104,
REF_STDNT_CMD_ID_UI_STR = 0x0110,
REF_STDNT_CMD_ID_CUSTOM = 0x0200,
} Referee_StudentCMDID_t;
typedef struct __packed {
Referee_StudentCMDID_t data_cmd_id;
uint16_t id_sender;
uint16_t id_receiver;
uint8_t *data;
} Referee_InterStudent_t;
typedef struct __packed {
uint8_t place_holder;
} Referee_InterStudent_Custom_t;
typedef struct {
Referee_Status_t ref_status;
Referee_GameStatus_t game_status;
Referee_GameResult_t game_result;
Referee_GameRobotHP_t game_robot_hp;
Referee_DartStatus_t dart_status;
Referee_ICRAZoneStatus_t icra_zone;
Referee_FieldEvents_t field_event;
Referee_SupplyAction_t supply_action;
Referee_RequestSupply_t request_supply;
Referee_Warning_t warning;
Referee_DartCountdown_t dart_countdown;
Referee_RobotStatus_t robot_status;
Referee_PowerHeat_t power_heat;
Referee_RobotPos_t robot_pos;
Referee_RobotBuff_t robot_buff;
Referee_DroneEnergy_t drone_energy;
Referee_RobotDamage_t robot_danage;
Referee_ShootData_t shoot_data;
Referee_BulletRemain_t bullet_remain;
Referee_RFID_t rfid;
Referee_DartClient_t dart_client;
Referee_InterStudent_Custom_t custom;
Referee_ClientMap_t client_map;
Referee_KeyboardMouse_t keyboard_mouse;
osTimerId_t ui_fast_timer_id;
osTimerId_t ui_slow_timer_id;
} Referee_t;
typedef struct {
CMD_ChassisMode_t mode;
float angle;
} Referee_ChassisUI_t;
typedef struct {
float percentage;
// CAN_CapStatus_t status;
} Referee_CapUI_t;
typedef struct {
CMD_GimbalMode_t mode;
} Referee_GimbalUI_t;
typedef struct {
CMD_ShootMode_t mode;
CMD_FireMode_t fire;
} Referee_ShootUI_t;
typedef struct __packed {
/* UI缓冲数据 */
UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM];
UI_Drawcharacter_t character_data[REF_UI_MAX_STRING_NUM];
UI_Del_t del[REF_UI_MAX_DEL_NUM];
/* 待发送数量 */
uint8_t grapic_counter;
uint8_t character_counter;
uint8_t del_counter;
/* UI所需信息 */
Referee_CapUI_t cap_ui;
Referee_ChassisUI_t chassis_ui;
Referee_ShootUI_t shoot_ui;
Referee_GimbalUI_t gimbal_ui;
bool cmd_pc;
/* 屏幕分辨率 */
const CMD_Screen_t *screen;
} Referee_UI_t;
typedef struct __packed {
uint16_t data_cmd_id;
uint16_t sender_ID;
uint16_t receiver_ID;
} Referee_Interactive_Header_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_1_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_1_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_2_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_2_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_5_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_5_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_7_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_7_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawcharacter_t data;
uint16_t crc16;
} Referee_UI_Drawcharacter_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Del_t data;
uint16_t crc16;
} Referee_UI_Del_t;
typedef struct {
Referee_Status_t ref_status;
float chassis_watt;
float chassis_power_limit;
float chassis_pwr_buff;
} Referee_ForCap_t;
typedef struct {
Referee_Status_t ref_status;
} Referee_ForAI_t;
typedef struct {
Referee_Status_t ref_status;
float chassis_power_limit;
float chassis_pwr_buff;
} Referee_ForChassis_t;
typedef struct {
Referee_Status_t ref_status;
Referee_PowerHeat_t power_heat;
Referee_RobotStatus_t robot_status;
Referee_ShootData_t shoot_data;
} Referee_ForShoot_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui,
const CMD_Screen_t *screen);
int8_t Referee_Restart(void);
int8_t Referee_StartReceiving(Referee_t *ref);
int8_t Referee_Parse(Referee_t *ref);
void Referee_HandleOffline(Referee_t *referee);
int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
Referee_StudentCMDID_t cmd_id, uint8_t sender_id);
int8_t Referee_StartSend(uint8_t *data, uint32_t len);
int8_t Referee_MoveData(void *data, void *tmp, uint32_t len);
int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref);
UI_Ele_t *Referee_GetGrapicAdd(Referee_UI_t *ref_ui);
UI_Drawcharacter_t *Referee_GetCharacterAdd(Referee_UI_t *ref_ui);
uint8_t Referee_PraseCmd(Referee_UI_t *ref_ui, CMD_UI_t cmd);
uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref);
uint8_t Referee_PackShoot(Referee_ForShoot_t *ai, Referee_t *ref);
uint8_t Referee_PackChassis(Referee_ForChassis_t *chassis,
const Referee_t *ref);
uint8_t Referee_PackAI(Referee_ForAI_t *shoot, const Referee_t *ref);
uint8_t Referee_UIRefresh(Referee_UI_t *ui);
#ifdef __cplusplus
}
#endif

26
engineer/User/module/arm.c
View File

@ -6,6 +6,7 @@
#include "component/pid.h" #include "component/pid.h"
#include "component/user_math.h" #include "component/user_math.h"
#include "bsp/dwt.h" #include "bsp/dwt.h"
#include "component/cmd.h"
@ -262,7 +263,7 @@ void arm_status_reset(arm_t* arm)
arm->status.step=0; arm->status.step=0;
} }
void arm_follow_control(arm_t* arm,arm_rx_data_t* data) void arm_follow_control(arm_t* arm,CMD_Arm_t* data)
{ {
assert_param(!(arm==NULL||data==NULL)); assert_param(!(arm==NULL||data==NULL));
switch (arm->status.step) { switch (arm->status.step) {
@ -270,7 +271,7 @@ void arm_follow_control(arm_t* arm,arm_rx_data_t* data)
{ {
if(arm->status.busy==0) //检测是否非忙碌,并进行第一次初始化 if(arm->status.busy==0) //检测是否非忙碌,并进行第一次初始化
{ {
arm_path_start(arm, data->ref_angle); //初始化路径规划,设定终点为控制器当前值 arm_path_start(arm, data->data.ref_angle); //初始化路径规划,设定终点为控制器当前值
arm->status.busy=1; //开始路径，进入忙碌 arm->status.busy=1; //开始路径，进入忙碌
} }
arm_path_process(arm); //计算并设定一次路径点 arm_path_process(arm); //计算并设定一次路径点
@ -285,14 +286,14 @@ void arm_follow_control(arm_t* arm,arm_rx_data_t* data)
{ {
arm->status.busy=0; //退出忙碌，可以接收指令 arm->status.busy=0; //退出忙碌，可以接收指令
for (int i=0; i<JOINT_NUM; i++) { for (int i=0; i<JOINT_NUM; i++) {
arm->joint[i].ref.position += data->ref_angle[i]; //跟踪控制器 arm->joint[i].ref.position += data->data.ref_angle[i]; //跟踪控制器
} }
break; break;
} }
} }
} }
void arm_controler_process(arm_t* arm,arm_rx_data_t* data) //自定义控制器指令处理 void arm_controler_process(arm_t* arm,CMD_Arm_t* data) //自定义控制器指令处理
{ {
assert_param(!(arm==NULL||data==NULL)); assert_param(!(arm==NULL||data==NULL));
@ -304,24 +305,27 @@ void arm_controler_process(arm_t* arm,arm_rx_data_t* data) //自定义控制器
if(data->cmd_prepare==0&&arm->status.cmd_prepare==1) //接收准备信号结束，并且已处于接收准备状态 if(data->cmd_prepare==0&&arm->status.cmd_prepare==1) //接收准备信号结束，并且已处于接收准备状态
{ {
arm_status_reset(arm); //清除状态 arm_status_reset(arm); //清除状态
switch (data->cmd) { switch (data->mode) {
case set_relax_mode: case SET_ARM_MODE_RELAX:
arm->mode = RELAX_MODE; arm->mode = RELAX_MODE;
break; break;
case set_follow_mode: case SET_ARM_MODE_FOLLOW:
arm->mode = FOLLOW_MODE; arm->mode = FOLLOW_MODE;
break; break;
case start_storage: case SET_ARM_MODE_STORAGE:
arm->mode=STORAGE_MODE; arm->mode=STORAGE_MODE;
break; break;
case start_take: case SET_ARM_MODE_TAKE:
arm->mode=TAKE_MODE; arm->mode=TAKE_MODE;
break; break;
case SET_ARM_MODE_POSITION:
arm->mode=SET_ARM_MODE_POSITION;
break;
} }
} }
} }
void arm_control_process(arm_t* arm,arm_rx_data_t* data) void arm_control_process(arm_t* arm,CMD_Arm_t* data)
{ {
switch (arm->mode) { switch (arm->mode) {
case RELAX_MODE: case RELAX_MODE:
@ -334,6 +338,8 @@ void arm_control_process(arm_t* arm,arm_rx_data_t* data)
break; break;
case TAKE_MODE: case TAKE_MODE:
break; break;
case POSITION_MODE:
break;
} }
} }

12
engineer/User/module/arm.h
View File

@ -1,4 +1,9 @@
#pragma once #pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "main.h" #include "main.h"
#include "component/kinematics.h" #include "component/kinematics.h"
#include "device/motor_dm.h" #include "device/motor_dm.h"
@ -87,7 +92,6 @@ typedef struct{
typedef struct hand_t typedef struct hand_t
{ {
float mass[6]; float mass[6];
robot_t kine;
joint_t joint[JOINT_NUM]; joint_t joint[JOINT_NUM];
struct { struct {
uint8_t cmd_prepare; uint8_t cmd_prepare;
@ -99,10 +103,14 @@ typedef struct hand_t
RELAX_MODE=0, RELAX_MODE=0,
FOLLOW_MODE, FOLLOW_MODE,
STORAGE_MODE, STORAGE_MODE,
TAKE_MODE TAKE_MODE,
POSITION_MODE
}mode; }mode;
}arm_t; }arm_t;
#ifdef __cplusplus
}
#endif

5
engineer/User/task/control.c
View File

@ -8,6 +8,9 @@
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "component/kinematics.h" #include "component/kinematics.h"
#include "device/motor_lz.h" #include "device/motor_lz.h"
#include "device/motor_dm.h"
#include "component/pid.h"
#include "component/filter.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -130,4 +133,4 @@ void Task_control(void *argument) {
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }
} }

3
engineer/User/task/init.c
View File

@ -31,7 +31,6 @@ void Task_Init(void *argument) {
/* 创建任务线程 */ /* 创建任务线程 */
task_runtime.thread.control = osThreadNew(Task_control, NULL, &attr_control); task_runtime.thread.control = osThreadNew(Task_control, NULL, &attr_control);
task_runtime.thread.display = osThreadNew(Task_display, NULL, &attr_display);
// 创建消息队列 // 创建消息队列
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
@ -40,4 +39,4 @@ void Task_Init(void *argument) {
osKernelUnlock(); // 解锁内核 osKernelUnlock(); // 解锁内核
osThreadTerminate(osThreadGetId()); // 任务完成后结束自身 osThreadTerminate(osThreadGetId()); // 任务完成后结束自身
} }

4
engineer/cmake/stm32cubemx/CMakeLists.txt
View File

@ -26,6 +26,7 @@ set(MX_Application_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/can.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/can.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/usart.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_it.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_it.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_msp.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_msp.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_timebase_tim.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_timebase_tim.c
@ -52,7 +53,8 @@ set(STM32_Drivers_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
) )
# Drivers Midllewares # Drivers Midllewares