/* 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 "usart.h" #include "gpio.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include #include #include // 用于isnan、isinf函数 /* 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 */ 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}; /** * @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 = 0x200; 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.075f// 轮子半径 (米) #define WHEELBASE_WIDTH 0.21f // 轮距宽度/2 (米) #define WHEELBASE_LENGTH 0.17f // 轮距长度/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) { // 检查输入参数有效性 if (dt <= 0.0f || dt > 1.0f) { dt = 0.01f; // 使用默认时间间隔 } // 检查error是否为NaN或无穷大 if (isnan(error) || isinf(error)) { error = 0.0f; } // 比例项 float proportional = pid->kp * error; // 检查proportional是否为NaN if (isnan(proportional) || isinf(proportional)) { proportional = 0.0f; } // 积分项（带限幅） 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; // 检查integral是否为NaN if (isnan(integral) || isinf(integral)) { integral = 0.0f; pid->integral = 0.0f; } // 微分项（添加dt保护） float derivative = 0.0f; if (dt > 0.001f) { // 确保dt足够大，避免除以很小的数 derivative = pid->kd * (error - pid->prev_error) / dt; } // 检查derivative是否为NaN if (isnan(derivative) || isinf(derivative)) { derivative = 0.0f; } pid->prev_error = error; // 计算总输出 pid->output = proportional + integral + derivative; // 检查总输出是否为NaN if (isnan(pid->output) || isinf(pid->output)) { pid->output = 0.0f; // 重置PID状态 pid->integral = 0.0f; pid->prev_error = 0.0f; } // 输出限幅 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 = 400.0f; // 速度环比例系数 float speed_ki = 0.05f; // 速度环积分系数 float speed_kd = 300.0f; // 速度环微分系数 for (int i = 0; i < 4; i++) { PID_Init(&motor_controller[i].speed_pid, speed_kp, speed_ki, speed_kd, 3000.0f, 3000.0f); } } /* *函数简介:位置式PID清理 *参数说明:位置式PID参数结构体 *返回类型:无 *备注:重置PID内部状态，用于模式切换或异常情况 */ void PID_PositionClean(PID_Controller_t* pid) { pid->integral = 0.0f; // 清理积分项，防止积分饱和 pid->prev_error = 0.0f; // 清理上一次误差，防止微分冲击 pid->output = 0.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 电机速度控制任务（带数据验证） */ void Motor_Speed_Control_Task(float dt) { for (int i = 0; i < 4; i++) { // 验证电机反馈数据 if (isnan(motor_controller[i].measure.speed_rpm) || motor_controller[i].measure.speed_rpm > 10000 || motor_controller[i].measure.speed_rpm < -10000) { // 数据异常，使用默认值 motor_controller[i].measure.speed_rpm = 0; } // 将电机反馈的RPM转换为rad/s float actual_speed_rads = RPM_To_RadPerSec(motor_controller[i].measure.speed_rpm); // 验证转换结果 if (isnan(actual_speed_rads) || isinf(actual_speed_rads)) { actual_speed_rads = 0.0f; } // 计算速度误差 float speed_error = motor_controller[i].target_speed - actual_speed_rads; // 验证误差值 if (isnan(speed_error) || isinf(speed_error)) { speed_error = 0.0f; } // PID计算 float pid_output = PID_Calculate(&motor_controller[i].speed_pid, speed_error, dt); // 验证PID输出 if (isnan(pid_output) || isinf(pid_output)) { pid_output = 0.0f; // 重置这个电机的PID状态 PID_PositionClean(&motor_controller[i].speed_pid); } // 存储控制命令 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) { float A = WHEELBASE_WIDTH + WHEELBASE_LENGTH; // 标准麦克纳姆轮运动学逆解算公式 // 轮子编号：0-右上, 1-左上, 2-左下, 3-右下 // 标准麦克纳姆轮运动学逆解算公式 float wheel1_speed = ( vx +vy + vz * A ) / WHEEL_RADIUS; // 右上轮 float wheel2_speed = ( vx - vy + vz * A ) / WHEEL_RADIUS; // 左上轮 float wheel3_speed = (-vx -vy + vz * A ) / WHEEL_RADIUS; // 左下轮 float wheel4_speed = (-vx +vy +vz * A ) / 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); // 右下轮 } float Map_Remote_to_Speed(int16_t input, float max_speed) { if (input > 660) input = 660; if (input < -660) input = -660; // 死区内直接返回0 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(); /* USER CODE BEGIN 2 */ dbus_uart_init(); HAL_CAN_Start(&hcan1); Motor_PID_Init(); can_filter_init(); uint32_t last_control_time = 0; float dt = 0; volatile int16_t debug_motor0, debug_motor1, debug_motor2, debug_motor3; /* 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 >= 20) {// 确保时间间隔计算正确 uint32_t time_diff = current_time - last_control_time; if (time_diff == 0) { time_diff = 20; // 防止除以零 } dt = time_diff * 0.001f; // 限制dt在合理范围内 if (dt < 0.005f) dt = 0.005f; if (dt > 0.1f) dt = 0.1f; 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; debug_motor0 = motor_commands[0]; debug_motor1 = motor_commands[1]; debug_motor2 = motor_commands[2]; debug_motor3 = motor_commands[3]; } 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 */