加了一堆东西
This commit is contained in:
shentou
parent
2ebe359343
commit
0f6cf39492
2
.gitattributes
vendored
2
.gitattributes
vendored
@ -1 +1 @@
|
||||
* text=auto
|
||||
* text=auto eol=lf
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -50,6 +50,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/bsp/mm.c
|
||||
User/bsp/time.c
|
||||
User/bsp/dwt.c
|
||||
./User/bsp/uart.c
|
||||
|
||||
# User/component sources
|
||||
User/component/ahrs.c
|
||||
@ -58,6 +59,14 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/component/pid.c
|
||||
User/component/user_math.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/motor.c
|
||||
|
||||
52
engineer/Core/Inc/dma.h
Normal file
52
engineer/Core/Inc/dma.h
Normal 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__ */
|
||||
|
||||
@ -64,7 +64,7 @@
|
||||
/* #define HAL_MMC_MODULE_ENABLED */
|
||||
/* #define HAL_SPI_MODULE_ENABLED */
|
||||
#define HAL_TIM_MODULE_ENABLED
|
||||
/* #define HAL_UART_MODULE_ENABLED */
|
||||
#define HAL_UART_MODULE_ENABLED
|
||||
/* #define HAL_USART_MODULE_ENABLED */
|
||||
/* #define HAL_IRDA_MODULE_ENABLED */
|
||||
/* #define HAL_SMARTCARD_MODULE_ENABLED */
|
||||
|
||||
@ -55,6 +55,8 @@ void DebugMon_Handler(void);
|
||||
void CAN1_TX_IRQHandler(void);
|
||||
void CAN1_RX0_IRQHandler(void);
|
||||
void CAN1_RX1_IRQHandler(void);
|
||||
void USART1_IRQHandler(void);
|
||||
void USART2_IRQHandler(void);
|
||||
void TIM8_TRG_COM_TIM14_IRQHandler(void);
|
||||
/* USER CODE BEGIN EFP */
|
||||
|
||||
|
||||
55
engineer/Core/Inc/usart.h
Normal file
55
engineer/Core/Inc/usart.h
Normal 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
65
engineer/Core/Src/dma.c
Normal 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 */
|
||||
|
||||
@ -44,6 +44,7 @@ void MX_GPIO_Init(void)
|
||||
|
||||
/* GPIO Ports Clock Enable */
|
||||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOD_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOH_CLK_ENABLE();
|
||||
|
||||
|
||||
@ -20,6 +20,7 @@
|
||||
#include "main.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "can.h"
|
||||
#include "usart.h"
|
||||
#include "gpio.h"
|
||||
|
||||
/* Private includes ----------------------------------------------------------*/
|
||||
@ -90,6 +91,8 @@ int main(void)
|
||||
/* Initialize all configured peripherals */
|
||||
MX_GPIO_Init();
|
||||
MX_CAN1_Init();
|
||||
MX_USART1_UART_Init();
|
||||
MX_USART2_UART_Init();
|
||||
/* USER CODE BEGIN 2 */
|
||||
|
||||
/* USER CODE END 2 */
|
||||
|
||||
@ -56,6 +56,8 @@
|
||||
|
||||
/* External variables --------------------------------------------------------*/
|
||||
extern CAN_HandleTypeDef hcan1;
|
||||
extern UART_HandleTypeDef huart1;
|
||||
extern UART_HandleTypeDef huart2;
|
||||
extern TIM_HandleTypeDef htim14;
|
||||
|
||||
/* USER CODE BEGIN EV */
|
||||
@ -202,6 +204,34 @@ void CAN1_RX1_IRQHandler(void)
|
||||
/* 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.
|
||||
*/
|
||||
|
||||
196
engineer/Core/Src/usart.c
Normal file
196
engineer/Core/Src/usart.c
Normal 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 */
|
||||
@ -22,18 +22,24 @@ Mcu.IP1=FREERTOS
|
||||
Mcu.IP2=NVIC
|
||||
Mcu.IP3=RCC
|
||||
Mcu.IP4=SYS
|
||||
Mcu.IPNb=5
|
||||
Mcu.IP5=USART1
|
||||
Mcu.IP6=USART2
|
||||
Mcu.IPNb=7
|
||||
Mcu.Name=STM32F407I(E-G)Hx
|
||||
Mcu.Package=UFBGA176
|
||||
Mcu.Pin0=PA14
|
||||
Mcu.Pin1=PA13
|
||||
Mcu.Pin2=PD0
|
||||
Mcu.Pin3=PD1
|
||||
Mcu.Pin4=PH0-OSC_IN
|
||||
Mcu.Pin5=PH1-OSC_OUT
|
||||
Mcu.Pin6=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin7=VP_SYS_VS_tim14
|
||||
Mcu.PinsNb=8
|
||||
Mcu.Pin10=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin11=VP_SYS_VS_tim14
|
||||
Mcu.Pin2=PB7
|
||||
Mcu.Pin3=PB6
|
||||
Mcu.Pin4=PD6
|
||||
Mcu.Pin5=PD0
|
||||
Mcu.Pin6=PD5
|
||||
Mcu.Pin7=PD1
|
||||
Mcu.Pin8=PH0-OSC_IN
|
||||
Mcu.Pin9=PH1-OSC_OUT
|
||||
Mcu.PinsNb=12
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
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.TimeBase=TIM8_TRG_COM_TIM14_IRQn
|
||||
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
|
||||
PA13.Mode=Serial_Wire
|
||||
PA13.Signal=SYS_JTMS-SWDIO
|
||||
PA14.Mode=Serial_Wire
|
||||
PA14.Signal=SYS_JTCK-SWCLK
|
||||
PB6.Mode=Asynchronous
|
||||
PB6.Signal=USART1_TX
|
||||
PB7.Mode=Asynchronous
|
||||
PB7.Signal=USART1_RX
|
||||
PD0.Locked=true
|
||||
PD0.Mode=CAN_Activate
|
||||
PD0.Signal=CAN1_RX
|
||||
PD1.Locked=true
|
||||
PD1.Mode=CAN_Activate
|
||||
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.Signal=RCC_OSC_IN
|
||||
PH1-OSC_OUT.Mode=HSE-External-Oscillator
|
||||
@ -140,6 +156,10 @@ RCC.VCOI2SOutputFreq_Value=384000000
|
||||
RCC.VCOInputFreq_Value=2000000
|
||||
RCC.VCOOutputFreq_Value=336000000
|
||||
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.Signal=FREERTOS_VS_CMSIS_V2
|
||||
VP_SYS_VS_tim14.Mode=TIM14
|
||||
|
||||
159
engineer/User/bsp/uart.c
Normal file
159
engineer/User/bsp/uart.c
Normal 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
69
engineer/User/bsp/uart.h
Normal 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
387
engineer/User/component/cmd.c
Normal 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
342
engineer/User/component/cmd.h
Normal 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
62
engineer/User/component/crc16.c
Normal 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
30
engineer/User/component/crc16.h
Normal 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
52
engineer/User/component/crc8.c
Normal 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
30
engineer/User/component/crc8.h
Normal 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
107
engineer/User/component/limiter.c
Normal 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
63
engineer/User/component/limiter.h
Normal 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
24
engineer/User/component/matrix.cpp
Normal 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
259
engineer/User/component/matrix.hpp
Normal 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
340
engineer/User/component/robotics.cpp
Normal 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
407
engineer/User/component/robotics.hpp
Normal 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
301
engineer/User/component/ui.c
Normal 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
284
engineer/User/component/ui.h
Normal 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
56
engineer/User/component/utils.cpp
Normal 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
27
engineer/User/component/utils.hpp
Normal 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
788
engineer/User/device/referee.c
Normal 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
508
engineer/User/device/referee.h
Normal 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
|
||||
@ -6,6 +6,7 @@
|
||||
#include "component/pid.h"
|
||||
#include "component/user_math.h"
|
||||
#include "bsp/dwt.h"
|
||||
#include "component/cmd.h"
|
||||
|
||||
|
||||
|
||||
@ -262,7 +263,7 @@ void arm_status_reset(arm_t* arm)
|
||||
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));
|
||||
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) //检测是否非忙碌,并进行第一次初始化
|
||||
{
|
||||
arm_path_start(arm, data->ref_angle); //初始化路径规划,设定终点为控制器当前值
|
||||
arm_path_start(arm, data->data.ref_angle); //初始化路径规划,设定终点为控制器当前值
|
||||
arm->status.busy=1; //开始路径，进入忙碌
|
||||
}
|
||||
arm_path_process(arm); //计算并设定一次路径点
|
||||
@ -285,14 +286,14 @@ void arm_follow_control(arm_t* arm,arm_rx_data_t* data)
|
||||
{
|
||||
arm->status.busy=0; //退出忙碌，可以接收指令
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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));
|
||||
|
||||
@ -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) //接收准备信号结束，并且已处于接收准备状态
|
||||
{
|
||||
arm_status_reset(arm); //清除状态
|
||||
switch (data->cmd) {
|
||||
case set_relax_mode:
|
||||
switch (data->mode) {
|
||||
case SET_ARM_MODE_RELAX:
|
||||
arm->mode = RELAX_MODE;
|
||||
break;
|
||||
case set_follow_mode:
|
||||
case SET_ARM_MODE_FOLLOW:
|
||||
arm->mode = FOLLOW_MODE;
|
||||
break;
|
||||
case start_storage:
|
||||
case SET_ARM_MODE_STORAGE:
|
||||
arm->mode=STORAGE_MODE;
|
||||
break;
|
||||
case start_take:
|
||||
case SET_ARM_MODE_TAKE:
|
||||
arm->mode=TAKE_MODE;
|
||||
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) {
|
||||
case RELAX_MODE:
|
||||
@ -334,6 +338,8 @@ void arm_control_process(arm_t* arm,arm_rx_data_t* data)
|
||||
break;
|
||||
case TAKE_MODE:
|
||||
break;
|
||||
case POSITION_MODE:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1,4 +1,9 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "main.h"
|
||||
#include "component/kinematics.h"
|
||||
#include "device/motor_dm.h"
|
||||
@ -87,7 +92,6 @@ typedef struct{
|
||||
typedef struct hand_t
|
||||
{
|
||||
float mass[6];
|
||||
robot_t kine;
|
||||
joint_t joint[JOINT_NUM];
|
||||
struct {
|
||||
uint8_t cmd_prepare;
|
||||
@ -99,10 +103,14 @@ typedef struct hand_t
|
||||
RELAX_MODE=0,
|
||||
FOLLOW_MODE,
|
||||
STORAGE_MODE,
|
||||
TAKE_MODE
|
||||
TAKE_MODE,
|
||||
POSITION_MODE
|
||||
}mode;
|
||||
|
||||
|
||||
|
||||
}arm_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -8,6 +8,9 @@
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "component/kinematics.h"
|
||||
#include "device/motor_lz.h"
|
||||
#include "device/motor_dm.h"
|
||||
#include "component/pid.h"
|
||||
#include "component/filter.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
|
||||
@ -31,7 +31,6 @@ void Task_Init(void *argument) {
|
||||
|
||||
/* 创建任务线程 */
|
||||
task_runtime.thread.control = osThreadNew(Task_control, NULL, &attr_control);
|
||||
task_runtime.thread.display = osThreadNew(Task_display, NULL, &attr_display);
|
||||
|
||||
// 创建消息队列
|
||||
/* USER MESSAGE BEGIN */
|
||||
|
||||
@ -26,6 +26,7 @@ set(MX_Application_Src
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.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_hal_msp.c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_timebase_tim.c
|
||||
@ -53,6 +54,7 @@ set(STM32_Drivers_Src
|
||||
${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_exti.c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
|
||||
)
|
||||
|
||||
# Drivers Midllewares
|
||||
|
||||
Loading…
Reference in New Issue
Block a user