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

dm

Browse Source
This commit is contained in:
Robofish 2025-09-23 21:31:03 +08:00
parent 3e7ff5a16a
commit 040176d54f
16 changed files with 1703 additions and 531 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.mxproject
View File

File diff suppressed because one or more lines are too long

3
CMakeLists.txt
View File

@ -72,6 +72,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/dr16.c
User/device/motor.c
User/device/motor_lk.c
User/device/motor_dm.c
User/device/motor_rm.c
User/device/motor_lz.c
# User/module sources
@ -82,6 +84,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/task/atti_esti.c
User/task/blink.c
User/task/ctrl_chassis.c
User/task/ctrl_gimbal.c
User/task/init.c
User/task/rc.c
User/task/user_task.c

996
Core/Src/stm32f4xx_it.c
View File

@ -1,498 +1,498 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32f4xx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
* @attention
*
* <h2><center>&copy; Copyright (c) 2025 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under Ultimate Liberty license
* SLA0044, the "License"; You may not use this file except in compliance with
* the License. You may obtain a copy of the License at:
* www.st.com/SLA0044
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f4xx_it.h"
#include "FreeRTOS.h"
#include "task.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "bsp/uart.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
extern DMA_HandleTypeDef hdma_i2c2_tx;
extern DMA_HandleTypeDef hdma_i2c3_rx;
extern DMA_HandleTypeDef hdma_spi1_rx;
extern DMA_HandleTypeDef hdma_spi1_tx;
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim7;
extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart6_rx;
extern DMA_HandleTypeDef hdma_usart6_tx;
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart6;
/* USER CODE BEGIN EV */
/* USER CODE END EV */
/******************************************************************************/
/* Cortex-M4 Processor Interruption and Exception Handlers */
/******************************************************************************/
/**
* @brief This function handles Non maskable interrupt.
*/
void NMI_Handler(void)
{
/* USER CODE BEGIN NonMaskableInt_IRQn 0 */
/* USER CODE END NonMaskableInt_IRQn 0 */
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
while (1)
{
}
/* USER CODE END NonMaskableInt_IRQn 1 */
}
/**
* @brief This function handles Hard fault interrupt.
*/
void HardFault_Handler(void)
{
/* USER CODE BEGIN HardFault_IRQn 0 */
/* USER CODE END HardFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
/* USER CODE END W1_HardFault_IRQn 0 */
}
}
/**
* @brief This function handles Memory management fault.
*/
void MemManage_Handler(void)
{
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
/* USER CODE END MemoryManagement_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
/* USER CODE END W1_MemoryManagement_IRQn 0 */
}
}
/**
* @brief This function handles Pre-fetch fault, memory access fault.
*/
void BusFault_Handler(void)
{
/* USER CODE BEGIN BusFault_IRQn 0 */
/* USER CODE END BusFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_BusFault_IRQn 0 */
/* USER CODE END W1_BusFault_IRQn 0 */
}
}
/**
* @brief This function handles Undefined instruction or illegal state.
*/
void UsageFault_Handler(void)
{
/* USER CODE BEGIN UsageFault_IRQn 0 */
/* USER CODE END UsageFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_UsageFault_IRQn 0 */
/* USER CODE END W1_UsageFault_IRQn 0 */
}
}
/**
* @brief This function handles Debug monitor.
*/
void DebugMon_Handler(void)
{
/* USER CODE BEGIN DebugMonitor_IRQn 0 */
/* USER CODE END DebugMonitor_IRQn 0 */
/* USER CODE BEGIN DebugMonitor_IRQn 1 */
/* USER CODE END DebugMonitor_IRQn 1 */
}
/**
* @brief This function handles System tick timer.
*/
void SysTick_Handler(void)
{
/* USER CODE BEGIN SysTick_IRQn 0 */
/* USER CODE END SysTick_IRQn 0 */
HAL_IncTick();
#if (INCLUDE_xTaskGetSchedulerState == 1 )
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED)
{
#endif /* INCLUDE_xTaskGetSchedulerState */
xPortSysTickHandler();
#if (INCLUDE_xTaskGetSchedulerState == 1 )
}
#endif /* INCLUDE_xTaskGetSchedulerState */
/* USER CODE BEGIN SysTick_IRQn 1 */
/* USER CODE END SysTick_IRQn 1 */
}
/******************************************************************************/
/* STM32F4xx Peripheral Interrupt Handlers */
/* Add here the Interrupt Handlers for the used peripherals. */
/* For the available peripheral interrupt handler names, */
/* please refer to the startup file (startup_stm32f4xx.s). */
/******************************************************************************/
/**
* @brief This function handles EXTI line0 interrupt.
*/
void EXTI0_IRQHandler(void)
{
/* USER CODE BEGIN EXTI0_IRQn 0 */
/* USER CODE END EXTI0_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(USER_KEY_Pin);
/* USER CODE BEGIN EXTI0_IRQn 1 */
/* USER CODE END EXTI0_IRQn 1 */
}
/**
* @brief This function handles EXTI line3 interrupt.
*/
void EXTI3_IRQHandler(void)
{
/* USER CODE BEGIN EXTI3_IRQn 0 */
/* USER CODE END EXTI3_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(CMPS_INT_Pin);
/* USER CODE BEGIN EXTI3_IRQn 1 */
/* USER CODE END EXTI3_IRQn 1 */
}
/**
* @brief This function handles EXTI line4 interrupt.
*/
void EXTI4_IRQHandler(void)
{
/* USER CODE BEGIN EXTI4_IRQn 0 */
/* USER CODE END EXTI4_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(ACCL_INT_Pin);
/* USER CODE BEGIN EXTI4_IRQn 1 */
/* USER CODE END EXTI4_IRQn 1 */
}
/**
* @brief This function handles DMA1 stream1 global interrupt.
*/
void DMA1_Stream1_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
/* USER CODE END DMA1_Stream1_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart3_rx);
/* USER CODE BEGIN DMA1_Stream1_IRQn 1 */
/* USER CODE END DMA1_Stream1_IRQn 1 */
}
/**
* @brief This function handles DMA1 stream2 global interrupt.
*/
void DMA1_Stream2_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream2_IRQn 0 */
/* USER CODE END DMA1_Stream2_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_i2c3_rx);
/* USER CODE BEGIN DMA1_Stream2_IRQn 1 */
/* USER CODE END DMA1_Stream2_IRQn 1 */
}
/**
* @brief This function handles CAN1 RX0 interrupts.
*/
void CAN1_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX0_IRQn 0 */
/* USER CODE END CAN1_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX0_IRQn 1 */
/* USER CODE END CAN1_RX0_IRQn 1 */
}
/**
* @brief This function handles CAN1 RX1 interrupt.
*/
void CAN1_RX1_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX1_IRQn 0 */
/* USER CODE END CAN1_RX1_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX1_IRQn 1 */
/* USER CODE END CAN1_RX1_IRQn 1 */
}
/**
* @brief This function handles EXTI line[9:5] interrupts.
*/
void EXTI9_5_IRQHandler(void)
{
/* USER CODE BEGIN EXTI9_5_IRQn 0 */
/* USER CODE END EXTI9_5_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GYRO_INT_Pin);
/* USER CODE BEGIN EXTI9_5_IRQn 1 */
/* USER CODE END EXTI9_5_IRQn 1 */
}
/**
* @brief This function handles TIM1 break interrupt and TIM9 global interrupt.
*/
void TIM1_BRK_TIM9_IRQHandler(void)
{
/* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 0 */
/* USER CODE END TIM1_BRK_TIM9_IRQn 0 */
HAL_TIM_IRQHandler(&htim1);
/* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 1 */
/* USER CODE END TIM1_BRK_TIM9_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 DMA1 stream7 global interrupt.
*/
void DMA1_Stream7_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream7_IRQn 0 */
/* USER CODE END DMA1_Stream7_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_i2c2_tx);
/* USER CODE BEGIN DMA1_Stream7_IRQn 1 */
/* USER CODE END DMA1_Stream7_IRQn 1 */
}
/**
* @brief This function handles TIM7 global interrupt.
*/
void TIM7_IRQHandler(void)
{
/* USER CODE BEGIN TIM7_IRQn 0 */
/* USER CODE END TIM7_IRQn 0 */
HAL_TIM_IRQHandler(&htim7);
/* USER CODE BEGIN TIM7_IRQn 1 */
/* USER CODE END TIM7_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_usart6_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_spi1_rx);
/* 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_spi1_tx);
/* USER CODE BEGIN DMA2_Stream3_IRQn 1 */
/* USER CODE END DMA2_Stream3_IRQn 1 */
}
/**
* @brief This function handles CAN2 RX0 interrupts.
*/
void CAN2_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_RX0_IRQn 0 */
/* USER CODE END CAN2_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_RX0_IRQn 1 */
/* USER CODE END CAN2_RX0_IRQn 1 */
}
/**
* @brief This function handles CAN2 RX1 interrupt.
*/
void CAN2_RX1_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_RX1_IRQn 0 */
/* USER CODE END CAN2_RX1_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_RX1_IRQn 1 */
/* USER CODE END CAN2_RX1_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream5 global interrupt.
*/
void DMA2_Stream5_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream5_IRQn 0 */
/* USER CODE END DMA2_Stream5_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_rx);
/* USER CODE BEGIN DMA2_Stream5_IRQn 1 */
/* USER CODE END DMA2_Stream5_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream6 global interrupt.
*/
void DMA2_Stream6_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream6_IRQn 0 */
/* USER CODE END DMA2_Stream6_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart6_tx);
/* USER CODE BEGIN DMA2_Stream6_IRQn 1 */
/* USER CODE END DMA2_Stream6_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream7 global interrupt.
*/
void DMA2_Stream7_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream7_IRQn 0 */
/* USER CODE END DMA2_Stream7_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_tx);
/* USER CODE BEGIN DMA2_Stream7_IRQn 1 */
/* USER CODE END DMA2_Stream7_IRQn 1 */
}
/**
* @brief This function handles USART6 global interrupt.
*/
void USART6_IRQHandler(void)
{
/* USER CODE BEGIN USART6_IRQn 0 */
/* USER CODE END USART6_IRQn 0 */
HAL_UART_IRQHandler(&huart6);
/* USER CODE BEGIN USART6_IRQn 1 */
BSP_UART_IRQHandler(&huart6);
/* USER CODE END USART6_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32f4xx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
* @attention
*
* <h2><center>&copy; Copyright (c) 2025 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under Ultimate Liberty license
* SLA0044, the "License"; You may not use this file except in compliance with
* the License. You may obtain a copy of the License at:
* www.st.com/SLA0044
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f4xx_it.h"
#include "FreeRTOS.h"
#include "task.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "bsp/uart.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
extern DMA_HandleTypeDef hdma_i2c2_tx;
extern DMA_HandleTypeDef hdma_i2c3_rx;
extern DMA_HandleTypeDef hdma_spi1_rx;
extern DMA_HandleTypeDef hdma_spi1_tx;
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim7;
extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart6_rx;
extern DMA_HandleTypeDef hdma_usart6_tx;
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart6;
/* USER CODE BEGIN EV */
/* USER CODE END EV */
/******************************************************************************/
/* Cortex-M4 Processor Interruption and Exception Handlers */
/******************************************************************************/
/**
* @brief This function handles Non maskable interrupt.
*/
void NMI_Handler(void)
{
/* USER CODE BEGIN NonMaskableInt_IRQn 0 */
/* USER CODE END NonMaskableInt_IRQn 0 */
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
while (1)
{
}
/* USER CODE END NonMaskableInt_IRQn 1 */
}
/**
* @brief This function handles Hard fault interrupt.
*/
void HardFault_Handler(void)
{
/* USER CODE BEGIN HardFault_IRQn 0 */
/* USER CODE END HardFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
/* USER CODE END W1_HardFault_IRQn 0 */
}
}
/**
* @brief This function handles Memory management fault.
*/
void MemManage_Handler(void)
{
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
/* USER CODE END MemoryManagement_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
/* USER CODE END W1_MemoryManagement_IRQn 0 */
}
}
/**
* @brief This function handles Pre-fetch fault, memory access fault.
*/
void BusFault_Handler(void)
{
/* USER CODE BEGIN BusFault_IRQn 0 */
/* USER CODE END BusFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_BusFault_IRQn 0 */
/* USER CODE END W1_BusFault_IRQn 0 */
}
}
/**
* @brief This function handles Undefined instruction or illegal state.
*/
void UsageFault_Handler(void)
{
/* USER CODE BEGIN UsageFault_IRQn 0 */
/* USER CODE END UsageFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_UsageFault_IRQn 0 */
/* USER CODE END W1_UsageFault_IRQn 0 */
}
}
/**
* @brief This function handles Debug monitor.
*/
void DebugMon_Handler(void)
{
/* USER CODE BEGIN DebugMonitor_IRQn 0 */
/* USER CODE END DebugMonitor_IRQn 0 */
/* USER CODE BEGIN DebugMonitor_IRQn 1 */
/* USER CODE END DebugMonitor_IRQn 1 */
}
/**
* @brief This function handles System tick timer.
*/
void SysTick_Handler(void)
{
/* USER CODE BEGIN SysTick_IRQn 0 */
/* USER CODE END SysTick_IRQn 0 */
HAL_IncTick();
#if (INCLUDE_xTaskGetSchedulerState == 1 )
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED)
{
#endif /* INCLUDE_xTaskGetSchedulerState */
xPortSysTickHandler();
#if (INCLUDE_xTaskGetSchedulerState == 1 )
}
#endif /* INCLUDE_xTaskGetSchedulerState */
/* USER CODE BEGIN SysTick_IRQn 1 */
/* USER CODE END SysTick_IRQn 1 */
}
/******************************************************************************/
/* STM32F4xx Peripheral Interrupt Handlers */
/* Add here the Interrupt Handlers for the used peripherals. */
/* For the available peripheral interrupt handler names, */
/* please refer to the startup file (startup_stm32f4xx.s). */
/******************************************************************************/
/**
* @brief This function handles EXTI line0 interrupt.
*/
void EXTI0_IRQHandler(void)
{
/* USER CODE BEGIN EXTI0_IRQn 0 */
/* USER CODE END EXTI0_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(USER_KEY_Pin);
/* USER CODE BEGIN EXTI0_IRQn 1 */
/* USER CODE END EXTI0_IRQn 1 */
}
/**
* @brief This function handles EXTI line3 interrupt.
*/
void EXTI3_IRQHandler(void)
{
/* USER CODE BEGIN EXTI3_IRQn 0 */
/* USER CODE END EXTI3_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(CMPS_INT_Pin);
/* USER CODE BEGIN EXTI3_IRQn 1 */
/* USER CODE END EXTI3_IRQn 1 */
}
/**
* @brief This function handles EXTI line4 interrupt.
*/
void EXTI4_IRQHandler(void)
{
/* USER CODE BEGIN EXTI4_IRQn 0 */
/* USER CODE END EXTI4_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(ACCL_INT_Pin);
/* USER CODE BEGIN EXTI4_IRQn 1 */
/* USER CODE END EXTI4_IRQn 1 */
}
/**
* @brief This function handles DMA1 stream1 global interrupt.
*/
void DMA1_Stream1_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
/* USER CODE END DMA1_Stream1_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart3_rx);
/* USER CODE BEGIN DMA1_Stream1_IRQn 1 */
/* USER CODE END DMA1_Stream1_IRQn 1 */
}
/**
* @brief This function handles DMA1 stream2 global interrupt.
*/
void DMA1_Stream2_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream2_IRQn 0 */
/* USER CODE END DMA1_Stream2_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_i2c3_rx);
/* USER CODE BEGIN DMA1_Stream2_IRQn 1 */
/* USER CODE END DMA1_Stream2_IRQn 1 */
}
/**
* @brief This function handles CAN1 RX0 interrupts.
*/
void CAN1_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX0_IRQn 0 */
/* USER CODE END CAN1_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX0_IRQn 1 */
/* USER CODE END CAN1_RX0_IRQn 1 */
}
/**
* @brief This function handles CAN1 RX1 interrupt.
*/
void CAN1_RX1_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX1_IRQn 0 */
/* USER CODE END CAN1_RX1_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX1_IRQn 1 */
/* USER CODE END CAN1_RX1_IRQn 1 */
}
/**
* @brief This function handles EXTI line[9:5] interrupts.
*/
void EXTI9_5_IRQHandler(void)
{
/* USER CODE BEGIN EXTI9_5_IRQn 0 */
/* USER CODE END EXTI9_5_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GYRO_INT_Pin);
/* USER CODE BEGIN EXTI9_5_IRQn 1 */
/* USER CODE END EXTI9_5_IRQn 1 */
}
/**
* @brief This function handles TIM1 break interrupt and TIM9 global interrupt.
*/
void TIM1_BRK_TIM9_IRQHandler(void)
{
/* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 0 */
/* USER CODE END TIM1_BRK_TIM9_IRQn 0 */
HAL_TIM_IRQHandler(&htim1);
/* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 1 */
/* USER CODE END TIM1_BRK_TIM9_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 DMA1 stream7 global interrupt.
*/
void DMA1_Stream7_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream7_IRQn 0 */
/* USER CODE END DMA1_Stream7_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_i2c2_tx);
/* USER CODE BEGIN DMA1_Stream7_IRQn 1 */
/* USER CODE END DMA1_Stream7_IRQn 1 */
}
/**
* @brief This function handles TIM7 global interrupt.
*/
void TIM7_IRQHandler(void)
{
/* USER CODE BEGIN TIM7_IRQn 0 */
/* USER CODE END TIM7_IRQn 0 */
HAL_TIM_IRQHandler(&htim7);
/* USER CODE BEGIN TIM7_IRQn 1 */
/* USER CODE END TIM7_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_usart6_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_spi1_rx);
/* 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_spi1_tx);
/* USER CODE BEGIN DMA2_Stream3_IRQn 1 */
/* USER CODE END DMA2_Stream3_IRQn 1 */
}
/**
* @brief This function handles CAN2 RX0 interrupts.
*/
void CAN2_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_RX0_IRQn 0 */
/* USER CODE END CAN2_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_RX0_IRQn 1 */
/* USER CODE END CAN2_RX0_IRQn 1 */
}
/**
* @brief This function handles CAN2 RX1 interrupt.
*/
void CAN2_RX1_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_RX1_IRQn 0 */
/* USER CODE END CAN2_RX1_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_RX1_IRQn 1 */
/* USER CODE END CAN2_RX1_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream5 global interrupt.
*/
void DMA2_Stream5_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream5_IRQn 0 */
/* USER CODE END DMA2_Stream5_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_rx);
/* USER CODE BEGIN DMA2_Stream5_IRQn 1 */
/* USER CODE END DMA2_Stream5_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream6 global interrupt.
*/
void DMA2_Stream6_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream6_IRQn 0 */
/* USER CODE END DMA2_Stream6_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart6_tx);
/* USER CODE BEGIN DMA2_Stream6_IRQn 1 */
/* USER CODE END DMA2_Stream6_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream7 global interrupt.
*/
void DMA2_Stream7_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream7_IRQn 0 */
/* USER CODE END DMA2_Stream7_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_tx);
/* USER CODE BEGIN DMA2_Stream7_IRQn 1 */
/* USER CODE END DMA2_Stream7_IRQn 1 */
}
/**
* @brief This function handles USART6 global interrupt.
*/
void USART6_IRQHandler(void)
{
/* USER CODE BEGIN USART6_IRQn 0 */
/* USER CODE END USART6_IRQn 0 */
HAL_UART_IRQHandler(&huart6);
/* USER CODE BEGIN USART6_IRQn 1 */
BSP_UART_IRQHandler(&huart6);
/* USER CODE END USART6_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

4
DevC.ioc
View File

@ -561,12 +561,12 @@ ProjectManager.ProjectName=DevC
ProjectManager.ProjectStructure=
ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x1000
ProjectManager.TargetToolchain=MDK-ARM V5.32
ProjectManager.TargetToolchain=CMake
ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath=
ProjectManager.UAScriptBeforePath=
ProjectManager.UnderRoot=false
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_ADC3_Init-ADC3-false-HAL-true,6-MX_CAN1_Init-CAN1-false-HAL-true,7-MX_CAN2_Init-CAN2-false-HAL-true,8-MX_I2C1_Init-I2C1-false-HAL-true,9-MX_SPI1_Init-SPI1-false-HAL-true,10-MX_TIM4_Init-TIM4-false-HAL-true,11-MX_TIM5_Init-TIM5-false-HAL-true,12-MX_USART3_UART_Init-USART3-false-HAL-true,13-MX_TIM8_Init-TIM8-false-HAL-true,14-MX_CRC_Init-CRC-false-HAL-true,15-MX_RNG_Init-RNG-false-HAL-true,16-MX_I2C2_Init-I2C2-false-HAL-true,17-MX_I2C3_Init-I2C3-false-HAL-true,18-MX_SPI2_Init-SPI2-false-HAL-true,19-MX_TIM1_Init-TIM1-false-HAL-true,20-MX_TIM3_Init-TIM3-false-HAL-true,21-MX_TIM10_Init-TIM10-false-HAL-true,22-MX_USART1_UART_Init-USART1-false-HAL-true,23-MX_USART6_UART_Init-USART6-false-HAL-true,24-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false,25-MX_TIM7_Init-TIM7-false-HAL-true
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_ADC3_Init-ADC3-false-HAL-true,6-MX_CAN1_Init-CAN1-false-HAL-true,7-MX_CAN2_Init-CAN2-false-HAL-true,8-MX_I2C1_Init-I2C1-false-HAL-true,9-MX_SPI1_Init-SPI1-false-HAL-true,10-MX_TIM4_Init-TIM4-false-HAL-true,11-MX_TIM5_Init-TIM5-false-HAL-true,12-MX_USART3_UART_Init-USART3-false-HAL-true,13-MX_TIM8_Init-TIM8-false-HAL-true,14-MX_CRC_Init-CRC-false-HAL-true,15-MX_RNG_Init-RNG-false-HAL-true,16-MX_I2C2_Init-I2C2-false-HAL-true,17-MX_I2C3_Init-I2C3-false-HAL-true,18-MX_SPI2_Init-SPI2-false-HAL-true,19-MX_TIM1_Init-TIM1-false-HAL-true,20-MX_TIM3_Init-TIM3-false-HAL-true,21-MX_TIM10_Init-TIM10-false-HAL-true,22-MX_USART1_UART_Init-USART1-false-HAL-true,23-MX_USART6_UART_Init-USART6-false-HAL-true,24-MX_TIM7_Init-TIM7-false-HAL-true,25-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true
RCC.48MHZClocksFreq_Value=48000000
RCC.AHBFreq_Value=168000000
RCC.APB1CLKDivider=RCC_HCLK_DIV4

3
User/device/device_config.yaml
View File

@ -18,3 +18,6 @@ motor_lk:
motor_lz:
bsp_config: {}
enabled: true
motor_rm:
bsp_config: {}
enabled: true

498
User/device/motor_dm.c Normal file
View File

@ -0,0 +1,498 @@
#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \
((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1))))
#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \
(((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits))))
/* Includes ----------------------------------------------------------------- */
#include "device/motor_dm.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include "string.h"
#include "math.h"
/* Private constants -------------------------------------------------------- */
/* DM电机数据映射范围 */
#define DM_P_MIN (-12.56637f)
#define DM_P_MAX (12.56637f)
#define DM_V_MIN (-30.0f)
#define DM_V_MAX (30.0f)
#define DM_T_MIN (-12.0f)
#define DM_T_MAX (12.0f)
#define DM_KP_MIN (0.0f)
#define DM_KP_MAX (500.0f)
#define DM_KD_MIN (0.0f)
#define DM_KD_MAX (5.0f)
/* CAN ID偏移量 */
#define DM_CAN_ID_OFFSET_POS_VEL 0x100
#define DM_CAN_ID_OFFSET_VEL 0x200
/* Private macro ------------------------------------------------------------ */
#define FLOAT_TO_UINT(x, x_min, x_max, bits) \
(uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min))
#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \
((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min)
/* Private variables -------------------------------------------------------- */
static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
static int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
/* Converts a float to an unsigned int, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
/* converts unsigned int to float, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
/* Private function prototypes ---------------------------------------------- */
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data);
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output);
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel);
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel);
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can);
/* Private functions -------------------------------------------------------- */
/**
* @brief 解析DM电机反馈帧
* @param motor 电机实例
* @param data CAN数据
* @return 解析结果
*/
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data) {
if (motor == NULL || data == NULL) {
return DEVICE_ERR_NULL;
}
uint16_t p_int=(data[1]<<8)|data[2];
motor->motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5)
uint16_t v_int=(data[3]<<4)|(data[4]>>4);
motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0)
uint16_t t_int=((data[4]&0xF)<<8)|data[5];
motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0)
motor->motor.feedback.temp = (float)(data[6]);
while (motor->motor.feedback.rotor_abs_angle < 0) {
motor->motor.feedback.rotor_abs_angle += M_2PI;
}
while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
motor->motor.feedback.rotor_abs_angle -= M_2PI;
}
if (motor->param.reverse) {
motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
}
return DEVICE_OK;
}
/**
* @brief 发送MIT模式控制命令
* @param motor 电机实例
* @param output MIT控制参数
* @return 发送结果
*/
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) {
if (motor == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8];
uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
uint16_t id = motor->param.can_id;
pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12);
kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12);
tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
/* 打包数据 */
data[0] = (pos_tmp >> 8);
data[1] = pos_tmp;
data[2] = (vel_tmp >> 4);
data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
data[4] = kp_tmp;
data[5] = (kd_tmp >> 4);
data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
data[7] = tor_tmp;
/* 发送CAN消息 */
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
BSP_CAN_WaitTxMailboxEmpty(motor->param.can, 1);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送位置速度模式控制命令
* @param motor 电机实例
* @param pos 目标位置
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &pos, 4); // 位置，低位在前
memcpy(&data[4], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x100 */
uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
BSP_CAN_WaitTxMailboxEmpty(motor->param.can, 1);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送速度模式控制命令
* @param motor 电机实例
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[4] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x200 */
uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 4;
memcpy(frame.data, data, 4);
BSP_CAN_WaitTxMailboxEmpty(motor->param.can, 1);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 获取指定CAN总线的管理器
* @param can CAN总线
* @return CAN管理器指针
*/
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) {
return NULL;
}
return can_managers[can];
}
/**
* @brief 创建CAN管理器
* @param can CAN总线
* @return 创建结果
*/
static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 注册一个DM电机
* @param param 电机参数
* @return 注册结果
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
/* 创建CAN管理器 */
if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) {
return DEVICE_ERR;
}
/* 获取CAN管理器 */
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR;
}
/* 检查是否已注册 */
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
return DEVICE_ERR_INITED;
}
}
/* 检查是否已达到最大数量 */
if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) {
return DEVICE_ERR;
}
/* 分配内存 */
MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t));
if (motor == NULL) {
return DEVICE_ERR;
}
/* 初始化电机 */
memset(motor, 0, sizeof(MOTOR_DM_t));
memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t));
motor->motor.header.online = false;
motor->motor.reverse = param->reverse;
/* 注册CAN接收ID - DM电机使用Master ID接收反馈 */
uint16_t feedback_id = param->master_id;
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
BSP_Free(motor);
return DEVICE_ERR;
}
/* 添加到管理器 */
manager->motors[manager->motor_count] = motor;
manager->motor_count++;
return DEVICE_OK;
}
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return 更新结果
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 查找电机 */
MOTOR_DM_t *motor = NULL;
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
motor = manager->motors[i];
break;
}
}
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 主动接收CAN消息 */
uint16_t feedback_id = param->master_id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒
motor->motor.header.online = false;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data);
return DEVICE_OK;
}
/**
* @brief 更新所有电机数据
* @return 更新结果
*/
int8_t MOTOR_DM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
/**
* @brief MIT模式控制
* @param param 电机参数
* @param output MIT控制参数
* @return 控制结果
*/
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) {
if (param == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendMITCmd(motor, output);
}
/**
* @brief 位置速度模式控制
* @param param 电机参数
* @param target_pos 目标位置
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel);
}
/**
* @brief 速度模式控制
* @param param 电机参数
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendVelCmd(motor, target_vel);
}
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return 电机实例指针
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return NULL;
}
/* 查找对应的电机 */
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor && motor->param.can == param->can &&
motor->param.master_id == param->master_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
frame.data[0] = 0XFF;
frame.data[1] = 0xFF;
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFC;
BSP_CAN_WaitTxMailboxEmpty(motor->param.can, 1);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 使电机松弛（设置输出为0）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_MIT_Output_t output = {0};
return MOTOR_DM_MITCtrl(param, &output);
}
/**
* @brief 使电机离线（设置在线状态为false）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
motor->motor.header.online = false;
return DEVICE_OK;
}

98
User/device/motor_dm.h Normal file
View File

@ -0,0 +1,98 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_DM_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_DM_J4310,
} MOTOR_DM_Module_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t master_id; /* 主站ID，用于发送控制命令 */
uint16_t can_id; /* 反馈ID，用于接收电机反馈 */
MOTOR_DM_Module_t module;
bool reverse;
} MOTOR_DM_Param_t;
/*电机实例*/
typedef struct{
MOTOR_DM_Param_t param;
MOTOR_t motor;
} MOTOR_DM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_DM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param);
/**
* @brief 更新所有电机数据
* @return
*/
int8_t MOTOR_DM_UpdateAll(void);
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output);
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param);
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param);
#ifdef __cplusplus
}
#endif

313
User/device/motor_rm.c Normal file
View File

@ -0,0 +1,313 @@
/*
RM电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_rm.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define GM6020_FB_ID_BASE (0x205)
#define GM6020_CTRL_ID_BASE (0x1ff)
#define GM6020_CTRL_ID_EXTAND (0x2ff)
#define M3508_M2006_FB_ID_BASE (0x201)
#define M3508_M2006_CTRL_ID_BASE (0x200)
#define M3508_M2006_CTRL_ID_EXTAND (0x1ff)
#define M3508_M2006_ID_SETTING_ID (0x700)
#define GM6020_MAX_ABS_LSB (30000)
#define M3508_MAX_ABS_LSB (16384)
#define M2006_MAX_ABS_LSB (10000)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
#define MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */
#define MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006:
case MOTOR_M3508:
if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) {
return can_id - M3508_M2006_FB_ID_BASE;
}
break;
case MOTOR_GM6020:
if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) {
return can_id - GM6020_FB_ID_BASE + 4;
}
break;
default:
break;
}
return DEVICE_ERR;
}
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return 36.0f;
case MOTOR_M3508: return 19.0f;
case MOTOR_GM6020: return 1.0f;
default: return 1.0f;
}
}
static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return M2006_MAX_ABS_LSB;
case MOTOR_M3508: return M3508_MAX_ABS_LSB;
case MOTOR_GM6020: return GM6020_MAX_ABS_LSB;
default: return DEVICE_ERR;
}
}
static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]);
int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]);
int16_t lsb = MOTOR_RM_GetLSB(motor->param.module);
float ratio = MOTOR_RM_GetRatio(motor->param.module);
float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI;
float rotor_speed = raw_speed;
float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES;
if (motor->param.gear) {
// 多圈累加
int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle;
if (delta > (MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count--;
} else if (delta < -(MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count++;
}
motor->last_raw_angle = raw_angle;
float single_turn = rotor_angle;
motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio;
// 输出轴多圈绝对值
motor->feedback.rotor_abs_angle = motor->gearbox_total_angle;
motor->feedback.rotor_speed = rotor_speed / ratio;
motor->feedback.torque_current = torque_current * ratio;
} else {
// 非gear模式，直接用转子单圈
motor->gearbox_round_count = 0;
motor->last_raw_angle = raw_angle;
motor->gearbox_total_angle = 0.0f;
motor->feedback.rotor_abs_angle = rotor_angle;
motor->feedback.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current;
}
motor->feedback.temp = msg->data[6];
motor->motor.feedback.rotor_abs_angle = motor->feedback.rotor_abs_angle;
motor->motor.feedback.rotor_speed = motor->feedback.rotor_speed;
motor->motor.feedback.torque_current = motor->feedback.torque_current;
motor->motor.feedback.temp = motor->feedback.temp;
}
/* Exported functions ------------------------------------------------------- */
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_RM_Decode(motor, &rx_msg);
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_RM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_RM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f;
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module);
if (logical_index < 0) return DEVICE_ERR;
MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(param->module));
output_msg->output[logical_index] = output_value;
return DEVICE_OK;
}
int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t id = param->id;
switch (id) {
case M3508_M2006_FB_ID_BASE:
case M3508_M2006_FB_ID_BASE+1:
case M3508_M2006_FB_ID_BASE+2:
case M3508_M2006_FB_ID_BASE+3:
tx_frame.id = M3508_M2006_CTRL_ID_BASE;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 0; i < 4; i++) {
tx_frame.data[i*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[i*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
break;
case M3508_M2006_FB_ID_BASE+4:
case M3508_M2006_FB_ID_BASE+5:
case M3508_M2006_FB_ID_BASE+6:
case M3508_M2006_FB_ID_BASE+7:
tx_frame.id = M3508_M2006_CTRL_ID_EXTAND;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 4; i < 8; i++) {
tx_frame.data[(i-4)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[(i-4)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
break;
case GM6020_FB_ID_BASE+4:
case GM6020_FB_ID_BASE+5:
case GM6020_FB_ID_BASE+6:
tx_frame.id = GM6020_CTRL_ID_EXTAND;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 8; i < 11; i++) {
tx_frame.data[(i-8)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[(i-8)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
tx_frame.data[6] = 0;
tx_frame.data[7] = 0;
break;
default:
return DEVICE_ERR;
}
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param) {
return MOTOR_RM_SetOutput(param, 0.0f);
}
int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param) {
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

132
User/device/motor_rm.h Normal file
View File

@ -0,0 +1,132 @@
#pragma once
#include "motor.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_RM_MAX_MOTORS 11
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_M2006,
MOTOR_M3508,
MOTOR_GM6020,
} MOTOR_RM_Module_t;
/*一个can最多控制11个电机*/
typedef union {
int16_t output[MOTOR_RM_MAX_MOTORS];
struct {
int16_t m3508_m2006_id201;
int16_t m3508_m2006_id202;
int16_t m3508_m2006_id203;
int16_t m3508_m2006_id204;
int16_t m3508_m2006_gm6020_id205;
int16_t m3508_m2006_gm6020_id206;
int16_t m3508_m2006_gm6020_id207;
int16_t m3508_m2006_gm6020_id208;
int16_t gm6020_id209;
int16_t gm6020_id20A;
int16_t gm6020_id20B;
} named;
} MOTOR_RM_MsgOutput_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
MOTOR_RM_Module_t module;
bool reverse;
bool gear;
} MOTOR_RM_Param_t;
typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t;
typedef struct {
MOTOR_RM_Param_t param;
MOTOR_RM_Feedback_t feedback;
MOTOR_t motor;
// 多圈相关变量，仅gear模式下有效
uint16_t last_raw_angle;
int32_t gearbox_round_count;
float gearbox_total_angle;
} MOTOR_RM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_RM_MsgOutput_t output_msg;
MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_RM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个RM电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值，范围[-1.0, 1.0]
* @return
*/
int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value);
/**
* @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param);
/**
* @brief
* @param
* @return
*/
int8_t MOTOR_RM_UpdateAll(void);
#ifdef __cplusplus
}
#endif

7
User/task/config.yaml
View File

@ -26,3 +26,10 @@
function: Task_ctrl_chassis
name: ctrl_chassis
stack: 512
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_ctrl_gimbal
name: ctrl_gimbal
stack: 256

4
User/task/ctrl_chassis.c
View File

@ -70,4 +70,6 @@ void Task_ctrl_chassis(void *argument) {
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}}
}
}

120
User/task/ctrl_gimbal.c Normal file
View File

@ -0,0 +1,120 @@
/*
ctrl_gimbal Task
*/
/* Includes ----------------------------------------------------------------- */
#include "device/motor_rm.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/motor_dm.h"
#include "device/motor_rm.h"
#include "device/motor.h"
#include "component/pid.h"
#include <stdbool.h>
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
MOTOR_DM_Param_t dm_4310_param = {
.can = BSP_CAN_1,
.master_id = 0x11, // 主站ID
.can_id = 0x01, // 反馈ID 10进制是85
.module = MOTOR_DM_J4310,
.reverse = false,
};
MOTOR_RM_Param_t m2006 = {
.can = BSP_CAN_1,
.id = 0x201,
.module = MOTOR_M2006,
.reverse = false,
.gear = true,
};
MOTOR_MIT_Output_t gimbal_output = {
.angle = 0.0f,
.velocity = 0.0f,
.torque = 0.0f,
.kp = 0.0f,
.kd = 0.0f,
};
MOTOR_DM_t *dm_4310_motor = NULL;
MOTOR_Feedback_t m2006_fb;
MOTOR_Feedback_t dm_4310_fb;
// KPID_Params_t gimbal_angle_pid = {
// .leg_theta={
// .k=1.0f,
// .p=5.0f, /* 摆角比例系数 */
// .i=0.0f, /* 摆角积分系数 */
// .d=0.0f, /* 摆角微分系数 */
// .i_limit=0.0f, /* 积分限幅 */
// .out_limit=0.05f, /* 输出限幅，腿长差值限制 */
// .d_cutoff_freq=30.0f, /* 微分截止频率 */
// .range=-1.0f, /* 不使用循环误差处理 */
// };
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_ctrl_gimbal(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_GIMBAL_FREQ;
osDelay(CTRL_GIMBAL_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
// BSP_CAN_Init();
// MOTOR_DM_Register(&dm_4310_param);
// MOTOR_RM_Register(&m2006);
// MOTOR_DM_Enable(&dm_4310_param); // 使能电机
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
// MOTOR_RM_Update(&m2006);
// MOTOR_DM_Enable(&dm_4310_param); // 使能电机
// MOTOR_DM_MITCtrl(&dm_4310_param, &gimbal_output);
// MOTOR_DM_Update(&dm_4310_param);
// MOTOR_DM_t *dm_4310_motor = MOTOR_DM_GetMotor(&dm_4310_param);
// if (dm_4310_motor) {
// dm_4310_fb = dm_4310_motor->motor.feedback;
// }
// MOTOR_RM_t *m2006_motor = MOTOR_RM_GetMotor(&m2006);
// m2006_fb = m2006_motor->motor.feedback;
// MOTOR_DM_Update(&dm_4310_param);
// if (dm_4310_motor == NULL) {
// dm_4310_motor = MOTOR_DM_GetMotor(&dm_4310_param);
// if (dm_4310_motor == NULL) {
// // 获取电机实例失败，跳过本次循环
// osDelayUntil(tick);
// continue;
// }
// }
// if (dm_4310_motor->motor.header.online == false) {
// MOTOR_DM_Enable(&dm_4310_param); // 使能电机
// } else {
// MOTOR_DM_MITCtrl(&dm_4310_param, &gimbal_output);
// }
// 发送云台控制命令
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

1
User/task/init.c
View File

@ -36,6 +36,7 @@ void Task_Init(void *argument) {
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
// 创建消息队列
/* USER MESSAGE BEGIN */

5
User/task/user_task.c
View File

@ -28,4 +28,9 @@ const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis",
.priority = osPriorityNormal,
.stack_size = 512 * 4,
};
const osThreadAttr_t attr_ctrl_gimbal = {
.name = "ctrl_gimbal",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};

8
User/task/user_task.h
View File

@ -17,6 +17,7 @@ extern "C" {
#define RC_FREQ (500.0)
#define ATTI_ESTI_FREQ (1000.0)
#define CTRL_CHASSIS_FREQ (500.0)
#define CTRL_GIMBAL_FREQ (500.0)
/* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u)
@ -24,6 +25,7 @@ extern "C" {
#define RC_INIT_DELAY (0)
#define ATTI_ESTI_INIT_DELAY (0)
#define CTRL_CHASSIS_INIT_DELAY (500)
#define CTRL_GIMBAL_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
@ -37,6 +39,7 @@ typedef struct {
osThreadId_t rc;
osThreadId_t atti_esti;
osThreadId_t ctrl_chassis;
osThreadId_t ctrl_gimbal;
} thread;
/* USER MESSAGE BEGIN */
@ -66,6 +69,7 @@ typedef struct {
UBaseType_t rc;
UBaseType_t atti_esti;
UBaseType_t ctrl_chassis;
UBaseType_t ctrl_gimbal;
} stack_water_mark;
/* 各任务运行频率 */
@ -74,6 +78,7 @@ typedef struct {
float rc;
float atti_esti;
float ctrl_chassis;
float ctrl_gimbal;
} freq;
/* 任务最近运行时间 */
@ -82,6 +87,7 @@ typedef struct {
float rc;
float atti_esti;
float ctrl_chassis;
float ctrl_gimbal;
} last_up_time;
} Task_Runtime_t;
@ -95,6 +101,7 @@ extern const osThreadAttr_t attr_blink;
extern const osThreadAttr_t attr_rc;
extern const osThreadAttr_t attr_atti_esti;
extern const osThreadAttr_t attr_ctrl_chassis;
extern const osThreadAttr_t attr_ctrl_gimbal;
/* 任务函数声明 */
void Task_Init(void *argument);
@ -102,6 +109,7 @@ void Task_blink(void *argument);
void Task_rc(void *argument);
void Task_atti_esti(void *argument);
void Task_ctrl_chassis(void *argument);
void Task_ctrl_gimbal(void *argument);
#ifdef __cplusplus
}

36
cmake/stm32cubemx/CMakeLists.txt
View File

@ -11,25 +11,17 @@ set(MX_Defines_Syms
# STM32CubeMX generated include paths
set(MX_Include_Dirs
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Inc
${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/App
${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/Target
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Inc
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/include
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Core/Inc
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Device/ST/STM32F4xx/Include
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Include
)
# STM32CubeMX generated application sources
set(MX_Application_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/Target/usbd_conf.c
${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/App/usb_device.c
${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/App/usbd_desc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/App/usbd_cdc_if.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/main.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c
@ -42,6 +34,7 @@ set(MX_Application_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/spi.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/tim.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/usart.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/usb_otg.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/sysmem.c
@ -52,9 +45,9 @@ set(MX_Application_Src
# STM32 HAL/LL Drivers
set(STM32_Drivers_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/system_stm32f4xx.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c
@ -68,9 +61,6 @@ 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_adc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c
@ -79,18 +69,15 @@ set(STM32_Drivers_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c
)
# Drivers Midllewares
set(USB_Device_Library_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c
)
set(FreeRTOS_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/croutine.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c
@ -112,7 +99,7 @@ set(MX_LINK_DIRS
set(MX_LINK_LIBS
STM32_Drivers
${TOOLCHAIN_LINK_LIBRARIES}
USB_Device_Library FreeRTOS
FreeRTOS
)
# Interface library for includes and symbols
add_library(stm32cubemx INTERFACE)
@ -125,11 +112,6 @@ target_sources(STM32_Drivers PRIVATE ${STM32_Drivers_Src})
target_link_libraries(STM32_Drivers PUBLIC stm32cubemx)
# Create USB_Device_Library static library
add_library(USB_Device_Library OBJECT)
target_sources(USB_Device_Library PRIVATE ${USB_Device_Library_Src})
target_link_libraries(USB_Device_Library PUBLIC stm32cubemx)
# Create FreeRTOS static library
add_library(FreeRTOS OBJECT)
target_sources(FreeRTOS PRIVATE ${FreeRTOS_Src})