/* USER CODE BEGIN Header */ /** ****************************************************************************** * @file : main.c * @brief : Main program body ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" #include "can.h" #include "dma.h" #include "tim.h" #include "usart.h" #include "gpio.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include #include /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* 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 */ uint8_t dbus_buf[DBUS_BUFLEN]; rc_info_t rc = rc_Init; extern UART_HandleTypeDef huart3; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); /* USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ //can总线电机6020 static int uart_receive_dma_no_it(UART_HandleTypeDef* huart, uint8_t* pData, uint32_t Size) { uint32_t tmp1 = 0; tmp1 = huart->RxState; if (tmp1 == HAL_UART_STATE_READY) { if ((pData == NULL) || (Size == 0)) { return HAL_ERROR; } huart->pRxBuffPtr = pData; huart->RxXferSize = Size; huart->ErrorCode = HAL_UART_ERROR_NONE; HAL_DMA_Start(huart->hdmarx, (uint32_t)&huart->Instance->DR, (uint32_t)pData, Size); SET_BIT(huart->Instance->CR3, USART_CR3_DMAR); return HAL_OK; } else { return HAL_BUSY; } } void dbus_uart_init(void) { __HAL_UART_CLEAR_IDLEFLAG(&DBUS_HUART); __HAL_UART_ENABLE_IT(&DBUS_HUART, UART_IT_IDLE); uart_receive_dma_no_it(&DBUS_HUART, dbus_buf, DBUS_MAX_LEN); } void rc_callback_handler(rc_info_t *rc, uint8_t *buff) { rc->ch0 = (buff[0] | buff[1] << 8) & 0x07FF; rc->ch0 -= 1024; rc->ch1 = (buff[1] >> 3 | buff[2] << 5) & 0x07FF; rc->ch1 -= 1024; rc->ch2 = (buff[2] >> 6 | buff[3] << 2 | buff[4] << 10) & 0x07FF; rc->ch2 -= 1024; rc->ch3 = (buff[4] >> 1 | buff[5] << 7) & 0x07FF; rc->ch3 -= 1024; rc->roll = (buff[16] | (buff[17] << 8)) & 0x07FF; rc->roll -= 1024; rc->sw1 = ((buff[5] >> 4) & 0x000C) >> 2; rc->sw2 = (buff[5] >> 4) & 0x0003; if ((abs(rc->ch0) > 660) || \ (abs(rc->ch1) > 660) || \ (abs(rc->ch2) > 660) || \ (abs(rc->ch3) > 660)) { memset(rc, 0, sizeof(rc_info_t)); } } uint16_t dma_current_data_counter(DMA_Stream_TypeDef *dma_stream) { return ((uint16_t)(dma_stream->NDTR)); } static void uart_rx_idle_callback(UART_HandleTypeDef* huart) { __HAL_UART_CLEAR_IDLEFLAG(huart); if (huart == &DBUS_HUART) { __HAL_DMA_DISABLE(huart->hdmarx); if ((DBUS_MAX_LEN - dma_current_data_counter(huart->hdmarx->Instance)) == DBUS_BUFLEN) { rc_callback_handler(&rc, dbus_buf); } __HAL_DMA_SET_COUNTER(huart->hdmarx, DBUS_MAX_LEN); __HAL_DMA_ENABLE(huart->hdmarx); } } void uart_receive_handler(UART_HandleTypeDef *huart) { if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE) && __HAL_UART_GET_IT_SOURCE(huart, UART_IT_IDLE)) { uart_rx_idle_callback(huart); } }typedef struct { uint16_t ecd;//编码器值 -16384--+16384 int16_t speed_rpm;//转速 转每分钟 int16_t given_current;//输出电流值 uint8_t temperate;//电机温度 int16_t last_ecd;//上次编码器值 } motor_measure_t; motor_measure_t motor_chassis[4] = {0}; //void Drive_Motor(float vx, float vy, float vz); //void Motor_Speed_Control_Task(float dt); //void Send_Motor_Commands(void); //void Motor_PID_Init(void); //void Update_Motor_Measure(uint8_t motor_id, motor_measure_t* measure); //void Print_Motor_Status(void); //void Update_Motor_Measure(uint8_t motor_id, motor_measure_t* measure); /** * @brief 更新电机测量数据到控制器 * @param motor_id: 电机ID (0-3) * @param measure: 电机测量数据指针 */ static CAN_TxHeaderTypeDef chassis_tx_message; static uint8_t chassis_can_send_data[8]; void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4) //can总线数据发送函数 //motor1-右上 motor2-左上 motor3-左下 motor4-右下 //以c板的正方向为基准 { uint32_t send_mail_box; chassis_tx_message.StdId = 0x1FF; chassis_tx_message.IDE = CAN_ID_STD; chassis_tx_message.RTR = CAN_RTR_DATA; chassis_tx_message.DLC = 0x08; chassis_can_send_data[0] = motor1 >> 8; chassis_can_send_data[1] = motor1; chassis_can_send_data[2] = motor2 >> 8; chassis_can_send_data[3] = motor2; chassis_can_send_data[4] = motor3 >> 8; chassis_can_send_data[5] = motor3; chassis_can_send_data[6] = motor4 >> 8; chassis_can_send_data[7] = motor4; HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box); } //滤波器 只接受 ID 1-4 的电机 //此段后续可优化为列表模式 void can_filter_init(void) { CAN_FilterTypeDef can_filter_st; can_filter_st.FilterActivation = ENABLE; can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK; can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT; can_filter_st.FilterIdHigh = 0x0000;//高八位 can_filter_st.FilterIdLow = 0x0000;//低八位 can_filter_st.FilterMaskIdHigh = 0x0000; can_filter_st.FilterMaskIdLow = 0x0007; can_filter_st.FilterBank = 0; can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0; HAL_CAN_ConfigFilter(&hcan1, &can_filter_st); HAL_CAN_Start(&hcan1); HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); can_filter_st.SlaveStartFilterBank = 14; can_filter_st.FilterBank = 14; HAL_CAN_ConfigFilter(&hcan1, &can_filter_st); HAL_CAN_Start(&hcan1); HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); } //过滤器笔记 //SlaveStartFilterBank参数只有在双CAN模式（CAN1和CAN2）下才需要，如果只有CAN1，可以不用设置 // PID控制器结构体 typedef struct { float kp; // 比例系数 float ki; // 积分系数 float kd; // 微分系数 float integral; // 积分项 float prev_error; // 上一次误差 float output; // 输出值 float integral_limit; // 积分限幅 float output_limit; // 输出限幅 } PID_Controller_t; // 电机控制结构体 typedef struct { motor_measure_t measure; // 电机测量值（速度单位：RPM） PID_Controller_t speed_pid; // 速度环PID float target_speed; // 目标速度 (rad/s) } Motor_Controller_t; // 四个电机的控制器 Motor_Controller_t motor_controller[4] = {0}; // 存储四个电机的控制命令 int16_t motor_commands[4] = {0}; // 底盘参数（根据实际机械结构调整） #define WHEEL_RADIUS 0.175f // 轮子半径 (米) #define WHEELBASE_WIDTH 0.135f // 轮距宽度/2 (米) #define WHEELBASE_LENGTH 0.2f // 轮距长度/2 (米) /** * @brief 更新电机测量数据到控制器 * @param motor_id: 电机ID (0-3) * @param measure: 电机测量数据指针 */ void Update_Motor_Measure(uint8_t motor_id, motor_measure_t* measure) { if (motor_id < 4) { // 直接结构体赋值（更简洁） motor_controller[motor_id].measure = *measure; } } //中断回调函数 //判断接受对象的ID-符合条件-完成解码-发送数据包 //motor_chassis：motor_measure_t 类型的数组，其中装有电机转子角度，电机转子转速，控制电流，温度 //定义函数 #define get_motor_measure(ptr, data) \ { \ (ptr)->last_ecd = (ptr)->ecd; \ (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \ (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \ (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \ (ptr)->temperate = (data)[6]; \ } //can传回参数结构体定义 //回调函数 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { CAN_RxHeaderTypeDef rx_header; uint8_t rx_data[8]; HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data); switch (rx_header.StdId) { case 1: case 2: case 3: case 4: { static uint8_t i = 0; //读取电机ID i = rx_header.StdId - 1; get_motor_measure(&motor_chassis[i], rx_data); Update_Motor_Measure(i, &motor_chassis[i]); break; } default: { break; } } } //HAL库接收函数HAL_CAN_GetRxMessage HAL_StatusTypeDef HAL_CAN_GetRxMessage(CAN_HandleTypeDef *hcan, uint32_t RxFifo, CAN_RxHeaderTypeDef *pHeader, uint8_t aData[]); /** * @brief 将RPM转换为rad/s */ float RPM_To_RadPerSec(int16_t rpm) { return (float)rpm * 2.0f * 3.1415926535f / 60.0f; } /** * @brief 将rad/s转换为RPM */ int16_t RadPerSec_To_RPM(float rad_per_sec) { return (int16_t)(rad_per_sec * 60.0f / (2.0f * 3.1415926535f)); } /** * @brief PID控制器初始化 */ void PID_Init(PID_Controller_t* pid, float kp, float ki, float kd, float integral_limit, float output_limit) { pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->integral = 0.0f; pid->prev_error = 0.0f; pid->output = 0.0f; pid->integral_limit = integral_limit; pid->output_limit = output_limit; } /** * @brief PID计算函数 */ float PID_Calculate(PID_Controller_t* pid, float error, float dt) { // 比例项 float proportional = pid->kp * error; // 积分项（带限幅） pid->integral += error * dt; if (pid->integral > pid->integral_limit) { pid->integral = pid->integral_limit; } else if (pid->integral < -pid->integral_limit) { pid->integral = -pid->integral_limit; } float integral = pid->ki * pid->integral; // 微分项 float derivative = pid->kd * (error - pid->prev_error) / dt; pid->prev_error = error; // 计算总输出 pid->output = proportional + integral + derivative; // 输出限幅 if (pid->output > pid->output_limit) { pid->output = pid->output_limit; } else if (pid->output < -pid->output_limit) { pid->output = -pid->output_limit; } return pid->output; } /** * @brief 初始化所有电机的PID控制器 */ void Motor_PID_Init(void) { // PID参数（需要根据实际电机调试） float speed_kp = 5.0f; // 速度环比例系数 float speed_ki = 0.5f; // 速度环积分系数 float speed_kd = 0.1f; // 速度环微分系数 for (int i = 0; i < 4; i++) { PID_Init(&motor_controller[i].speed_pid, speed_kp, speed_ki, speed_kd, 1000.0f, 16000.0f); } } /** * @brief 设置单个电机的目标速度 * @param motor_id: 电机ID (0-3) * @param target_speed_rad_s: 目标速度 (rad/s) */ void Set_Motor_Target_Speed(uint8_t motor_id, float target_speed_rad_s) { if (motor_id < 4) { motor_controller[motor_id].target_speed = target_speed_rad_s; } } /** * @brief 电机速度控制任务（需要定期调用） * @param dt: 时间间隔 (秒) */ void Motor_Speed_Control_Task(float dt) { for (int i = 0; i < 4; i++) { // 将电机反馈的RPM转换为rad/s float actual_speed_rads = RPM_To_RadPerSec(motor_controller[i].measure.speed_rpm); // 计算速度误差（单位：rad/s） float speed_error = motor_controller[i].target_speed - actual_speed_rads; // PID计算 float pid_output = PID_Calculate(&motor_controller[i].speed_pid, speed_error, dt); // 存储控制命令 motor_commands[i] = (int16_t)pid_output; } } /** * @brief 麦克纳姆轮速度解算函数 * @param vx: X方向速度 (m/s) * @param vy: Y方向速度 (m/s) * @param vz: 旋转角速度 (rad/s) */ void Drive_Motor(float vx, float vy, float vz) { // 麦克纳姆轮运动学逆解算（输出单位：rad/s） float wheel1_speed = (+vx + vy + vz * (WHEELBASE_WIDTH + WHEELBASE_LENGTH)) / WHEEL_RADIUS; float wheel2_speed = (+vx - vy - vz * (WHEELBASE_WIDTH + WHEELBASE_LENGTH)) / WHEEL_RADIUS; float wheel3_speed = (+vx + vy - vz * (WHEELBASE_WIDTH + WHEELBASE_LENGTH)) / WHEEL_RADIUS; float wheel4_speed = (+vx - vy + vz * (WHEELBASE_WIDTH + WHEELBASE_LENGTH)) / WHEEL_RADIUS; Set_Motor_Target_Speed(0, wheel1_speed); // 右上轮 Set_Motor_Target_Speed(1, wheel2_speed); // 左上轮 Set_Motor_Target_Speed(2, wheel3_speed); // 左下轮 Set_Motor_Target_Speed(3, wheel4_speed); // 右下轮 } /** * @brief 将遥控器值(-660~660)映射为速度值(m/s) * @param input: 遥控器输入值 (-660 到 660) * @param max_speed: 最大速度 (m/s) * @return 映射后的速度值 */ float Map_Remote_to_Speed(int16_t input, float max_speed) { // 限制输入范围 if (input > 660) input = 660; if (input < -660) input = -660; // 死区处理（避免摇杆微小抖动） if (abs(input) < 50) { return 0.0f; } // 线性映射到速度 return (input / 660.0f) * max_speed; } /** * @brief 处理遥控器数据并设置底盘速度 */ void Process_Remote_Control(void) { // 映射遥控器值到速度 (m/s) float vx = Map_Remote_to_Speed(rc.ch2, 2.0f); // 前后方向，最大2m/s float vy = Map_Remote_to_Speed(rc.ch3, 2.0f); // 左右方向，最大2m/s float vz = Map_Remote_to_Speed(rc.ch0, 3.0f); // 旋转方向，最大3rad/s // 调用运动学解算 Drive_Motor(vx, vy, vz); } /* USER CODE END 0 */ /** * @brief The application entry point. * @retval int */ int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_DMA_Init(); MX_CAN1_Init(); MX_USART3_UART_Init(); MX_TIM1_Init(); /* USER CODE BEGIN 2 */ dbus_uart_init(); HAL_CAN_Start(&hcan1); Motor_PID_Init(); can_filter_init(); uint32_t last_control_time = 0; uint32_t dt = 0; /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { uint32_t current_time = HAL_GetTick(); // 控制任务：固定频率执行（100Hz，10ms） if (current_time - last_control_time >= 10) { dt = (current_time - last_control_time) * 0.001f; Process_Remote_Control( ); Motor_Speed_Control_Task(dt); CAN_cmd_chassis(motor_commands[0],motor_commands[1],motor_commands[2],motor_commands[3]); last_control_time=current_time; } HAL_Delay(1); /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ } /** * @brief System Clock Configuration * @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Configure the main internal regulator output voltage */ __HAL_RCC_PWR_CLK_ENABLE(); __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); /** Initializes the RCC Oscillators according to the specified parameters * in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; RCC_OscInitStruct.PLL.PLLM = 8; RCC_OscInitStruct.PLL.PLLN = 168; RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; RCC_OscInitStruct.PLL.PLLQ = 4; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) { Error_Handler(); } } /* USER CODE BEGIN 4 */ /* USER CODE END 4 */ /** * @brief This function is executed in case of error occurrence. * @retval None */ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ /* User can add his own implementation to report the HAL error return state */ __disable_irq(); while (1) { } /* USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** * @brief Reports the name of the source file and the source line number * where the assert_param error has occurred. * @param file: pointer to the source file name * @param line: assert_param error line source number * @retval None */ void assert_failed(uint8_t *file, uint32_t line) { /* USER CODE BEGIN 6 */ /* User can add his own implementation to report the file name and line number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ /* USER CODE END 6 */ } #endif /* USE_FULL_ASSERT */