robofish/CM_DOG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

运动学正确

Browse Source
This commit is contained in:
Robofish 2025-06-26 05:11:10 +08:00
parent 068ec4099a
commit 4af0cc6bf0
44 changed files with 7640 additions and 6224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

9
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,9 @@
{
"files.associations": {
"device.h": "c",
"ahrs.h": "c",
"string.h": "c",
"user_math.h": "c",
"__locale": "c"
}
}

3
Core/Inc/stm32h7xx_it.h
View File

@ -71,6 +71,9 @@ void EXTI15_10_IRQHandler(void);
void DMA1_Stream7_IRQHandler(void);
void UART5_IRQHandler(void);
void DMA2_Stream0_IRQHandler(void);
void DMA2_Stream1_IRQHandler(void);
void DMA2_Stream2_IRQHandler(void);
void DMA2_Stream3_IRQHandler(void);
void OTG_HS_IRQHandler(void);
void UART7_IRQHandler(void);
void USART10_IRQHandler(void);

9
Core/Src/dma.c
View File

@ -71,6 +71,15 @@ void MX_DMA_Init(void)
/* DMA2_Stream0_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
/* DMA2_Stream1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
/* DMA2_Stream2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
/* DMA2_Stream3_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn);
}

6
Core/Src/spi.c
View File

@ -89,7 +89,7 @@ void MX_SPI2_Init(void)
hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi2.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi2.Init.NSS = SPI_NSS_SOFT;
hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
@ -210,7 +210,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
hdma_spi2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_spi2_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_spi2_rx.Init.Mode = DMA_NORMAL;
hdma_spi2_rx.Init.Priority = DMA_PRIORITY_LOW;
hdma_spi2_rx.Init.Priority = DMA_PRIORITY_VERY_HIGH;
hdma_spi2_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_spi2_rx) != HAL_OK)
{
@ -228,7 +228,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
hdma_spi2_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_spi2_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_spi2_tx.Init.Mode = DMA_NORMAL;
hdma_spi2_tx.Init.Priority = DMA_PRIORITY_LOW;
hdma_spi2_tx.Init.Priority = DMA_PRIORITY_VERY_HIGH;
hdma_spi2_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_spi2_tx) != HAL_OK)
{

53
Core/Src/stm32h7xx_it.c
View File

@ -65,11 +65,14 @@ extern DMA_HandleTypeDef hdma_spi2_rx;
extern DMA_HandleTypeDef hdma_spi2_tx;
extern SPI_HandleTypeDef hspi2;
extern DMA_HandleTypeDef hdma_uart5_rx;
extern DMA_HandleTypeDef hdma_uart7_rx;
extern DMA_HandleTypeDef hdma_uart7_tx;
extern DMA_HandleTypeDef hdma_usart2_rx;
extern DMA_HandleTypeDef hdma_usart2_tx;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart3_tx;
extern DMA_HandleTypeDef hdma_usart10_tx;
extern DMA_HandleTypeDef hdma_usart10_rx;
extern UART_HandleTypeDef huart5;
extern UART_HandleTypeDef huart7;
extern UART_HandleTypeDef huart1;
@ -358,7 +361,7 @@ void USART1_IRQHandler(void)
/* USER CODE END USART1_IRQn 0 */
HAL_UART_IRQHandler(&huart1);
/* USER CODE BEGIN USART1_IRQn 1 */
BSP_UART_IRQHandler(&huart1);
/* USER CODE END USART1_IRQn 1 */
}
@ -430,7 +433,7 @@ void UART5_IRQHandler(void)
/* USER CODE END UART5_IRQn 0 */
HAL_UART_IRQHandler(&huart5);
/* USER CODE BEGIN UART5_IRQn 1 */
BSP_UART_IRQHandler(&huart5);
/* USER CODE END UART5_IRQn 1 */
}
@ -448,6 +451,48 @@ void DMA2_Stream0_IRQHandler(void)
/* USER CODE END DMA2_Stream0_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream1 global interrupt.
*/
void DMA2_Stream1_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream1_IRQn 0 */
/* USER CODE END DMA2_Stream1_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_uart7_rx);
/* USER CODE BEGIN DMA2_Stream1_IRQn 1 */
/* USER CODE END DMA2_Stream1_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream2 global interrupt.
*/
void DMA2_Stream2_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream2_IRQn 0 */
/* USER CODE END DMA2_Stream2_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_uart7_tx);
/* USER CODE BEGIN DMA2_Stream2_IRQn 1 */
/* USER CODE END DMA2_Stream2_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream3 global interrupt.
*/
void DMA2_Stream3_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream3_IRQn 0 */
/* USER CODE END DMA2_Stream3_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart10_rx);
/* USER CODE BEGIN DMA2_Stream3_IRQn 1 */
/* USER CODE END DMA2_Stream3_IRQn 1 */
}
/**
* @brief This function handles USB On The Go HS global interrupt.
*/
@ -472,7 +517,7 @@ void UART7_IRQHandler(void)
/* USER CODE END UART7_IRQn 0 */
HAL_UART_IRQHandler(&huart7);
/* USER CODE BEGIN UART7_IRQn 1 */
BSP_UART_IRQHandler(&huart7);
/* USER CODE END UART7_IRQn 1 */
}
@ -486,7 +531,7 @@ void USART10_IRQHandler(void)
/* USER CODE END USART10_IRQn 0 */
HAL_UART_IRQHandler(&huart10);
/* USER CODE BEGIN USART10_IRQn 1 */
BSP_UART_IRQHandler(&huart10);
/* USER CODE END USART10_IRQn 1 */
}

67
Core/Src/usart.c
View File

@ -31,11 +31,14 @@ UART_HandleTypeDef huart2;
UART_HandleTypeDef huart3;
UART_HandleTypeDef huart10;
DMA_HandleTypeDef hdma_uart5_rx;
DMA_HandleTypeDef hdma_uart7_rx;
DMA_HandleTypeDef hdma_uart7_tx;
DMA_HandleTypeDef hdma_usart2_rx;
DMA_HandleTypeDef hdma_usart2_tx;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart3_tx;
DMA_HandleTypeDef hdma_usart10_tx;
DMA_HandleTypeDef hdma_usart10_rx;
/* UART5 init function */
void MX_UART5_Init(void)
@ -268,7 +271,7 @@ void MX_USART10_UART_Init(void)
/* USER CODE END USART10_Init 1 */
huart10.Instance = USART10;
huart10.Init.BaudRate = 115200;
huart10.Init.BaudRate = 921600;
huart10.Init.WordLength = UART_WORDLENGTH_8B;
huart10.Init.StopBits = UART_STOPBITS_1;
huart10.Init.Parity = UART_PARITY_NONE;
@ -399,6 +402,43 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
GPIO_InitStruct.Alternate = GPIO_AF7_UART7;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
/* UART7 DMA Init */
/* UART7_RX Init */
hdma_uart7_rx.Instance = DMA2_Stream1;
hdma_uart7_rx.Init.Request = DMA_REQUEST_UART7_RX;
hdma_uart7_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_uart7_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_uart7_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_uart7_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_uart7_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_uart7_rx.Init.Mode = DMA_NORMAL;
hdma_uart7_rx.Init.Priority = DMA_PRIORITY_MEDIUM;
hdma_uart7_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_uart7_rx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmarx,hdma_uart7_rx);
/* UART7_TX Init */
hdma_uart7_tx.Instance = DMA2_Stream2;
hdma_uart7_tx.Init.Request = DMA_REQUEST_UART7_TX;
hdma_uart7_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_uart7_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_uart7_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_uart7_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_uart7_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_uart7_tx.Init.Mode = DMA_NORMAL;
hdma_uart7_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
hdma_uart7_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_uart7_tx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmatx,hdma_uart7_tx);
/* UART7 interrupt Init */
HAL_NVIC_SetPriority(UART7_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(UART7_IRQn);
@ -648,7 +688,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
hdma_usart10_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart10_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart10_tx.Init.Mode = DMA_NORMAL;
hdma_usart10_tx.Init.Priority = DMA_PRIORITY_LOW;
hdma_usart10_tx.Init.Priority = DMA_PRIORITY_VERY_HIGH;
hdma_usart10_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_usart10_tx) != HAL_OK)
{
@ -657,6 +697,24 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
__HAL_LINKDMA(uartHandle,hdmatx,hdma_usart10_tx);
/* USART10_RX Init */
hdma_usart10_rx.Instance = DMA2_Stream3;
hdma_usart10_rx.Init.Request = DMA_REQUEST_USART10_RX;
hdma_usart10_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_usart10_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart10_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart10_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart10_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart10_rx.Init.Mode = DMA_NORMAL;
hdma_usart10_rx.Init.Priority = DMA_PRIORITY_VERY_HIGH;
hdma_usart10_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_usart10_rx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart10_rx);
/* USART10 interrupt Init */
HAL_NVIC_SetPriority(USART10_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART10_IRQn);
@ -708,6 +766,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
*/
HAL_GPIO_DeInit(GPIOE, GPIO_PIN_7|GPIO_PIN_8);
/* UART7 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmarx);
HAL_DMA_DeInit(uartHandle->hdmatx);
/* UART7 interrupt Deinit */
HAL_NVIC_DisableIRQ(UART7_IRQn);
/* USER CODE BEGIN UART7_MspDeInit 1 */
@ -802,6 +864,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
/* USART10 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmatx);
HAL_DMA_DeInit(uartHandle->hdmarx);
/* USART10 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART10_IRQn);

74
CtrBoard-H7_ALL.ioc
View File

@ -44,6 +44,8 @@ Dma.ADC1.0.SyncRequestNumber=1
Dma.ADC1.0.SyncSignalID=NONE
Dma.Request0=ADC1
Dma.Request1=UART5_RX
Dma.Request10=UART7_TX
Dma.Request11=USART10_RX
Dma.Request2=SPI2_RX
Dma.Request3=SPI2_TX
Dma.Request4=USART3_RX
@ -51,7 +53,8 @@ Dma.Request5=USART3_TX
Dma.Request6=USART2_RX
Dma.Request7=USART2_TX
Dma.Request8=USART10_TX
Dma.RequestsNb=9
Dma.Request9=UART7_RX
Dma.RequestsNb=12
Dma.SPI2_RX.2.Direction=DMA_PERIPH_TO_MEMORY
Dma.SPI2_RX.2.EventEnable=DISABLE
Dma.SPI2_RX.2.FIFOMode=DMA_FIFOMODE_DISABLE
@ -62,7 +65,7 @@ Dma.SPI2_RX.2.Mode=DMA_NORMAL
Dma.SPI2_RX.2.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.SPI2_RX.2.PeriphInc=DMA_PINC_DISABLE
Dma.SPI2_RX.2.Polarity=HAL_DMAMUX_REQ_GEN_RISING
Dma.SPI2_RX.2.Priority=DMA_PRIORITY_LOW
Dma.SPI2_RX.2.Priority=DMA_PRIORITY_VERY_HIGH
Dma.SPI2_RX.2.RequestNumber=1
Dma.SPI2_RX.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
Dma.SPI2_RX.2.SignalID=NONE
@ -80,7 +83,7 @@ Dma.SPI2_TX.3.Mode=DMA_NORMAL
Dma.SPI2_TX.3.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.SPI2_TX.3.PeriphInc=DMA_PINC_DISABLE
Dma.SPI2_TX.3.Polarity=HAL_DMAMUX_REQ_GEN_RISING
Dma.SPI2_TX.3.Priority=DMA_PRIORITY_LOW
Dma.SPI2_TX.3.Priority=DMA_PRIORITY_VERY_HIGH
Dma.SPI2_TX.3.RequestNumber=1
Dma.SPI2_TX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
Dma.SPI2_TX.3.SignalID=NONE
@ -106,6 +109,60 @@ Dma.UART5_RX.1.SyncEnable=DISABLE
Dma.UART5_RX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.UART5_RX.1.SyncRequestNumber=1
Dma.UART5_RX.1.SyncSignalID=NONE
Dma.UART7_RX.9.Direction=DMA_PERIPH_TO_MEMORY
Dma.UART7_RX.9.EventEnable=DISABLE
Dma.UART7_RX.9.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.UART7_RX.9.Instance=DMA2_Stream1
Dma.UART7_RX.9.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.UART7_RX.9.MemInc=DMA_MINC_ENABLE
Dma.UART7_RX.9.Mode=DMA_NORMAL
Dma.UART7_RX.9.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.UART7_RX.9.PeriphInc=DMA_PINC_DISABLE
Dma.UART7_RX.9.Polarity=HAL_DMAMUX_REQ_GEN_RISING
Dma.UART7_RX.9.Priority=DMA_PRIORITY_MEDIUM
Dma.UART7_RX.9.RequestNumber=1
Dma.UART7_RX.9.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
Dma.UART7_RX.9.SignalID=NONE
Dma.UART7_RX.9.SyncEnable=DISABLE
Dma.UART7_RX.9.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.UART7_RX.9.SyncRequestNumber=1
Dma.UART7_RX.9.SyncSignalID=NONE
Dma.UART7_TX.10.Direction=DMA_MEMORY_TO_PERIPH
Dma.UART7_TX.10.EventEnable=DISABLE
Dma.UART7_TX.10.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.UART7_TX.10.Instance=DMA2_Stream2
Dma.UART7_TX.10.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.UART7_TX.10.MemInc=DMA_MINC_ENABLE
Dma.UART7_TX.10.Mode=DMA_NORMAL
Dma.UART7_TX.10.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.UART7_TX.10.PeriphInc=DMA_PINC_DISABLE
Dma.UART7_TX.10.Polarity=HAL_DMAMUX_REQ_GEN_RISING
Dma.UART7_TX.10.Priority=DMA_PRIORITY_MEDIUM
Dma.UART7_TX.10.RequestNumber=1
Dma.UART7_TX.10.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
Dma.UART7_TX.10.SignalID=NONE
Dma.UART7_TX.10.SyncEnable=DISABLE
Dma.UART7_TX.10.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.UART7_TX.10.SyncRequestNumber=1
Dma.UART7_TX.10.SyncSignalID=NONE
Dma.USART10_RX.11.Direction=DMA_PERIPH_TO_MEMORY
Dma.USART10_RX.11.EventEnable=DISABLE
Dma.USART10_RX.11.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.USART10_RX.11.Instance=DMA2_Stream3
Dma.USART10_RX.11.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.USART10_RX.11.MemInc=DMA_MINC_ENABLE
Dma.USART10_RX.11.Mode=DMA_NORMAL
Dma.USART10_RX.11.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART10_RX.11.PeriphInc=DMA_PINC_DISABLE
Dma.USART10_RX.11.Polarity=HAL_DMAMUX_REQ_GEN_RISING
Dma.USART10_RX.11.Priority=DMA_PRIORITY_VERY_HIGH
Dma.USART10_RX.11.RequestNumber=1
Dma.USART10_RX.11.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
Dma.USART10_RX.11.SignalID=NONE
Dma.USART10_RX.11.SyncEnable=DISABLE
Dma.USART10_RX.11.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.USART10_RX.11.SyncRequestNumber=1
Dma.USART10_RX.11.SyncSignalID=NONE
Dma.USART10_TX.8.Direction=DMA_MEMORY_TO_PERIPH
Dma.USART10_TX.8.EventEnable=DISABLE
Dma.USART10_TX.8.FIFOMode=DMA_FIFOMODE_DISABLE
@ -116,7 +173,7 @@ Dma.USART10_TX.8.Mode=DMA_NORMAL
Dma.USART10_TX.8.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART10_TX.8.PeriphInc=DMA_PINC_DISABLE
Dma.USART10_TX.8.Polarity=HAL_DMAMUX_REQ_GEN_RISING
Dma.USART10_TX.8.Priority=DMA_PRIORITY_LOW
Dma.USART10_TX.8.Priority=DMA_PRIORITY_VERY_HIGH
Dma.USART10_TX.8.RequestNumber=1
Dma.USART10_TX.8.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
Dma.USART10_TX.8.SignalID=NONE
@ -395,6 +452,9 @@ NVIC.DMA1_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.FDCAN1_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@ -777,10 +837,10 @@ SPI1.Direction=SPI_DIRECTION_2LINES_TXONLY
SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPolarity
SPI1.Mode=SPI_MODE_MASTER
SPI1.VirtualType=VM_MASTER
SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_32
SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_8
SPI2.CLKPhase=SPI_PHASE_2EDGE
SPI2.CLKPolarity=SPI_POLARITY_HIGH
SPI2.CalculateBaudRate=3.75 MBits/s
SPI2.CalculateBaudRate=15.0 MBits/s
SPI2.DataSize=SPI_DATASIZE_8BIT
SPI2.Direction=SPI_DIRECTION_2LINES
SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPolarity,CLKPhase
@ -820,7 +880,7 @@ UART7.IPParameters=BaudRate
USART1.BaudRate=921600
USART1.IPParameters=VirtualMode-Asynchronous,BaudRate
USART1.VirtualMode-Asynchronous=VM_ASYNC
USART10.BaudRate=115200
USART10.BaudRate=921600
USART10.IPParameters=VirtualMode,BaudRate
USART10.VirtualMode=VM_ASYNC
USART2.BaudRate=4000000

3109
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

File diff suppressed because it is too large Load Diff

243
MDK-ARM/CtrBoard-H7_ALL.uvprojx
View File

@ -1,7 +1,10 @@
<?xml version="1.0" encoding="UTF-8"?>
<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" noNamespaceSchemaLocation="project_projx.xsd">
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_projx.xsd">
<SchemaVersion>2.1</SchemaVersion>
<Header>### uVision Project, (C) Keil Software</Header>
<Targets>
<Target>
<TargetName>CtrBoard-H7_ALL</TargetName>
@ -16,28 +19,28 @@
<PackID>Keil.STM32H7xx_DFP.4.0.0</PackID>
<PackURL>https://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x24000000-0x2401FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(12000000) FPU3(DFPU) CPUTYPE("Cortex-M7") ELITTLE TZ</Cpu>
<FlashUtilSpec />
<StartupFile />
<FlashDriverDll />
<FlashUtilSpec></FlashUtilSpec>
<StartupFile></StartupFile>
<FlashDriverDll></FlashDriverDll>
<DeviceId>0</DeviceId>
<RegisterFile />
<MemoryEnv />
<Cmp />
<Asm />
<Linker />
<OHString />
<InfinionOptionDll />
<SLE66CMisc />
<SLE66AMisc />
<SLE66LinkerMisc />
<RegisterFile></RegisterFile>
<MemoryEnv></MemoryEnv>
<Cmp></Cmp>
<Asm></Asm>
<Linker></Linker>
<OHString></OHString>
<InfinionOptionDll></InfinionOptionDll>
<SLE66CMisc></SLE66CMisc>
<SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>$$Device:STM32H723VGTx$CMSIS\SVD\STM32H723.svd</SFDFile>
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv>
<BinPath />
<IncludePath />
<LibPath />
<RegisterFilePath />
<DBRegisterFilePath />
<BinPath></BinPath>
<IncludePath></IncludePath>
<LibPath></LibPath>
<RegisterFilePath></RegisterFilePath>
<DBRegisterFilePath></DBRegisterFilePath>
<TargetStatus>
<Error>0</Error>
<ExitCodeStop>0</ExitCodeStop>
@ -52,15 +55,15 @@
<CreateHexFile>1</CreateHexFile>
<DebugInformation>1</DebugInformation>
<BrowseInformation>1</BrowseInformation>
<ListingPath />
<ListingPath></ListingPath>
<HexFormatSelection>1</HexFormatSelection>
<Merge32K>0</Merge32K>
<CreateBatchFile>0</CreateBatchFile>
<BeforeCompile>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name />
<UserProg2Name />
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopU1X>0</nStopU1X>
@ -69,8 +72,8 @@
<BeforeMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name />
<UserProg2Name />
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopB1X>0</nStopB1X>
@ -78,16 +81,16 @@
</BeforeMake>
<AfterMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>1</RunUserProg2>
<UserProg1Name />
<UserProg2Name />
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopA1X>0</nStopA1X>
<nStopA2X>0</nStopA2X>
</AfterMake>
<SelectedForBatchBuild>1</SelectedForBatchBuild>
<SVCSIdString />
<SVCSIdString></SVCSIdString>
</TargetCommonOption>
<CommonProperty>
<UseCPPCompiler>0</UseCPPCompiler>
@ -101,8 +104,8 @@
<AssembleAssemblyFile>0</AssembleAssemblyFile>
<PublicsOnly>0</PublicsOnly>
<StopOnExitCode>3</StopOnExitCode>
<CustomArgument />
<IncludeLibraryModules />
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>0</ComprImg>
</CommonProperty>
<DllOption>
@ -135,11 +138,11 @@
</Flash1>
<bUseTDR>1</bUseTDR>
<Flash2>BIN\UL2V8M.DLL</Flash2>
<Flash3 />
<Flash4 />
<pFcarmOut />
<pFcarmGrp />
<pFcArmRoot />
<Flash3></Flash3>
<Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
<pFcArmRoot></pFcArmRoot>
<FcArmLst>0</FcArmLst>
</Utilities>
<TargetArmAds>
@ -172,7 +175,7 @@
<RvctClst>0</RvctClst>
<GenPPlst>0</GenPPlst>
<AdsCpuType>"Cortex-M7"</AdsCpuType>
<RvctDeviceName />
<RvctDeviceName></RvctDeviceName>
<mOS>0</mOS>
<uocRom>0</uocRom>
<uocRam>0</uocRam>
@ -306,7 +309,7 @@
<Size>0x20000</Size>
</OCR_RVCT10>
</OnChipMemories>
<RvctStartVector />
<RvctStartVector></RvctStartVector>
</ArmAdsMisc>
<Cads>
<interw>1</interw>
@ -333,9 +336,9 @@
<v6WtE>0</v6WtE>
<v6Rtti>0</v6Rtti>
<VariousControls>
<MiscControls />
<MiscControls></MiscControls>
<Define>USE_PWR_LDO_SUPPLY,USE_HAL_DRIVER,STM32H723xx</Define>
<Undefine />
<Undefine></Undefine>
<IncludePath>../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../User;../Middlewares/Third_Party/Unitree</IncludePath>
</VariousControls>
</Cads>
@ -351,9 +354,9 @@
<useXO>0</useXO>
<ClangAsOpt>1</ClangAsOpt>
<VariousControls>
<MiscControls />
<Define />
<Undefine />
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath>../Drivers/CMSIS/Include</IncludePath>
</VariousControls>
</Aads>
@ -364,15 +367,15 @@
<noStLib>0</noStLib>
<RepFail>1</RepFail>
<useFile>0</useFile>
<TextAddressRange />
<DataAddressRange />
<pXoBase />
<TextAddressRange></TextAddressRange>
<DataAddressRange></DataAddressRange>
<pXoBase></pXoBase>
<ScatterFile>CtrBoard-H7_ALL\CtrBoard-H7_ALL.sct</ScatterFile>
<IncludeLibs />
<IncludeLibsPath />
<Misc />
<LinkerInputFile />
<DisabledWarnings />
<IncludeLibs></IncludeLibs>
<IncludeLibsPath></IncludeLibsPath>
<Misc></Misc>
<LinkerInputFile></LinkerInputFile>
<DisabledWarnings></DisabledWarnings>
</LDads>
</TargetArmAds>
</TargetOption>
@ -417,6 +420,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -444,6 +449,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -735,6 +746,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<GroupArmAds>
@ -762,6 +775,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
<Aads>
<interw>2</interw>
@ -774,6 +793,12 @@
<uSurpInc>2</uSurpInc>
<useXO>2</useXO>
<ClangAsOpt>1</ClangAsOpt>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Aads>
</GroupArmAds>
</GroupOption>
@ -795,6 +820,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -822,6 +849,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -843,6 +876,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -870,6 +905,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -891,6 +932,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -918,6 +961,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -939,6 +988,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -966,6 +1017,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -987,6 +1044,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -1014,6 +1073,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -1035,6 +1100,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -1062,6 +1129,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -1083,6 +1156,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -1110,6 +1185,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -1131,6 +1212,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -1158,6 +1241,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -1179,6 +1268,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -1206,6 +1297,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -1227,6 +1324,8 @@
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
@ -1254,6 +1353,12 @@
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
@ -1339,9 +1444,19 @@
<FilePath>..\User\component\FreeRTOS_CLI.c</FilePath>
</File>
<File>
<FileName>kinematics.c</FileName>
<FileName>crc8.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\kinematics.c</FilePath>
<FilePath>..\User\component\crc8.c</FilePath>
</File>
<File>
<FileName>crc16.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\crc16.c</FilePath>
</File>
<File>
<FileName>limiter.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\limiter.c</FilePath>
</File>
</Files>
</Group>
@ -1363,11 +1478,21 @@
<FileType>1</FileType>
<FilePath>..\User\device\go.c</FilePath>
</File>
<File>
<FileName>n100.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\n100.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/module</GroupName>
<Files>
<File>
<FileName>kinematics.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\kinematics.c</FilePath>
</File>
<File>
<FileName>chassis.c</FileName>
<FileType>1</FileType>
@ -1461,18 +1586,20 @@
</Groups>
</Target>
</Targets>
<RTE>
<apis />
<apis/>
<components>
<component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="4.3.0" condition="CMSIS Core">
<package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.5.0" />
<package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.5.0"/>
<targetInfos>
<targetInfo name="CtrBoard-H7_ALL" />
<targetInfo name="CtrBoard-H7_ALL"/>
</targetInfos>
</component>
</components>
<files />
<files/>
</RTE>
<LayerInfo>
<Layers>
<Layer>
@ -1481,5 +1608,5 @@
</Layer>
</Layers>
</LayerInfo>
</Project>
</Project>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

8598
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

4
User/bsp/uart.c
View File

@ -16,6 +16,8 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
if (huart->Instance == USART3)
return BSP_UART_RIGHT_LEG;
if (huart->Instance == USART10)
return BSP_UART_IMU_N100;
if (huart->Instance == UART7)
return BSP_UART_AI;
// else if (huart->Instance == USARTX)
// return BSP_UART_XXX;
@ -114,6 +116,8 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
case BSP_UART_RIGHT_LEG:
return &huart3;
case BSP_UART_AI:
return &huart7;
case BSP_UART_IMU_N100:
return &huart10;
// case BSP_UART_XXX:
// return &huartX;

1
User/bsp/uart.h
View File

@ -20,6 +20,7 @@ typedef enum {
BSP_UART_DR16,
BSP_UART_LEFT_LEG,
BSP_UART_RIGHT_LEG,
BSP_UART_IMU_N100,
BSP_UART_AI,
/*BSP_UART_XXX*/
BSP_UART_NUM,

4
User/component/cmd.c
View File

@ -53,8 +53,8 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
break;
}
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
cmd->chassis.ctrl_vec.vx = rc->ch_l_y;
cmd->chassis.ctrl_vec.vy = rc->ch_l_x;
cmd->chassis.ctrl_vec.wz = rc->ch_r_x;
cmd->chassis.delta_hight = rc->ch_res;

35
User/component/crc16.c Normal file
View File

@ -0,0 +1,35 @@
/*
N100陀螺仪包头crc16校验
*/
#include <stdint.h>
// CRC16 查找表（多项式 0x1021，初始值 0x0000）
static const uint16_t CRC16Table[256] = {
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};
// 计算 CRC16 校验值
uint16_t crc16_calc(const uint8_t *data, uint8_t len) {
uint16_t crc = 0x0000;
for (uint8_t i = 0; i < len; i++) {
uint8_t index = (crc >> 8) ^ data[i];
crc = (crc << 8) ^ CRC16Table[index];
}
return crc;
}

17
User/component/crc16.h Normal file
View File

@ -0,0 +1,17 @@
/*
N100陀螺仪包头crc16校验
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
uint16_t crc16_calc(const uint8_t *data, uint8_t len);
#ifdef __cplusplus
}
#endif

33
User/component/crc8.c Normal file
View File

@ -0,0 +1,33 @@
/*
N100陀螺仪包头crc8校验
*/
#include <stdint.h>
#include "crc8.h"
// CRC8 查找表（多项式 0x07，初始值 0x00）
static const uint8_t CRC8Table[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,
};
// 计算 CRC8 校验值
uint8_t crc8_calc(const uint8_t *data, uint8_t len) {
uint8_t crc = 0x00;
for (uint8_t i = 0; i < len; i++) {
crc = CRC8Table[crc ^ data[i]];
}
return crc;
}

17
User/component/crc8.h Normal file
View File

@ -0,0 +1,17 @@
/*
N100陀螺仪包头crc8校验
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
uint8_t crc8_calc(const uint8_t *data, uint8_t len);
#ifdef __cplusplus
}
#endif

117
User/component/kinematics.c
View File

@ -3,9 +3,35 @@
*/
#include "filter.h"
#include <math.h>
#include "kinematics.h"
static float q1_ik(float py, float pz, float l1) {
float q1;
float L = sqrtf(py*py + pz*pz - l1*l1);
q1 = atan2f(pz*l1 + py*L, py*l1 - pz*L);
return q1;
}
static float q3_ik(float b3z, float b4z, float b) {
float q3, temp;
temp = (b3z*b3z + b4z*b4z - b*b) / (2.0f * fabsf(b3z * b4z));
if (temp > 1.0f) temp = 1.0f;
if (temp < -1.0f) temp = -1.0f;
q3 = acosf(temp);
q3 = -(M_PI - q3);
return q3;
}
static float q2_ik(float q1, float q3, float px, float py, float pz, float b3z, float b4z) {
float a1 = py * sinf(q1) - pz * cosf(q1);
float a2 = px;
float m1 = b4z * sinf(q3);
float m2 = b3z + b4z * cosf(q3);
float q2 = atan2f(m1 * a1 + m2 * a2, m1 * a2 - m2 * a1);
return q2;
}
int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_JointData_t *in, float ratio, float joint_zero){
real->Pos = (in->Pos-joint_zero) / ratio;
@ -23,27 +49,30 @@ int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_Joint
return 0;
}
/**
* @brief 单腿正运动学
* @param theta[3] 关节角度数组（单位：弧度）：[hip, thigh, calf]
* @param leg_params 单腿机械参数（长度）
* @param foot_out 足端位置输出 [x, y, z]（单位：米）
*/
// void Leg_ForwardKinematics(const float theta[3], const joint_params *leg_params, float foot_out[3]) {
// float l1 = leg_params->named.lf_hip; // 髋关节到大腿长度
// float l2 = leg_params->named.lf_thigh; // 大腿长度
// float l3 = leg_params->named.lf_calf; // 小腿长度
void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3], Kinematics_Sign_t *sign) {
float l1 = leg_params->hip * sign->hip;
float l2 = -leg_params->thigh;
float l3 = -leg_params->calf;
// float t1 = theta[0]; // 髋关节侧摆（绕y轴）
// float t2 = theta[1]; // 髋关节前后
// float t3 = theta[2]; // 膝关节
float q1 = theta_in[0];
float q2 = theta_in[1] * sign->thigh;
float q3 = theta_in[2] * sign->calf;
// // 末端位置
// float leg_proj = l2 * cosf(t2) + l3 * cosf(t2 + t3);
// foot_out[0] = cosf(t1) * leg_proj + l1 * cosf(t1); // x
// foot_out[1] = sinf(t1) * leg_proj + l1 * sinf(t1); // y
// foot_out[2] = l2 * sinf(t2) + l3 * sinf(t2 + t3); // z
// }
float s1 = sinf(q1);
float s2 = sinf(q2);
float s3 = sinf(q3);
float c1 = cosf(q1);
float c2 = cosf(q2);
float c3 = cosf(q3);
float c23 = c2 * c3 - s2 * s3;
float s23 = s2 * c3 + c2 * s3;
foot_out[0] = l3 * s23 + l2 * s2;
foot_out[1] = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
foot_out[2] = l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
}
/**
* @brief 单腿逆运动学
@ -52,34 +81,28 @@ int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_Joint
* @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
* @return 0成功，-1目标不可达
*/
// int Leg_InverseKinematics(const float foot_in[3], const joint_params *leg_params, float theta_out[3]) {
// float l1 = leg_params->named.lf_hip;
// float l2 = leg_params->named.lf_thigh;
// float l3 = leg_params->named.lf_calf;
int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_LinkLength_t *leg_params, float theta_out[3], Kinematics_Sign_t *sign) {
float px = foot_in[0];
float py = foot_in[1];
float pz = foot_in[2];
// float x = foot_in[0];
// float y = foot_in[1];
// float z = foot_in[2];
float b2y = leg_params->hip * sign->hip;
float b3z = -leg_params->thigh;
float b4z = -leg_params->calf;
float a = leg_params->hip;
float c = sqrtf(px*px + py*py + pz*pz);
float b = sqrtf(c*c - a*a);
// // 1. 先算髋关节侧摆角（绕y轴）
// float r = sqrtf(x * x + y * y);
// float t1 = atan2f(y, x); // 髋关节侧摆角：使用 atan2f，计
// 检查可达性
if (b != b) return -1; // NaN
if (b > fabsf(b3z) + fabsf(b4z)) return -1;
// // 2. 去掉髋关节侧摆影响，得到在sagittal平面上的投影
// float xp = r - l1;
// float zp = z;
float q1 = q1_ik(py, pz, b2y);
float q3 = q3_ik(b3z, b4z, b);
float q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z);
// // 3. 逆解余下两个关节
// float D = (xp * xp + zp * zp - l2 * l2 - l3 * l3) / (2 * l2 * l3);
// if (D < -1.0f || D > 1.0f) return -1; // 不可达
// float t3 = atan2f(-sqrtf(1 - D * D), D); // 膝关节
// float k1 = l2 + l3 * cosf(t3);
// float k2 = l3 * sinf(t3);
// float t2 = atan2f(zp, xp) - atan2f(k2, k1);
// theta_out[0] = t1;
// theta_out[1] = t2;
// theta_out[2] = t3;
// return 0;
// }
theta_out[0] = q1;
theta_out[1] = q2 * sign->thigh;
theta_out[2] = q3 * sign->calf;
return 0;
}

37
User/component/kinematics.h
View File

@ -24,6 +24,24 @@ typedef struct {
float K_W; // 关节速度系数(0-25.599)
} Kinematics_JointCMD_t;
typedef struct {
float hip; // 髋关节长度(m)
float thigh; // 大腿长度(m)
float calf; // 小腿长度(m)
} Kinematics_LinkLength_t;
typedef struct {
float x;
float y;
float z;
} Kinematics_LegOffset_t;
typedef struct {
float hip;
float thigh;
float calf;
} Kinematics_Sign_t;
/**
* @brief 计算实际反馈
@ -44,6 +62,25 @@ int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_Jo
int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_JointCMD_t *out, float ratio, float joint_zero);
/**
* @brief 单腿正运动学
* @param theta_in 关节角度输入数组（单位：弧度）：[hip, thigh, calf]
* @param leg_params 单腿机械参数（长度）
* @param foot_out 足端位置输出 [x, y, z]（单位：米）
* @param side_sign 侧向标志（左前/左后为-1，右前/右后为1）
*/
void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3], Kinematics_Sign_t *sign);
/**
* @brief 单腿逆运动学
* @param foot_in 足端目标位置 [x, y, z]（单位：米）
* @param leg_params 单腿机械参数（长度）
* @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
* @param side_sign 侧向标志（左前/左后为-1，右前/右后为1）
* @return 0成功，-1目标不可达
*/
int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_LinkLength_t *leg_params, float theta_out[3], Kinematics_Sign_t *sign);
#ifdef __cplusplus
}

27
User/component/limiter.c Normal file
View File

@ -0,0 +1,27 @@
/*
限制器
*/
#include "limiter.h"
#include <math.h>
#include <stddef.h>
#include "component/user_math.h"
int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, float max_speed, float max_angle, float min_angle){
// 限制位置变化速度
if (*out_pos - feedback_pos > max_speed) {
*out_pos = feedback_pos + max_speed;
} else if (*out_pos - feedback_pos < -max_speed) {
*out_pos = feedback_pos - max_speed;
}
// 限制角度范围
if (*out_pos > max_angle) {
*out_pos = max_angle;
} else if (*out_pos < min_angle) {
*out_pos = min_angle;
}
return 0; // 成功
}

20
User/component/limiter.h Normal file
View File

@ -0,0 +1,20 @@
/*
限制器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos,
float max_speed, float max_angle, float min_angle);
#ifdef __cplusplus
}
#endif

175
User/component/unitreeLeg.cpp Executable file
View File

@ -0,0 +1,175 @@
/**********************************************************************
Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/
#include "common/unitreeLeg.h"
#include <iostream>
/************************/
/*******QuadrupedLeg*****/
/************************/
QuadrupedLeg::QuadrupedLeg(int legID, float abadLinkLength, float hipLinkLength,
float kneeLinkLength, Vec3 pHip2B)
:_abadLinkLength(abadLinkLength),
_hipLinkLength(hipLinkLength),
_kneeLinkLength(kneeLinkLength),
_pHip2B(pHip2B){
if (legID == 0 || legID == 2)
_sideSign = -1;
else if (legID == 1 || legID == 3)
_sideSign = 1;
else{
std::cout << "Leg ID incorrect!" << std::endl;
exit(-1);
}
}
// Forward Kinematics
Vec3 QuadrupedLeg::calcPEe2H(Vec3 q){
float l1 = _sideSign * _abadLinkLength;
float l2 = -_hipLinkLength;
float l3 = -_kneeLinkLength;
float s1 = std::sin(q(0));
float s2 = std::sin(q(1));
float s3 = std::sin(q(2));
float c1 = std::cos(q(0));
float c2 = std::cos(q(1));
float c3 = std::cos(q(2));
float c23 = c2 * c3 - s2 * s3;
float s23 = s2 * c3 + c2 * s3;
Vec3 pEe2H;
pEe2H(0) = l3 * s23 + l2 * s2;
pEe2H(1) = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
pEe2H(2) = l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
return pEe2H;
}
// Forward Kinematics
Vec3 QuadrupedLeg::calcPEe2B(Vec3 q){
return _pHip2B + calcPEe2H(q);
}
// Derivative Forward Kinematics
Vec3 QuadrupedLeg::calcVEe(Vec3 q, Vec3 qd){
return calcJaco(q) * qd;
}
// Inverse Kinematics
Vec3 QuadrupedLeg::calcQ(Vec3 pEe, FrameType frame){
Vec3 pEe2H;
if(frame == FrameType::HIP)
pEe2H = pEe;
else if(frame == FrameType::BODY)
pEe2H = pEe - _pHip2B;
else{
std::cout << "[ERROR] The frame of QuadrupedLeg::calcQ can only be HIP or BODY!" << std::endl;
exit(-1);
}
float q1, q2, q3;
Vec3 qResult;
float px, py, pz;
float b2y, b3z, b4z, a, b, c;
px = pEe2H(0);
py = pEe2H(1);
pz = pEe2H(2);
b2y = _abadLinkLength * _sideSign;
b3z = -_hipLinkLength;
b4z = -_kneeLinkLength;
a = _abadLinkLength;
c = sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)); // whole length
b = sqrt(pow(c, 2) - pow(a, 2)); // distance between shoulder and footpoint
q1 = q1_ik(py, pz, b2y);
q3 = q3_ik(b3z, b4z, b);
q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z);
qResult(0) = q1;
qResult(1) = q2;
qResult(2) = q3;
return qResult;
}
// Derivative Inverse Kinematics
Vec3 QuadrupedLeg::calcQd(Vec3 q, Vec3 vEe){
return calcJaco(q).inverse() * vEe;
}
// Derivative Inverse Kinematics
Vec3 QuadrupedLeg::calcQd(Vec3 pEe, Vec3 vEe, FrameType frame){
Vec3 q = calcQ(pEe, frame);
return calcJaco(q).inverse() * vEe;
}
// Inverse Dynamics
Vec3 QuadrupedLeg::calcTau(Vec3 q, Vec3 force){
return calcJaco(q).transpose() * force;
}
// Jacobian Matrix
Mat3 QuadrupedLeg::calcJaco(Vec3 q){
Mat3 jaco;
float l1 = _abadLinkLength * _sideSign;
float l2 = -_hipLinkLength;
float l3 = -_kneeLinkLength;
float s1 = std::sin(q(0));
float s2 = std::sin(q(1));
float s3 = std::sin(q(2));
float c1 = std::cos(q(0));
float c2 = std::cos(q(1));
float c3 = std::cos(q(2));
float c23 = c2 * c3 - s2 * s3;
float s23 = s2 * c3 + c2 * s3;
jaco(0, 0) = 0;
jaco(1, 0) = -l3 * c1 * c23 - l2 * c1 * c2 - l1 * s1;
jaco(2, 0) = -l3 * s1 * c23 - l2 * c2 * s1 + l1 * c1;
jaco(0, 1) = l3 * c23 + l2 * c2;
jaco(1, 1) = l3 * s1 * s23 + l2 * s1 * s2;
jaco(2, 1) = -l3 * c1 * s23 - l2 * c1 * s2;
jaco(0, 2) = l3 * c23;
jaco(1, 2) = l3 * s1 * s23;
jaco(2, 2) = -l3 * c1 * s23;
return jaco;
}
float QuadrupedLeg::q1_ik(float py, float pz, float l1){
float q1;
float L = sqrt(pow(py,2)+pow(pz,2)-pow(l1,2));
q1 = atan2(pz*l1+py*L, py*l1-pz*L);
return q1;
}
float QuadrupedLeg::q3_ik(float b3z, float b4z, float b){
float q3, temp;
temp = (pow(b3z, 2) + pow(b4z, 2) - pow(b, 2))/(2*fabs(b3z*b4z));
if(temp>1) temp = 1;
if(temp<-1) temp = -1;
q3 = acos(temp);
q3 = -(M_PI - q3); //0~180
return q3;
}
float QuadrupedLeg::q2_ik(float q1, float q3, float px, float py, float pz, float b3z, float b4z){
float q2, a1, a2, m1, m2;
a1 = py*sin(q1) - pz*cos(q1);
a2 = px;
m1 = b4z*sin(q3);
m2 = b3z + b4z*cos(q3);
q2 = atan2(m1*a1+m2*a2, m1*a2-m2*a1);
return q2;
}

1
User/component/user_math.c
View File

@ -130,3 +130,4 @@ void VerifyFailed(const char *file, uint32_t line) {
__NOP();
}
}

152
User/device/ai.c Normal file
View File

@ -0,0 +1,152 @@
/*
AI
*/
/* Includes ----------------------------------------------------------------- */
#include "ai.h"
#include <string.h>
#include "bsp\delay.h"
#include "bsp\uart.h"
#include "component\crc16.h"
#include "component\crc8.h"
#include "component\user_math.h"
#include "component\filter.h"
/* Private define ----------------------------------------------------------- */
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;
static uint8_t rxbuf[AI_LEN_RX_BUFF];
static bool inited = false;
static osThreadId_t thread_alert;
/* Private function -------------------------------------------------------- */
static void Ai_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
static void Ai_IdleLineCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */
int8_t AI_Init(AI_t *ai) {
UNUSED(ai);
ASSERT(ai);
if (inited) return DEVICE_ERR_INITED;
VERIFY((thread_alert = osThreadGetId()) != NULL);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
Ai_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
Ai_IdleLineCallback);
inited = true;
return 0;
}
int8_t AI_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
return DEVICE_OK;
}
int8_t AI_StartReceiving(AI_t *ai) {
UNUSED(ai);
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
AI_LEN_RX_BUFF) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool AI_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
SIGNAL_AI_RAW_REDY);
}
int8_t AI_ParseHost(AI_t *ai) {
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
goto error;
ai->ai_online = true;
memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
memset(rxbuf, 0, AI_LEN_RX_BUFF);
return DEVICE_OK;
error:
drop_message++;
return DEVICE_ERR;
}
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
}
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
if (ai == NULL) return DEVICE_ERR_NULL;
if (cmd_host == NULL) return DEVICE_ERR_NULL;
ai->ai_online = false;
memset(&(ai->from_host), 0, sizeof(ai->from_host));
memset(cmd_host, 0, sizeof(*cmd_host));
return 0;
}
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
ai->to_host.mcu.id = AI_ID_MCU;
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
sizeof(*quat));
ai->to_host.mcu.package.data.notice = 0;
if (ai->status == AI_STATUS_AUTOAIM)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
else if (ai->status == AI_STATUS_HITSWITCH)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
else if (ai->status == AI_STATUS_AUTOMATIC)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
ai->to_host.mcu.package.crc16 = CRC16_Calc(
(const uint8_t *)&(ai->to_host.mcu.package),
sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
(void)ref;
ai->to_host.ref.id = AI_ID_REF;
ai->to_host.ref.package.crc16 = CRC16_Calc(
(const uint8_t *)&(ai->to_host.ref.package),
sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_StartSend(AI_t *ai, bool ref_update) {
if (ref_update) {
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
} else {
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
(uint8_t *)&(ai->to_host.mcu),
sizeof(ai->to_host.mcu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
}

69
User/device/ai.h Normal file
View File

@ -0,0 +1,69 @@
/*
AI
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
#include "component\ahrs.h"
#include "component\cmd.h"
#include "component\user_math.h"
#include "component\filter.h"
#include "device\device.h"
#include "device\referee.h"
#include "protocol.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint8_t id;
Protocol_UpPackageReferee_t package;
} AI_UpPackageReferee_t;
typedef struct __packed {
uint8_t id;
Protocol_UpPackageMCU_t package;
} AI_UpPackageMCU_t;
typedef struct __packed {
osThreadId_t thread_alert;
Protocol_DownPackage_t from_host;
struct {
AI_UpPackageReferee_t ref;
AI_UpPackageMCU_t mcu;
} to_host;
CMD_AI_Status_t status;
bool ai_online;
} AI_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t AI_Init(AI_t *ai);
int8_t AI_Restart(void);
int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void);
int8_t AI_ParseHost(AI_t *ai);
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
int8_t AI_StartSend(AI_t *ai, bool option);
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
#ifdef __cplusplus
}
#endif

3
User/device/bmi088.c
View File

@ -365,5 +365,6 @@ int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
(void)bmi088;
return 400.0f;
return 200.0f;
}

13
User/device/device.h
View File

@ -19,19 +19,12 @@ extern "C" {
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)
#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 3)
#define SIGNAL_DR16_RAW_REDY (1u << 4)
#define SIGNAL_N100_NEW_DATA (1u << 4)
#define SIGNAL_DR16_RAW_REDY (1u << 5)
#define SIGNAL_GO_MOTOR_RECV (1u << 5)
#define SIGNAL_GO_MOTOR_RECV (1u << 6)
#define SIGNAL_IST8310_MAGN_NEW_DATA (1u << 8)
#define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
#define SIGNAL_REFEREE_RAW_REDY (1u << 10)
#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 11)
#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 12)
#define SIGNAL_AI_RAW_REDY (1u << 14)
#ifdef __cplusplus

118
User/device/go copy.c
View File

@ -1,118 +0,0 @@
/*
四足机器人12个关节驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "bsp/uart.h"
#include "go.h"
#include <stdbool.h>
#include <string.h>
#include "component/user_math.h"
#include "cmsis_os2.h"
static osSemaphoreId_t sem_left_rx = NULL;
static osSemaphoreId_t sem_right_rx = NULL;
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
// static RIS_MotorData_t raw_left __attribute__ ((section(".AXI_SRAM")));
static uint8_t raw_left_buff[16] __attribute__ ((section(".AXI_SRAM")));
static uint8_t raw_right_buff[16] __attribute__ ((section(".AXI_SRAM")));
// static RIS_MotorData_t raw_right __attribute__ ((section(".AXI_SRAM")));
static MotorCmd_t motor_cmd __attribute__ ((section(".AXI_SRAM")));
static osThreadId_t thread_alert;
static GO_t *go_rx;
static bool inited = false;
/* Private function -------------------------------------------------------- */
// static void GO_LeftRxCpltCallback(void) {
// osMessageQueuePut(go_rx->msgq_raw, &raw_left_buff, 0, 0);
// osSemaphoreRelease(sem_left_rx); // 通知接收完成
// memset(raw_left_buff, 0, sizeof(raw_left_buff)); // 清空缓存区
// HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
// (uint8_t *)&raw_left, sizeof(RIS_MotorData_t));
// }
// static void GO_RightRxCpltCallback(void) {
// osMessageQueuePut(go_rx->msgq_raw, &raw_right_buff, 0, 0);
// osSemaphoreRelease(sem_right_rx); // 通知接收完成
// memset(raw_right_buff, 0, sizeof(raw_right_buff)); // 清空缓存区
// HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
// (uint8_t *)&raw_right, sizeof(RIS_MotorData_t));
// }
/* Exported functions ------------------------------------------------------- */
int8_t GO_Init(GO_t *go, const GO_UART_Params_t *param){
if (go == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
go_rx->msgq_raw = osMessageQueueNew(36, 16u, NULL);
go->param = param;
sem_left_rx = osSemaphoreNew(1, 0, NULL);
sem_right_rx = osSemaphoreNew(1, 0, NULL);
// BSP_UART_RegisterCallback(BSP_UART_LEFT_LEG, BSP_UART_RX_CPLT_CB,
// GO_LeftRxCpltCallback);
// BSP_UART_RegisterCallback(BSP_UART_RIGHT_LEG, BSP_UART_RX_CPLT_CB,
// GO_RightRxCpltCallback);
inited = true;
return DEVICE_OK;
}
int8_t GO_Motor_Control(GO_MotorOutput_t *output, GO_t *go){
if (go == NULL) return DEVICE_ERR_NULL;
if (output == NULL) return DEVICE_ERR_NULL;
memset(raw_right_buff, 0, sizeof(raw_right_buff));
memset(raw_left_buff, 0, sizeof(raw_left_buff));
// HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
// (uint8_t *)&raw_left_buff, 16);
// HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
// (uint8_t *)&raw_right_buff, 16);
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
motor_cmd.id = i;
motor_cmd.mode = GO_MOTOR_MODE_FOC;
motor_cmd.T = output->as_array[i].tor_des;
motor_cmd.W = output->as_array[i].spd_des;
motor_cmd.Pos = output->as_array[i].pos_des;
motor_cmd.K_P = output->as_array[i].k_pos;
motor_cmd.K_W = output->as_array[i].k_spd;
modify_data(&motor_cmd);
if (i <= GO_MOTOR_LF_CALF || (i >= GO_MOTOR_LR_HIP && i <= GO_MOTOR_LR_CALF)) {
HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
(uint8_t *)&motor_cmd.motor_send_data,
sizeof(RIS_ControlData_t));
HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
(uint8_t *)&raw_left_buff, 16, 5);
} else {
HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
(uint8_t *)&motor_cmd.motor_send_data,
sizeof(RIS_ControlData_t));
HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
(uint8_t *)&raw_right_buff, 16, 5);
}
}
return DEVICE_OK;
}
int8_t GO_StoreMsg(GO_t *go, RIS_MotorData_t *go_rx){
return DEVICE_OK;
}

1
User/device/go.c
View File

@ -47,6 +47,7 @@ int8_t GO_Motor_Control(GO_t *go){
go->motor.output.id[i].mode = GO_MOTOR_MODE_FOC; // 设置电机模式
modify_data(&go->motor.output.id[i]);
if (i < GO_MOTOR_NUM / 4 || i >= GO_MOTOR_NUM/2 && i < GO_MOTOR_NUM * 3 / 4) {
// if (i < GO_MOTOR_NUM / 4) {
HAL_UART_Transmit(BSP_UART_GetHandle(go->uart_param->left_leg),
(uint8_t *)&go->motor.output.id[i].motor_send_data,
sizeof(go->motor.output.id[i].motor_send_data), 1);

156
User/device/n100.c Normal file
View File

@ -0,0 +1,156 @@
/*
N100 陀螺仪数据
*/
/* Includes ----------------------------------------------------------------- */
#include "n100.h"
#include <string.h>
#include "device/device.h"
#include "bsp/uart.h"
#include "component/user_math.h"
#include "component/crc8.h"
#include "component/crc16.h"
/* Private define ----------------------------------------------------------- */
#define RX_BUFF_SIZE 56
#define N100_BAG_HEAD 0xFC
#define N100_BAG_END 0xFD
// #define RX_BUFF_SIZE (sizeof(N100_AHRS_t))
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;
static uint8_t rxbuf[RX_BUFF_SIZE] __attribute__ ((section(".AXI_SRAM")));
// static N100_AHRS_t rxbuf __attribute__((section(".AXI_SRAM")));
static bool inited = false;
static osThreadId_t thread_alert;
/* Private function -------------------------------------------------------- */
static void N100_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_N100_NEW_DATA);
}
static void N100_ErrorCallback(void) {
N100_Restart();
}
static void N100_Decode(N100_t *n100, const uint8_t *rxbuf) {
if (n100 == NULL || rxbuf == NULL) return;
size_t offset = 0;
// header
n100->rx_raw.header.head = rxbuf[offset++];
n100->rx_raw.header.cmd = rxbuf[offset++];
n100->rx_raw.header.len = rxbuf[offset++];
n100->rx_raw.header.count = rxbuf[offset++];
n100->rx_raw.header.crc8 = rxbuf[offset++];
// data.crc16
n100->rx_raw.data.crc16 = (uint16_t)(rxbuf[offset] | (rxbuf[offset+1] << 8));
offset += 2;
//都是float32
memcpy(&n100->rx_raw.data.gyro.x, rxbuf + offset, 4);
offset += 4;
memcpy(&n100->rx_raw.data.gyro.y, rxbuf + offset, 4);
offset += 4;
memcpy(&n100->rx_raw.data.gyro.z, rxbuf + offset, 4);
offset += 4;
memcpy(&n100->rx_raw.data.eulr.rol, rxbuf + offset, 4);
offset += 4;
memcpy(&n100->rx_raw.data.eulr.pit, rxbuf + offset, 4);
offset += 4;
memcpy(&n100->rx_raw.data.eulr.yaw, rxbuf + offset, 4);
offset += 4;
// data.quat
memcpy(&n100->rx_raw.data.quat, rxbuf + offset, sizeof(AHRS_Quaternion_t));
offset += sizeof(AHRS_Quaternion_t);
// data.time
memcpy(&n100->rx_raw.data.time, rxbuf + offset, sizeof(int64_t));
offset += sizeof(int64_t);
// end
n100->rx_raw.end = rxbuf[offset];
}
// 直接对原始数据 rxbuf 校验
static bool N100_Verify(const uint8_t *rxbuf) {
if (rxbuf == NULL) return false;
// 检查包头包尾
if (rxbuf[0] != N100_BAG_HEAD || rxbuf[RX_BUFF_SIZE - 1] != N100_BAG_END) {
drop_message++;
return false;
}
// // CRC8 校验（包头4字节）
// if (rxbuf[4] != crc8_calc(rxbuf, 4)) {
// drop_message++;
// return false;
// }
// // CRC16 校验（数据段，从第7字节开始，长度为RX_BUFF_SIZE-8）
// uint16_t crc16 = (uint16_t)(rxbuf[5] | (rxbuf[6] << 8));
// if (crc16 != crc16_calc(rxbuf + 7, RX_BUFF_SIZE - 8)) {
// drop_message++;
// return false;
// }
return true;
}
/* Exported functions ------------------------------------------------------- */
int8_t N100_Init(N100_t *n100){
ASSERT(n100);
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_IMU_N100, BSP_UART_RX_CPLT_CB, N100_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_IMU_N100, BSP_UART_ERROR_CB, N100_ErrorCallback);
inited = true;
return DEVICE_OK;
}
int8_t N100_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_IMU_N100));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_IMU_N100));
return DEVICE_OK;
}
int8_t N100_StartReceiving(N100_t *n100) {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_IMU_N100), rxbuf, RX_BUFF_SIZE) != HAL_OK)
return DEVICE_ERR;
return DEVICE_OK;
}
bool N100_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_N100_NEW_DATA, osFlagsWaitAll, 200) ==
SIGNAL_N100_NEW_DATA);
}
int8_t N100_ParseData (N100_t *n100) {
if (n100 == NULL) return DEVICE_ERR_NULL;
// 先校验原始数据
if (!N100_Verify(rxbuf)) {
return DEVICE_ERR;
}
// 校验通过后再解码
N100_Decode(n100, rxbuf);
n100->eulr = n100->rx_raw.data.eulr;
n100->gyro = n100->rx_raw.data.gyro;
n100->quat = n100->rx_raw.data.quat;
return DEVICE_OK;
}
int8_t N100_HandleOffline(N100_t *n100) {
if (n100 == NULL) return DEVICE_ERR_NULL;
/*清空n100结构体*/
memset(n100, 0, sizeof(*n100));
return DEVICE_OK;
}

63
User/device/n100.h Normal file
View File

@ -0,0 +1,63 @@
/*
N100 陀螺仪数据
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
#include "component/ahrs.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
struct {
uint8_t head;
uint8_t cmd;
uint8_t len;
uint8_t count;
uint8_t crc8;
} header; /* 数据包头部 */
struct {
uint16_t crc16;
AHRS_Gyro_t gyro; /* 陀螺仪数据 */
AHRS_Eulr_t eulr; /* 欧拉角数据 */
AHRS_Quaternion_t quat; /* 四元数数据 */
int64_t time; /* 时间戳 */
} data; /* 数据包内容 */
uint8_t end;
} N100_AHRS_t;
typedef struct {
osThreadId_t thread_alert;
N100_AHRS_t rx_raw;
AHRS_Gyro_t gyro; /* 陀螺仪数据 */
AHRS_Eulr_t eulr; /* 欧拉角数据 */
AHRS_Quaternion_t quat; /* 四元数数据 */
} N100_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t N100_Init(N100_t *n100);
int8_t N100_Restart(void);
int8_t N100_StartReceiving(N100_t *n100);
bool N100_WaitDmaCplt(void);
int8_t N100_ParseData (N100_t *n100);
int8_t N100_HandleOffline(N100_t *n100);
#ifdef __cplusplus
}
#endif

36
User/module/chassis.c
View File

@ -10,12 +10,13 @@
#include "cmsis_os2.h"
// #include "component/limiter.h"
#include "device/go.h"
#include "component/limiter.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/*底盘默认高度*/
#define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */
#define CHASSIS_MAX_SPEED (0.03f)
/* Private macro ------------------------------------------------------------ */
@ -151,6 +152,7 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
// c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
c->output.id[i] = position_mode; /* 设置位置模式 */
}
// c->output.named.lf_thigh.Pos = 1.0f; /* 左前腿大腿位置 */
// c->output.named.lf_calf.Pos = 2.0f; /* 左前腿小腿位置 */
// c->output.named.rf_thigh.Pos = -1.0f; /* 右前腿大腿位置 */
@ -171,6 +173,11 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
}
break;
}
c->output.named.lf_hip.Pos = 0.0f; /* 左前腿髋关节位置 */
c->output.named.lr_hip.Pos = 0.0f; /* 左后腿髋关节位置 */
c->output.named.rf_hip.Pos = 0.0f; /* 右前腿髋关节位置 */
c->output.named.rr_hip.Pos = 0.0f; /* 右后腿髋关节位置 */
c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
c->output.named.lf_thigh.Pos = c->height * 5;
c->output.named.lf_calf.Pos = c->height * 10;
@ -181,8 +188,35 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
c->output.named.rr_thigh.Pos = -c->height * 5;
c->output.named.rr_calf.Pos = -c->height * 10;
chassis_mode_states = CHASSIS_MODE_RECOVER; /* 更新状态 */
for (uint8_t i = 0; i < 4; i++) {
float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]); /* 逆运动学计算 */
c->output.id[i * 3].Pos = angle_pose[0]; /* 髋关节位置 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 大腿位置 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 小腿位置 */
}
// float target_pose[3] = {c_cmd->ctrl_vec.vx/10.0f, c_cmd->ctrl_vec.vy/10.0f, -0.2f}; /* 目标位置 */
// float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
// float angle_pose[3];
// Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[0]); /* 逆运动学计算 */
// c->output.named.lf_hip.Pos = angle_pose[0]; /* 左前腿髋关节位置 */
// c->output.named.lf_thigh.Pos = angle_pose[1]; /* 左前腿大腿位置 */
// c->output.named.lf_calf.Pos = angle_pose[2]; /* 左前腿小腿位置 */
break;
}
// Limit_ChassicOutput(&c->feedback, &c->output, 0.5, 0.0, 0.0);
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
/* 限制输出 */
Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos,
c->param->mech_param.ratio.id[i] * CHASSIS_MAX_SPEED,
c->param->mech_param.limit.max.id[i],
c->param->mech_param.limit.min.id[i]);
}
}
void Chassis_DumpOutput(const Chassis_t *c, GO_ChassisCMD_t *out){

62
User/module/chassis.h
View File

@ -11,6 +11,7 @@ extern "C" {
/* Includes ----------------------------------------------------------------- */
#include "component/cmd.h"
#include "component/filter.h"
#include "component/kinematics.h"
#include "component/pid.h"
#include "device/go.h"
@ -30,6 +31,13 @@ typedef enum {
CHASSIS_TYPE_QUADRUPED, /* 四足机器人 */
} Chassis_Type_t;
typedef enum {
IMU_BOARD_BMI088,
IMU_WHEELREC_N100,
IMU_NUM,
IMU_ERROR,
} Chassis_ImuType_t;
// typedef union {
// float id[GO_MOTOR_NUM]; /* 零点角度，单位：弧度 */
// struct {
@ -66,46 +74,32 @@ typedef union {
} named;
} joint_params;
// /*关节减速比*/
// typedef union {
// float id[GO_MOTOR_NUM];
// struct{
// float lf_hip; /* 左前腿髋关节长度，单位：米 */
// float lf_thigh; /* 左前腿大腿长度，单位：米 */
// float lf_calf; /* 左前腿小腿长度，单位：米 */
// float rf_hip; /* 右前腿髋关节长度，单位：米 */
// float rf_thigh; /* 右前腿大腿长度，单位：米 */
// float rf_calf; /* 右前腿小腿长度，单位：米 */
// float lr_hip; /* 左后腿髋关节长度，单位：米 */
// float lr_thigh; /* 左后腿大腿长度，单位：米 */
// float lr_calf; /* 左后腿小腿长度，单位：米 */
// float rr_hip; /* 右后腿髋关节长度，单位：米 */
// float rr_thigh; /* 右后腿大腿长度，单位：米 */
// float rr_calf; /* 右后腿小腿长度，单位：米 */
// } named;
// } joint_ratio;
typedef struct {
joint_params min;
joint_params max; /* 关节的最小和最大角度，单位：弧度 */
} joint_limits;
// struct {
// float x; /* 机械中心X坐标，单位：米 */
// float y; /* 机械中心Y坐标，单位：米 */
// float z; /* 机械中心Z坐标，单位：米 */
// } mech_center; /* 机械中心位置 */
typedef union {
Kinematics_Sign_t leg[GO_MOTOR_NUM/3];
struct {
Kinematics_Sign_t lf; /* 左前腿 */
Kinematics_Sign_t rf; /* 右前腿 */
Kinematics_Sign_t lr; /* 左后腿 */
Kinematics_Sign_t rr; /* 右后腿 */
} named;
} Kinematics_DirectionSign_t;
// typedef struct {
// /*三个关节距离*/
typedef struct{
joint_params zero_point; /* 零点角度，单位：弧度 */
joint_params length; /* 关节长度，单位：米 */
// joint_params length; /* 关节长度，单位：米 */
joint_params ratio; /* 关节减速比 */
struct {
float x; /* 机械中心X坐标，单位：米 */
float y; /* 机械中心Y坐标，单位：米 */
float z; /* 机械中心Z坐标，单位：米 */
} mech_center; /* 机械中心位置 */
joint_limits limit; /* 关节的最小和最大角度，单位：弧度 */
Kinematics_LinkLength_t length; /* 关节长度，单位：米 */
Kinematics_LegOffset_t offset; /* 关节偏移，单位：米 */
Kinematics_DirectionSign_t sign; /* 关节侧向标志（左前/左后为-1，右前/右后为1） */
}Chassis_Mech_Params_t;

116
User/module/config.c
View File

@ -1,116 +0,0 @@
/*
* 配置相关
*/
#include "config.h"
#include <stdint.h>
#include <string.h>
#include "bsp/flash.h"
#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)
Config_t param_default = {
.uart = {
.left_leg = BSP_UART_LEFT_LEG,
.right_leg = BSP_UART_RIGHT_LEG,
},
.cali = {
.bmi088 = {
.gyro_offset = {0.0f, 0.0f, 0.0f},
},
},
.chassis = {
.type = CHASSIS_TYPE_QUADRUPED,
.torque_pid_param = {
.k = 1.0f, /* 控制器增益 */
.p = 1.0f, /* 比例项增益 */
.i = 0.0f, /* 积分项增益 */
.d = 0.0f, /* 微分项增益 */
.i_limit = 0.0f, /* 积分项上限 */
.out_limit = 0.0f, /* 输出绝对值限制 */
.d_cutoff_freq = 10.0f, /* D项低通截止频率 */
.range = -1.0f, /* 计算循环误差时使用，大于0时启用 */
},
.low_pass_cutoff_freq = {
.in = -1.0f,
.out = -1.0f,
},
.mech_param = {
.length = {
.named = {
.lf_hip = 0.0861f, /* 左前腿髋关节长度，单位：米 */
.lf_thigh = 0.20f, /* 左前腿大腿长度，单位：米 */
.lf_calf = 0.20f, /* 左前腿小腿长度，单位：米 */
.rf_hip = 0.0861f, /* 右前腿髋关节长度，单位：米 */
.rf_thigh = 0.20f, /* 右前腿大腿长度，单位：米 */
.rf_calf = 0.20f, /* 右前腿小腿长度，单位：米 */
.lr_hip = 0.0861f, /* 左后腿髋关节长度，单位：米 */
.lr_thigh = 0.20f, /* 左后腿大腿长度，单位：米 */
.lr_calf = 0.20f, /* 左后腿小腿长度，单位：米 */
.rr_hip = 0.0861f, /* 右后腿髋关节长度，单位：米 */
.rr_thigh = 0.20f, /* 右后腿大腿长度，单位：米 */
.rr_calf = 0.20f, /* 右后腿小腿长度，单位：米 */
}
},
.zero_point = {
.named = {
.lf_hip = 1.98f, /* 左前腿髋关节零点角度，单位：弧度 */
.lf_thigh = -2.86f, /* 左前腿大腿零点角度，单位：弧度 */
.lf_calf = -32.367f, /* 左前腿小腿零点角度，单位：弧度 */
.rf_hip = 5.56f, /* 右前腿髋关节零点角度，单位：弧度 */
.rf_thigh = 12.92f, /* 右前腿大腿零点角度，单位：弧度 */
.rf_calf = 37.047f, /* 右前腿小腿零点角度，单位：弧度 */
.lr_hip = 4.76f, /* 左后腿髋关节零点角度，单位：弧度 */
.lr_thigh = -4.57f, /* 左后腿大腿零点角度，单位：弧度 */
.lr_calf = -35.847f, /* 左后腿小腿零点角度，单位：弧度 */
.rr_hip = 2.42f, /* 右后腿髋关节零点角度，单位：弧度 */
.rr_thigh = 8.85f, /* 右后腿大腿零点角度，单位：弧度 */
.rr_calf = 38.55f, /* 右后腿小腿零点角度，单位：弧度 */
}
},
.ratio = {
.named = {
.lf_hip = 6.33f, /* 左前腿髋关节减速比 */
.lf_thigh = 6.33f, /* 左前腿大腿减速比 */
.lf_calf = 12.66f, /* 左前腿小腿减速比 */
.rf_hip = 6.33f, /* 右前腿髋关节减速比 */
.rf_thigh = 6.33f, /* 右前腿大腿减速比 */
.rf_calf = 12.66f, /* 右前腿小腿减速比 */
.lr_hip = 6.33f, /* 左后腿髋关节减速比 */
.lr_thigh = 6.33f, /* 左后腿大腿减速比 */
.lr_calf = 12.66f, /* 左后腿小腿减速比 */
.rr_hip = 6.33f, /* 右后腿髋关节减速比 */
.rr_thigh = 6.33f, /* 右后腿大腿减速比 */
.rr_calf = 12.66f, /* 右后腿小腿减速比 */
}
},
.mech_center = {
.x = 0.0f, /* 机械中心X坐标，单位：米 */
.y = 0.0f, /* 机械中心Y坐标，单位：米 */
.z = 0.0f, /* 机械中心Z坐标，单位：米 */
}, /* 机械中心位置 */
},
},
}; /* param_default */
/**
* \brief 获取配置信息
*
* \param cfg 配置信息
*/
void Config_Get(Config_t *cfg) {
//直接把param_default传进去
if (cfg == NULL) return; // 检查参数是否为NULL
memcpy(cfg, &param_default, sizeof(Config_t)); // 复制默认配置到cfg
}

3
User/module/config.h
View File

@ -37,7 +37,8 @@ typedef struct {
struct {
BMI088_Cali_t bmi088;
} cali; /* 校准 */
Chassis_ImuType_t chassis_imu_type; /* 底盘IMU类型 */
} Config_t;
/**

97
User/task/atti_esti.c
View File

@ -7,9 +7,11 @@
#include "task/user_task.h"
/* USER INCLUDE BEGIN*/
#include "bsp/pwm.h"
#include "bsp/gpio.h"
#include "component/ahrs.h"
#include "component/pid.h"
#include "device/bmi088.h"
#include "device/n100.h"
#include "task/user_task.h"
/* USER INCLUDE END*/
@ -19,6 +21,8 @@
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN*/
BMI088_t bmi088;
N100_t n100;
BMI088_Cali_t bmi088_cali = {
.gyro_offset = {0.0f, 0.0f, 0.0f},
}; /* BMI088校准数据 */
@ -48,44 +52,81 @@ void Task_atti_esti(void *argument) {
osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
/* USER CODE INIT BEGIN*/
BMI088_Init(&bmi088, &bmi088_cali); /* 初始化BMI088传感器 */
AHRS_Init(&gimbal_ahrs, &mage, BMI088_GetUpdateFreq(&bmi088)); /* 初始化姿态解算算法 */
/* 初始化IMU温度控制PID，防止温漂 */
PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
// BSP_PWM_Start(BSP_PWM_IMU_HEAT);
switch (task_runtime.config.chassis_imu_type) {
case IMU_BOARD_BMI088:
BMI088_Init(&bmi088, &bmi088_cali); /* 初始化BMI088传感器 */
AHRS_Init(&gimbal_ahrs, &mage, BMI088_GetUpdateFreq(&bmi088)); /* 初始化姿态解算算法 */
/* 初始化IMU温度控制PID，防止温漂 */
// PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
// 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
// BSP_PWM_Start(BSP_PWM_IMU_HEAT);
break;
case IMU_WHEELREC_N100:
N100_Init(&n100);
BSP_GPIO_SetPin(BSP_GPIO_5V_EN, BSP_GPIO_ON); /* 打开5V电源 */
break;
default:
break;
}
/* USER CODE INIT END*/
while (1) {
/* USER CODE BEGIN */
/* 等待IMU新数据 */
BMI088_WaitNew();
switch (task_runtime.config.chassis_imu_type) {
case IMU_BOARD_BMI088:
/* 等待IMU新数据 */
BMI088_WaitNew();
/* 开始数据接收DMA，加速度计和陀螺仪共用同一个SPI接口，
* 一次只能开启一个DMA
*/
BMI088_AcclStartDmaRecv();
BMI088_AcclWaitDmaCplt();
/* 开始数据接收DMA，加速度计和陀螺仪共用同一个SPI接口，
* 一次只能开启一个DMA
*/
BMI088_AcclStartDmaRecv();
BMI088_AcclWaitDmaCplt();
BMI088_GyroStartDmaRecv();
BMI088_GyroWaitDmaCplt();
BMI088_GyroStartDmaRecv();
BMI088_GyroWaitDmaCplt();
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
osKernelLock();
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
BMI088_ParseAccl(&bmi088);
BMI088_ParseGyro(&bmi088);
/* 根据设备接收到的数据进行姿态解析 */
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &mage);
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
osKernelUnlock();
/* 将欧拉角数据放入消息队列 */
osMessageQueueReset(task_runtime.msgq.body.eulr_imu);
osMessageQueuePut(task_runtime.msgq.body.eulr_imu, &eulr_to_send, 0, 0); /* 将欧拉角数据放入消息队列 */
/* PID控制IMU温度，PWM输出 */
// BSP_PWM_Set(BSP_PWM_IMU_HEAT,
// PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.01f));
break;
case IMU_WHEELREC_N100:
N100_StartReceiving(&n100);
if (N100_WaitDmaCplt()) {
osKernelLock();
N100_ParseData(&n100);
osKernelUnlock();
} else {
N100_HandleOffline(&n100);
}
osMessageQueueReset(task_runtime.msgq.body.eulr_imu); /* 重置消息队列 */
osMessageQueuePut(task_runtime.msgq.body.eulr_imu, &n100.eulr, 0, 0); /* 将欧拉角数据放入消息队列 */
break;
default:
break;
}
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
osKernelLock();
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
BMI088_ParseAccl(&bmi088);
BMI088_ParseGyro(&bmi088);
/* 根据设备接收到的数据进行姿态解析 */
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &mage);
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
osKernelUnlock();
/* PID控制IMU温度，PWM输出 */
// BSP_PWM_Set(BSP_PWM_IMU_HEAT,
// PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.01f));
/* USER CODE END */
}

4
User/task/config.yaml
View File

@ -37,14 +37,14 @@
function: Task_cmd
name: cmd
stack: 256
- delay: 0
- delay: 200
description: 四足控制
freq_control: true
frequency: 800
function: Task_ctrl_leg
name: ctrl_leg
stack: 512
- delay: 0
- delay: 200
description: 底盘控制，接受反馈和命令，计算输出给到两测腿控制
freq_control: true
frequency: 500

2
User/task/ctrl_leg.c
View File

@ -40,9 +40,7 @@ void Task_ctrl_leg(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
// osMessageQueueGet(task_runtime.msgq.chassis.output, &cmd, NULL, 0); /* 从消息队列中获取底盘控制命令 */
if (osMessageQueueGet(task_runtime.msgq.chassis.output, &go_cmd, NULL, 0) == osOK) { /* 从消息队列中获取底盘控制命令 */
GO_ParseCmd(&go, &go_cmd); /* 解析底盘控制命令 */
}

1
User/task/init.c
View File

@ -45,6 +45,7 @@ void Task_Init(void *argument) {
task_runtime.msgq.cmd.chassis = osMessageQueueNew(2u, sizeof(CMD_ChassisCmd_t), NULL);
task_runtime.msgq.chassis.feefback = osMessageQueueNew(2u, sizeof(GO_ChassisFeedback_t), NULL);
task_runtime.msgq.chassis.output = osMessageQueueNew(2u, sizeof(GO_ChassisCMD_t), NULL);
task_runtime.msgq.body.eulr_imu = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

10
User/task/user_task.h
View File

@ -16,7 +16,7 @@ extern "C" {
#define TEST_FREQ (1000)
#define MONITOR_FREQ (100)
#define CMD_FREQ (500)
#define CTRL_LEG_FREQ (500)
#define CTRL_LEG_FREQ (800)
#define CTRL_CHASSIS_FREQ (500)
/* 任务初始化延时ms */
@ -27,8 +27,8 @@ extern "C" {
#define CLI_INIT_DELAY (0)
#define MONITOR_INIT_DELAY (0)
#define CMD_INIT_DELAY (0)
#define CTRL_LEG_INIT_DELAY (0)
#define CTRL_CHASSIS_INIT_DELAY (0)
#define CTRL_LEG_INIT_DELAY (200)
#define CTRL_CHASSIS_INIT_DELAY (200)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
@ -51,6 +51,10 @@ typedef struct {
/* USER MESSAGE BEGIN */
struct {
// osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
struct {
osMessageQueueId_t eulr_imu;
} body;
struct {
struct {
osMessageQueueId_t rc; /* 原始数据消息队列 */

221
Utils/Kinematics.m Normal file
View File

@ -0,0 +1,221 @@
function quadruped_leg_visualization()
% 四足机器人单腿运动学可视化
% 清除之前的图形
close all;
clc;
% 定义腿部参数 (单位: 米)
leg_params.upper_leg_length = 0.15;
leg_params.lower_leg_length = 0.15;
leg_params.hip_offset_x = 0.05;
leg_params.hip_offset_z = -0.02;
leg_params.hip_yaw_offset = 0.01;
leg_params.knee_sign = 1; % 1或-1，取决于机械设计
% 目标位置 (在髋关节坐标系中)
target_pos = [0.2, 0.1, -0.25]; % x, y, z
% 计算逆运动学 (右腿hip_yaw_sign=1，左腿=-1)
joint_angles = leg_inverse_kinematics(target_pos, leg_params, 1);
% 显示计算结果
disp('关节角度 (弧度):');
disp(['髋关节偏航 (hip yaw): ', num2str(joint_angles(1)), ' rad']);
disp(['髋关节滚动 (hip roll): ', num2str(joint_angles(2)), ' rad']);
disp(['髋关节俯仰 (hip pitch): ', num2str(joint_angles(3)), ' rad']);
disp(['膝关节 (knee): ', num2str(joint_angles(4)), ' rad']);
disp('关节角度 (度):');
disp(['髋关节偏航 (hip yaw): ', num2str(rad2deg(joint_angles(1))), '°']);
disp(['髋关节滚动 (hip roll): ', num2str(rad2deg(joint_angles(2))), '°']);
disp(['髋关节俯仰 (hip pitch): ', num2str(rad2deg(joint_angles(3))), '°']);
disp(['膝关节 (knee): ', num2str(rad2deg(joint_angles(4))), '°']);
% 可视化腿部
visualize_leg(joint_angles, leg_params, target_pos);
end
function joint_angles = leg_inverse_kinematics(target_pos, leg_params, hip_yaw_sign)
% 逆运动学计算函数
% 从参数字典中提取参数
L1 = leg_params.upper_leg_length;
L2 = leg_params.lower_leg_length;
offset_x = leg_params.hip_offset_x;
offset_z = leg_params.hip_offset_z;
yaw_offset = leg_params.hip_yaw_offset;
knee_sign = leg_params.knee_sign;
% 第一步：计算髋关节偏航角 (hip yaw)
x = target_pos(1);
y = target_pos(2);
z = target_pos(3);
x = x - offset_x;
z = z - offset_z;
% 使用迭代法处理yaw offset
theta_yaw = atan2(hip_yaw_sign * y, x);
for i = 1:3
% 计算yaw旋转后的位置
x_rot = x * cos(theta_yaw) + hip_yaw_sign * y * sin(theta_yaw);
y_rot = -x * sin(theta_yaw) + hip_yaw_sign * y * cos(theta_yaw);
% 考虑yaw offset
x_rot = x_rot - yaw_offset * (1 - cos(theta_yaw));
y_rot = y_rot + yaw_offset * sin(theta_yaw);
theta_yaw = atan2(hip_yaw_sign * y_rot, x_rot);
end
% 第二步：计算髋关节滚转角 (hip roll) 和俯仰角 (hip pitch)
x_rot = x * cos(theta_yaw) + hip_yaw_sign * y * sin(theta_yaw) - yaw_offset;
z_rot = z;
% 计算髋关节滚转角
L = sqrt(x_rot^2 + z_rot^2);
theta_roll = atan2(z_rot, x_rot);
% 计算髋关节俯仰角和膝关节角度
D = (L^2 - L1^2 - L2^2) / (2 * L1 * L2);
D = max(min(D, 1.0), -1.0); % 确保数值在有效范围内
theta_knee = knee_sign * acos(D);
alpha = atan2(L2 * sin(theta_knee), L1 + L2 * cos(theta_knee));
theta_pitch = alpha + theta_roll;
% 返回关节角度 (hip_yaw, hip_roll, hip_pitch, knee)
joint_angles = [theta_yaw, theta_roll, theta_pitch, theta_knee];
end
function visualize_leg(joint_angles, leg_params, target_pos)
% 可视化腿部运动学
% 提取参数
L1 = leg_params.upper_leg_length;
L2 = leg_params.lower_leg_length;
offset_x = leg_params.hip_offset_x;
offset_z = leg_params.hip_offset_z;
yaw_offset = leg_params.hip_yaw_offset;
% 提取关节角度
theta_yaw = joint_angles(1);
theta_roll = joint_angles(2);
theta_pitch = joint_angles(3);
theta_knee = joint_angles(4);
% 创建图形窗口
figure('Name', '四足机器人单腿运动学可视化', 'NumberTitle', 'off', 'Position', [100, 100, 1200, 800]);
% 1. 3D视图
subplot(2,2,[1,3]);
hold on;
grid on;
axis equal;
view(3);
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Z (m)');
title('3D视图');
% 绘制坐标系
plot3([0, 0.1], [0, 0], [0, 0], 'r', 'LineWidth', 2); % X轴
plot3([0, 0], [0, 0.1], [0, 0], 'g', 'LineWidth', 2); % Y轴
plot3([0, 0], [0, 0], [0, 0.1], 'b', 'LineWidth', 2); % Z轴
% 计算关节位置
% 髋关节位置 (考虑了offset)
hip_pos = [offset_x, 0, offset_z];
% 应用yaw旋转
R_yaw = [cos(theta_yaw), -sin(theta_yaw), 0;
sin(theta_yaw), cos(theta_yaw), 0;
0, 0, 1];
% 应用roll旋转 (在yaw旋转后的坐标系中)
R_roll = [cos(theta_roll), 0, sin(theta_roll);
0, 1, 0;
-sin(theta_roll), 0, cos(theta_roll)];
% 应用pitch旋转
R_pitch = [1, 0, 0;
0, cos(theta_pitch), -sin(theta_pitch);
0, sin(theta_pitch), cos(theta_pitch)];
% 计算大腿末端位置
upper_leg_end = hip_pos + (R_yaw * R_roll * R_pitch * [L1; 0; 0])';
% 计算小腿末端位置
R_knee = [1, 0, 0;
0, cos(theta_knee), -sin(theta_knee);
0, sin(theta_knee), cos(theta_knee)];
lower_leg_end = upper_leg_end + (R_yaw * R_roll * R_pitch * R_knee * [L2; 0; 0])';
% 绘制连杆
plot3([hip_pos(1), upper_leg_end(1)], ...
[hip_pos(2), upper_leg_end(2)], ...
[hip_pos(3), upper_leg_end(3)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
plot3([upper_leg_end(1), lower_leg_end(1)], ...
[upper_leg_end(2), lower_leg_end(2)], ...
[upper_leg_end(3), lower_leg_end(3)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
% 绘制目标位置
plot3(target_pos(1), target_pos(2), target_pos(3), 'g*', 'MarkerSize', 15);
% 绘制yaw offset
plot3([hip_pos(1), hip_pos(1)+yaw_offset*sin(theta_yaw)], ...
[0, yaw_offset*cos(theta_yaw)], ...
[hip_pos(3), hip_pos(3)], 'm--', 'LineWidth', 2);
% 设置视图范围
axis_range = max([L1+L2, abs(offset_x)+0.1, abs(offset_z)+0.1]);
xlim([-axis_range, axis_range]);
ylim([-axis_range, axis_range]);
zlim([-axis_range, axis_range]);
% 2. X-Y平面视图
subplot(2,2,2);
hold on;
grid on;
axis equal;
xlabel('X (m)');
ylabel('Y (m)');
title('X-Y平面视图');
plot([0, 0.1], [0, 0], 'r', 'LineWidth', 2); % X轴
plot([0, 0], [0, 0.1], 'g', 'LineWidth', 2); % Y轴
plot([hip_pos(1), upper_leg_end(1)], [hip_pos(2), upper_leg_end(2)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
plot([upper_leg_end(1), lower_leg_end(1)], [upper_leg_end(2), lower_leg_end(2)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
plot(target_pos(1), target_pos(2), 'g*', 'MarkerSize', 15);
plot([hip_pos(1), hip_pos(1)+yaw_offset*sin(theta_yaw)], [0, yaw_offset*cos(theta_yaw)], 'm--', 'LineWidth', 2);
xlim([-axis_range, axis_range]);
ylim([-axis_range, axis_range]);
% 3. X-Z平面视图
subplot(2,2,4);
hold on;
grid on;
axis equal;
xlabel('X (m)');
ylabel('Z (m)');
title('X-Z平面视图');
plot([0, 0.1], [0, 0], 'r', 'LineWidth', 2); % X轴
plot([0, 0], [0, 0.1], 'b', 'LineWidth', 2); % Z轴
plot([hip_pos(1), upper_leg_end(1)], [hip_pos(3), upper_leg_end(3)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
plot([upper_leg_end(1), lower_leg_end(1)], [upper_leg_end(3), lower_leg_end(3)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
plot(target_pos(1), target_pos(3), 'g*', 'MarkerSize', 15);
xlim([-axis_range, axis_range]);
ylim([-axis_range, axis_range]);
% 添加图例
legend({'大腿', '小腿', '目标位置', 'Yaw偏移'}, 'Location', 'best');
end

78
Utils/Kinematics.py Normal file
View File

@ -0,0 +1,78 @@
# test_kinematics.py
import numpy as np
def forward_kinematics(theta, link, side_sign):
l1 = link['hip'] * side_sign
l2 = -link['thigh']
l3 = -link['calf']
q1, q2, q3 = theta
s1, s2, s3 = np.sin([q1, q2, q3])
c1, c2, c3 = np.cos([q1, q2, q3])
c23 = c2 * c3 - s2 * s3
s23 = s2 * c3 + c2 * s3
x = l3 * s23 + l2 * s2
y = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1
z = l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2
return np.array([x, y, z])
def q1_ik(py, pz, l1):
L = np.sqrt(py*py + pz*pz - l1*l1)
return np.arctan2(pz*l1 + py*L, py*l1 - pz*L)
def q3_ik(b3z, b4z, b):
temp = (b3z**2 + b4z**2 - b**2) / (2.0 * np.abs(b3z * b4z))
temp = np.clip(temp, -1.0, 1.0)
q3 = np.arccos(temp)
return -(np.pi - q3)
def q2_ik(q1, q3, px, py, pz, b3z, b4z):
a1 = py * np.sin(q1) - pz * np.cos(q1)
a2 = px
m1 = b4z * np.sin(q3)
m2 = b3z + b4z * np.cos(q3)
return np.arctan2(m1 * a1 + m2 * a2, m1 * a2 - m2 * a1)
def inverse_kinematics(foot, link, side_sign):
px, py, pz = foot
b2y = link['hip'] * side_sign
b3z = -link['thigh']
b4z = -link['calf']
a = link['hip']
c = np.sqrt(px*px + py*py + pz*pz)
b = np.sqrt(c*c - a*a)
if np.isnan(b) or b > abs(b3z) + abs(b4z):
return None
q1 = q1_ik(py, pz, b2y)
q3 = q3_ik(b3z, b4z, b)
q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z)
return np.array([q1, q2, q3])
# ...existing code...
def test():
link = {'hip': 0.05, 'thigh': 0.2, 'calf': 0.2}
side_sign = 1 # 左前腿
# 输入一个目标点
foot_target = np.array([0.0, 0.06, -0.2])
print("目标足端位置:", foot_target)
# 逆运动学求解关节角度
theta = inverse_kinematics(foot_target, link, side_sign)
if theta is None:
print("目标点不可达！")
return
print("逆运动学解（关节角度，单位：弧度）:", theta)
print("逆运动学解（关节角度，单位：度）:", np.degrees(theta))
# 正运动学验算
foot_check = forward_kinematics(theta, link, side_sign)
print("正运动学验算足端位置:", foot_check)
print("位置误差:", np.abs(foot_target - foot_check))
if __name__ == "__main__":
test()