714 lines
20 KiB
C
714 lines
20 KiB
C
/* 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 <string.h>
|
||
#include <stdlib.h>
|
||
#include <math.h> // 用于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 */
|