#include "stm32f10x.h" // Device header
#include "Delay.h"
#include "Motor.h"
#include "Trace.h"
#include "mycontrol.h"
#include "PWM.h"
#include "OLED.h"
#include "Timer.h"
#include "Encoder.h"
#include "incremental_pid.h"
#define SAMPLE_SIZE 5
int16_t right_speed_samples[SAMPLE_SIZE]={0};//数组缓冲区
int16_t left_speed_samples[SAMPLE_SIZE]={0};
static uint8_t sample_index = 0;// 采样缓冲区索引 [0 ~ SAMPLE_SIZE-1]
// 编码器参数
#define ENCODER_PPR 1024 // 每转脉冲数
#define ENCODER_CPR (ENCODER_PPR * 4) //四倍频
#define WHEEL_CIRCUMFERENCE 0.19f // 轮子周长(米),半径3cm → ~0.1884m
#define SAMPLE_TIME_MS 10 // 采样时间(ms),必须和主循环一致
int16_t LeftSpeed=0;
int16_t RightSpeed=0;
uint16_t Compare3=0;//控制右侧电机
uint16_t Compare4=0;//控制左侧电机
//extern float target_speed_ticks_10ms;
extern float target_left_ticks_10ms;
extern float target_right_ticks_10ms;
volatile float g_filtered_left_tick = 0;
volatile float g_filtered_right_tick = 0;
//float target_speed_ticks_10ms = 5.0f; // 单位:ticks / 10ms 表示每10ms接收多少脉冲,以此来表示速度
volatile uint8_t display_update_needed = 0;
volatile uint8_t display_counter = 0;
volatile uint8_t control_update_needed = 0;
volatile float left_output;
volatile float right_output;
extern void Motor_Control(void);
int main(void)
{
Timer1_Init();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
PWM_Init();
Motor_DIR_Init();
OLED_Init();
Trace_Init();
Delay_init();
// 内环速度PID(增量式)—— 使用新结构
IncPID_Init(&left_speed_pid, 300.0f, 0.0f, 0); // Kp=10, Ki=0.5, Kd=2
IncPID_Init(&right_speed_pid, 300.0f, 0.0f, 0); //2.0 0.1 0
//2.72 0.1 0 这个数据小车左右轮同时响应
Left_Encoder_Init();
Right_Encoder_Init();
while (1)
{
// 在主循环中进行循迹决策
if (control_update_needed)
{
control_update_needed = 0; // 清标志
Trace_Update(); // 更新灰度传感器状态
Motor_Control(); // 重新计算 target_left/right_ticks_10ms
}
if (display_update_needed)
{
display_update_needed = 0;
// 显示真实 m/s(仅用于显示,不影响 PID)
float display_left_mps = ((float)g_filtered_left_tick) * WHEEL_CIRCUMFERENCE / (ENCODER_CPR * 0.01f);
float display_right_mps = ((float)g_filtered_right_tick) * WHEEL_CIRCUMFERENCE / (ENCODER_CPR * 0.01f);
// 更新显示代码
// OLED_ShowNum(1, 1, (uint32_t)(display_left_mps * 100), 3);//左轮的真实速度,单位m/s
//OLED_ShowNum(1, 6, (uint32_t)(display_right_mps * 100), 3);//右轮的真实速度,单位m/s
OLED_ShowNum(2, 1, (uint32_t)(target_left_ticks_10ms * 100), 3);//目标速度
OLED_ShowNum(2, 6, (uint32_t)(target_right_ticks_10ms * 100), 3);
OLED_ShowSignedNum(3, 1,g_filtered_left_tick, 3);//左轮编码器反馈的脉冲数
OLED_ShowSignedNum(3, 6, g_filtered_right_tick, 3);//右轮编码器反馈的脉冲数
OLED_ShowSignedNum(4, 1, (int16_t)(left_output * 10), 5); //增量式pid算出的左轮的output // 显示 ×0.1
OLED_ShowSignedNum(4, 8, (int16_t)(right_output * 10), 5);//增量式pid算出的右轮的output // 显示 ×0.1
}
}
}
//定时中断函数
// 在定时器中断中添加计数器
void TIM1_UP_IRQHandler(void)
{
if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
// 读取编码器增量并清零计数器(去噪 + 降频更新显示)
int16_t new_left_speed = Left_Encoder_Get();
int16_t new_right_speed = Right_Encoder_Get();
right_speed_samples[sample_index] = new_right_speed;
left_speed_samples[sample_index] = new_left_speed;
sample_index = (sample_index + 1) % SAMPLE_SIZE;
// 对采样值进行排序
int16_t sorted_right[SAMPLE_SIZE];
int16_t sorted_left[SAMPLE_SIZE];
for (int i = 0; i < SAMPLE_SIZE; i++) {
sorted_right[i] = right_speed_samples[i];
sorted_left[i] = left_speed_samples[i];
}
for (int i = 0; i < SAMPLE_SIZE - 1; i++) {
for (int j = i + 1; j < SAMPLE_SIZE; j++) {
if (sorted_right[i] > sorted_right[j]) {
int16_t temp = sorted_right[i];
sorted_right[i] = sorted_right[j];
sorted_right[j] = temp;
}
if (sorted_left[i] > sorted_left[j]) {
int16_t temp = sorted_left[i];
sorted_left[i] = sorted_left[j];
sorted_left[j] = temp;
}
}
}
// 取中间值
int16_t Right_Speed_Feedback = sorted_right[SAMPLE_SIZE / 2];
int16_t Left_Speed_Feedback = sorted_left[SAMPLE_SIZE / 2];
// 更新 left_ticks / right_ticks
Encoder_Get_Speed(Left_Speed_Feedback, Right_Speed_Feedback);
g_filtered_left_tick = Left_Speed_Feedback;
g_filtered_right_tick = Right_Speed_Feedback;
//增量式 PID 控制开始
// 左轮 PID 控制
left_output = IncPID_Calc(&left_speed_pid,target_left_ticks_10ms, Left_Speed_Feedback);
// 右轮 PID 控制
right_output = IncPID_Calc(&right_speed_pid,target_right_ticks_10ms, Right_Speed_Feedback);
//根据输出控制 PWM 和方向
// 处理左侧电机
if (left_output >= 0)
{
Motor_DIR_Left_Forward(); // 正转
Compare4 = (uint16_t)left_output; // PWM 值
}
else
{
Motor_DIR_Left_Backward(); // 反转
Compare4 = (uint16_t)(-left_output); // 取正值
}
// 处理右侧电机
if (right_output >= 0)
{
Motor_DIR_Right_Forward();
Compare3 = (uint16_t)right_output;
}
else
{
Motor_DIR_Right_Backward();
Compare3 = (uint16_t)(-right_output);
}
// 限幅处理(防止超过PWM最大值)//88 100 我的限幅放在incremental_pid.c函数中
///if (Compare3 > 500) Compare3 = 500;
//if (Compare4 > 500) Compare4 = 500;
// 写入 PWM 比较寄存器(更新占空比)
TIM_SetCompare3(TIM2, Compare3); //控制右电机
TIM_SetCompare4(TIM2, Compare4); //控制左电机
//设标志:告诉主循环可以更新目标速度了
control_update_needed = 1;
// 每隔 5 次中断更新一次显示
display_counter++;
if (display_counter >= 5)
{
display_counter = 0;
display_update_needed = 1; // 设置标志
}
}
}
这是main.c#include "stm32f10x.h"
#include "core_cm3.h"
// ------------------ 静态全局变量 ------------------
__IO uint32_t g_millis = 0; // 必须放在文件作用域,且 static 隐藏内部实现
// ------------------ 函数声明 ------------------
// (可选:如果函数互相调用才需要提前声明)
// ------------------ 函数定义 ------------------
/**
* @brief 初始化系统滴答定时器 (SysTick),设置为每 1ms 中断一次
* @param 无
* @retval 无
*/
void Delay_init(void)
{
if (SysTick_Config(SystemCoreClock / 1000)) {
while(1); // 初始化失败,死循环报错
}
}
/**
* @brief 获取当前系统运行时间(毫秒)
* @param 无
* @retval 当前毫秒数,从启动开始累计,约 49.7 天回滚
*/
uint32_t millis(void)
{
return g_millis;
}
/**
* @brief 微秒级延时(适用于主频 72MHz)
* @param xus 延时时长(单位:微秒),推荐范围:1~800,000(即 < 0.8s)
* @note 依赖 SysTick 持续运行,不可用于中断服务程序(ISR)
* @retval 无
*/
void Delay_us(uint32_t xus)
{
uint32_t start = SysTick->VAL;
uint32_t ticks = (SystemCoreClock / 1000000) * xus;
uint32_t reload = SysTick->LOAD; // 通常是 SystemCoreClock/1000 - 1
while (1) {
uint32_t delta = start - SysTick->VAL;
if ((SysTick->CTRL & 0x00010000) != 0) { // 是否发生 COUNTFLAG 置位
delta += reload + 1; // 加上完整的重载值(注意是 LOAD+1)
}
if (delta >= ticks) break;
}
}
/**
* @brief 毫秒级延时(非阻塞式,基于 millis)
* @param ms 延时时长(单位:毫秒)
* @note 在此期间其他任务无法执行,请谨慎用于长时间延时
* @retval 无
*/
void Delay_ms(uint32_t ms)
{
uint32_t start = millis();
while (millis() - start < ms);
}
/**
* @brief 秒级延时(基于 delay_ms)
* @param xs 延时时长(单位:秒)
* @note ⚠️ 完全阻塞 CPU,不推荐用于实际控制任务
* @retval 无
*/
void Delay_s(uint32_t xs)
{
while (xs--) {
Delay_ms(1000);
}
}
void SysTick_Handler(void)
{
g_millis++;
}
这是delay.c#include "stm32f10x.h" // Device header
void Timer1_Init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
TIM_InternalClockConfig(TIM1);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 10000 - 1; //设置 TIM1 每 10ms 中断一次
TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1;
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0 ;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStructure);
TIM_ClearFlag(TIM1, TIM_FLAG_Update);
TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_Init(&NVIC_InitStructure);
TIM_Cmd(TIM1, ENABLE);
}
/*void TIM1_UP_IRQHandler(void)
{
if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
}
}
*/
这是timer.c#include "stm32f10x.h" // Device header
#include "Motor.h"
void PWM_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2|GPIO_Pin_3 ;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
TIM_InternalClockConfig(TIM2);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 900 - 1; //ARR
TIM_TimeBaseInitStructure.TIM_Prescaler = 8 - 1; //PSC
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0 ;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure);
//加入 PWM 死区保护(防击穿)
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
//PWM1控制右侧电机
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0 ; //CCR(可能需要调整!!!)
TIM_OC3Init(TIM2,&TIM_OCInitStructure);//使用TIM2定时器的CH3通道!!!
//PWM2控制左侧电机
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0 ; //CCR(可能需要调整!!!)
TIM_OC4Init(TIM2,&TIM_OCInitStructure);//使用TIM2定时器的CH4通道!!!
TIM_Cmd(TIM2, ENABLE);
TIM_SetCompare3(TIM2, 0);
TIM_SetCompare4(TIM2, 0);
}
void PWM_SetCompare3(uint16_t Compare3)//控制右侧电机的转速
{
TIM_SetCompare3(TIM2, Compare3);
}
void PWM_SetCompare4(uint16_t Compare4)//控制左侧电机的转速
{
TIM_SetCompare4(TIM2, Compare4);
}
这是pwm.c#include "stm32f10x.h" // Device header
#include "PWM.h"
void Motor_DIR_Init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5;//DIR1控制右测电机!
GPIO_Init(GPIOA, &GPIO_InitStructure); //DIR2控制左测电机!
PWM_Init();
}
void Motor_DIR_Left_Forward(void)
{
GPIO_ResetBits(GPIOA, GPIO_Pin_5);
}
void Motor_DIR_Left_Backward(void)
{
GPIO_SetBits(GPIOA, GPIO_Pin_5);
}
void Motor_DIR_Right_Forward(void)
{
GPIO_ResetBits(GPIOA, GPIO_Pin_4);
}
void Motor_DIR_Right_Backward(void)
{
GPIO_SetBits(GPIOA, GPIO_Pin_4);
}
这是motor.c#include "stm32f10x.h" // Device header
#include "Delay.h"
#include "Trace.h"
uint8_t X1, X2, X3, X4, X5, X6, X7, X8;
void Trace_Init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_8|GPIO_Pin_9|GPIO_Pin_10|GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
void Trace_Update(void)
{
X1 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_0);
X2 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_1);
X3 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4);
X4 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_5);
X5 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_8);
X6 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_9);
X7 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_10);
X8 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11);
}
//计算加权平均误差值(基于8路灰度传感器)//
float calculate_weighted_error() {
//新方法
// 定义每个传感器的位置权重(模拟横向坐标)
// 假设从左到右:X1 X2 X3 X4 X5 X6 X7 X8
// 权重设置为: -7, -5, -3, -1, +1, +3, +5, +7
const int w1 = -7, w2 = -5, w3 = -3, w4 = -1;
const int w5 = +1, w6 = +3, w7 = +5, w8 = +7;
int weighted_sum = 0; // 加权和
int active_count = 0; // 检测到黑线的传感器数量
// 判断每个传感器是否检测到黑线(0 表示黑线)
if (X1 == 0) { weighted_sum += w1; active_count++; }
if (X2 == 0) { weighted_sum += w2; active_count++; }
if (X3 == 0) { weighted_sum += w3; active_count++; }
if (X4 == 0) { weighted_sum += w4; active_count++; }
if (X5 == 0) { weighted_sum += w5; active_count++; }
if (X6 == 0) { weighted_sum += w6; active_count++; }
if (X7 == 0) { weighted_sum += w7; active_count++; }
if (X8 == 0) { weighted_sum += w8; active_count++; }
static float last_valid_error = 0.0f; // 上一次有效的误差
static uint8_t lost_counter = 0; // 连续脱线次数
const uint8_t MAX_LOST_COUNT = 5; // 最大脱线次数(防止无限增长)
// 情况1:完全脱线(全白)
if (active_count == 0)
{
lost_counter++;
if (lost_counter > MAX_LOST_COUNT) lost_counter = MAX_LOST_COUNT;
// 渐进式试探:根据上次方向,逐步加大转向力度
float base_correction = (float)(2 * lost_counter); // 2 → 4 → 6
return (last_valid_error >= 0) ? base_correction : -base_correction;
}
// 情况2:全黑(可能是终点或交叉口)
if (active_count == 8)
{
lost_counter = 0;
last_valid_error = 0.0f;
return 0.0f; // 居中处理
}
// 正常情况:计算加权平均误差
float error = (float)weighted_sum / active_count;
lost_counter = 0; // 重新看到线,重置脱线计数
last_valid_error = error; // 更新最后有效误差
return error;
}
这是trace.c#include "stm32f10x.h" // Device header
#include "Delay.h"
// 全局变量
volatile int16_t Left_Encoder_Count = 0;
volatile int16_t Right_Encoder_Count = 0;
float Left_Speed_Feedback = 0.0f;
float Right_Speed_Feedback = 0.0f;
/*TIM3初始化为编码器接口*/
void Left_Encoder_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 65536 - 1; //ARR
TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1; //PSC
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0 ;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStructure);
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInitStructure.TIM_ICFilter = 0xF;// 滤波(去抖)
TIM_ICInit(TIM3, &TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
TIM_Cmd(TIM3, ENABLE);
}
/* 获取左编码器本次增量(单位:脉冲数) */
int16_t Left_Encoder_Get(void)
{
int16_t temp;
temp = (int16_t)TIM_GetCounter(TIM3); // 强制转为有符号16位
TIM_SetCounter(TIM3, 0); // 清零,准备下次测量
return -temp;
}
//*TIM4初始化为编码器接口*/
void Right_Encoder_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 65536 - 1; //ARR
TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1; //PSC
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0 ;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStructure);
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM4, &TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM4, &TIM_ICInitStructure);
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising , TIM_ICPolarity_Falling);
TIM_Cmd(TIM4, ENABLE);
}
/* 获取右编码器本次增量(单位:脉冲数) */
int16_t Right_Encoder_Get(void)
{
int16_t temp;
temp = (int16_t)TIM_GetCounter(TIM4);
TIM_SetCounter(TIM4, 0);
return -temp;
}
// 直接将编码器增量作为“速度”反馈值(无需转换为 m/s 或 RPM)
// 假设采样周期是 10ms,则 left_ticks 就是 "每 10ms 的脉冲数",可代表速度大小
void Encoder_Get_Speed(int16_t left_ticks, int16_t right_ticks)
{
// 直接将编码器增量作为“速度”反馈值(无需转换为 m/s 或 RPM)
// 假设采样周期是 10ms,则 left_ticks 就是 "每 10ms 的脉冲数",可代表速度大小
Left_Speed_Feedback = (float)left_ticks; // 用左轮脉冲数作为速度量
Right_Speed_Feedback = (float)right_ticks; // 用右轮脉冲数作为速度量
}
这是encoder.c#include "incremental_pid.h"
#include "stm32f10x.h" // Device header
#include "Trace.h"
#define SAMPLE_TIME_MS 10.0f
#define DT (SAMPLE_TIME_MS / 1000.0f) // 0.01s
#define PID_MAX_OUTPUT 150.0f
#define PID_MIN_OUTPUT -150.0f
// 全局 PID 实例定义
Incremental_PID left_speed_pid;
Incremental_PID right_speed_pid;
/**
* @brief 初始化增量式PID控制器
* @param pid: PID结构体指针
* @param kp: 比例系数
* @param ki: 积分系数
* @param kd: 微分系数
*/
void IncPID_Init(Incremental_PID* pid, float kp, float ki, float kd)
{
pid->Kp = kp;
pid->Ki = ki;
pid->Kd = kd;
pid->error[0] = pid->error[1] = pid->error[2] = 0;
pid->output = 0;
}
/**
* @brief 增量式PID计算函数
* @param pid: PID结构体
* @param setpoint: 目标值(例如目标速度对应的compare等效值)
* @param feedback: 反馈值(例如当前速度映射的等效compare)
* @return 当前总输出值(output += Δu)
*/
float IncPID_Calc(Incremental_PID* pid, float setpoint, float feedback)
{
float error = setpoint - feedback;
// 更新误差历史
pid->error[2] = pid->error[1];// 上上次 = 上次
pid->error[1] = pid->error[0];// 上次 = 当前
pid->error[0] = error; // 当前误差 = 设定值 - 实际值
// 增量式PID公式:
// Δu = Kp*(e[k]-e[k-1]) + Ki*e[k] + Kd*(e[k] - 2*e[k-1] + e[k-2])
float delta_u = pid->Kp * (pid->error[0] - pid->error[1])
+ pid->Ki * pid->error[0]*DT
+ pid->Kd * (pid->error[0] - 2*pid->error[1] + pid->error[2])/DT;
// 累加到输出
float new_output = pid->output + delta_u;
//内部输出限幅(对应Compare范围)
if (new_output > PID_MAX_OUTPUT)
{
new_output = PID_MAX_OUTPUT;
}
else if (new_output < PID_MIN_OUTPUT)
{
new_output = PID_MIN_OUTPUT;
}
//抗积分饱和:只有在未饱和时才更新状态
if (new_output == pid->output + delta_u)
{
pid->output = new_output;
}
// 否则不更新(防止 windup)
return pid->output;
}
这是incremental_pid.c#include "stm32f10x.h" // Device header
#include "Delay.h"
#include "Trace.h"
#include "Motor.h"
#include "PWM.h"
#include "incremental_pid.h"
#define BASE_SPEED_TICKS_PER_10MS 10.0f // 每 10ms 期望读取到的脉冲数(对应速度)
#define MAX_SPEED_TICKS_PER_10MS 15.0f // 最大允许值
#define MIN_SPEED_TICKS_PER_10MS 2.0f // 最小允许值
#define K_TURN 1.2f // 转向灵敏度系数(调试关键参数)原来1.2f
float target_left_ticks_10ms = 0.0f;
float target_right_ticks_10ms = 0.0f;
/**
* @brief 根据灰度偏差 error 计算左右轮目标脉冲数(简化版)
* 思路:
* error -> 视作角速度 ω
* ω * K_TURN -> 转化为线速度差 Δv
* 左右轮 = 基础速度 ± Δv/2
*/
void Motor_Control(void)
{
// 1. 获取加权平均误差 [-7, +7],直接视作“虚拟角速度”
float omega = calculate_weighted_error(); // 即 angle ≈ ω (dt=1)
// 2. 将角速度映射为线速度差(可调灵敏度)
float delta_v = omega * K_TURN; // 总差速
float half_delta = delta_v * 0.5f; // 分配到每边
// 3. 计算左右轮目标速度
float left_target = BASE_SPEED_TICKS_PER_10MS - half_delta;
float right_target = BASE_SPEED_TICKS_PER_10MS + half_delta;
// 4. 限幅处理
if (left_target > MAX_SPEED_TICKS_PER_10MS) left_target = MAX_SPEED_TICKS_PER_10MS;
if (right_target > MAX_SPEED_TICKS_PER_10MS) right_target = MAX_SPEED_TICKS_PER_10MS;
if (left_target < MIN_SPEED_TICKS_PER_10MS) left_target = MIN_SPEED_TICKS_PER_10MS;
if (right_target < MIN_SPEED_TICKS_PER_10MS) right_target = MIN_SPEED_TICKS_PER_10MS;
// 5. 更新全局目标变量(供 PID 使用)
target_left_ticks_10ms = left_target;
target_right_ticks_10ms = right_target;
}
这是mycontrol.c。这是我的所有代码.c模块,调试时发现电机正反转乱转,而且小车越偏离黑线OLED_ShowNum(2, 1, (uint32_t)(target_left_ticks_10ms * 100), 3);//目标速度
OLED_ShowNum(2, 6, (uint32_t)(target_right_ticks_10ms * 100), 3);这两个值相减的差值反而越小,是限幅出现问题,还是代码逻辑问题