#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;
int16_t g_filtered_left_tick = 0;
int16_t g_filtered_right_tick = 0;
float target_speed_ticks_10ms = 5.0f; // 单位:ticks / 10ms 表示每10ms接收多少脉冲,以此来表示速度
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, 2.0f, 1.0f, 0); // Kp=10, Ki=0.5, Kd=2 IncPID_Init(&right_speed_pid, 2.0f, 1.0f, 0); //2.0 0.1 0 //2.72 0.1 0 这个数据小车左右轮同时响应 Left_Encoder_Init(); Right_Encoder_Init(); while (1) { // 显示真实 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);//右轮编码器反馈的脉冲数 }
}
//定时中断函数
// 在定时器中断中添加计数器
static uint8_t display_counter = 0;
volatile uint8_t update_display_flag = 0; // 声明为 volatile
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]; // 更新 filtered_left_speed / filtered_right_speed Encoder_Get_Speed(Left_Speed_Feedback, Right_Speed_Feedback); g_filtered_left_tick = Left_Speed_Feedback; g_filtered_right_tick = Right_Speed_Feedback; Motor_Control(); //增量式 PID 控制开始 // 左轮 PID 控制 float left_output = IncPID_Calc(&left_speed_pid,target_left_ticks_10ms, Left_Speed_Feedback); // 右轮 PID 控制 float 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); //控制左电机 // 每隔 5 次中断更新一次显示 display_counter++; if (display_counter >= 5) { display_counter = 0; update_display_flag = 1; // 只设置标志,不执行显示 } }
}
这是main.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#ifndef __ENCODER_H
#define __ENCODER_H
#include “stm32f10x.h” // Device header
extern float Left_Speed_Feedback;
extern float Right_Speed_Feedback;
void Left_Encoder_Init(void);
int16_t Left_Encoder_Get(void);
void Right_Encoder_Init(void);
int16_t Right_Encoder_Get(void);
void Encoder_Get_Speed(int16_t left_ticks, int16_t right_ticks);
#endif
这是encoder.h#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_error = 0.0f; // 情况1:没有任何传感器检测到黑线(全白 - 脱线) if (active_count == 0) { // 脱线了!尝试按上次方向微调回来 return (last_error > 0) ? 8.0f : -8.0f;//返回 8.0f 或 -8.0f 是一种 强纠正信号,是人为设定的“最大试探值”。 } // 情况2:所有传感器都在黑线上(全黑 - 可能是终点或交叉) if (active_count == 8) { // 全黑通常表示进入终点或十字交叉,可以认为居中 return 0.0f; // 居中处理,也可特殊判断停车 } // 正常情况:计算加权平均位置作为误差 float error = (float)weighted_sum / active_count; // 更新记忆:只有在有有效信号时才更新 last_error last_error = error; return error; // 返回值范围大致在 [-7, +7],负数表示偏左,正数表示偏右
}
这是trace.c#ifndef __Trace_H
#define __Trace_H
void Trace_Init(void);
void Trace_Update(void);
float calculate_weighted_error(void);
#endif
这是trace.h#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#ifndef __MOTOR_H
#define __MOTOR_H
void Motor_DIR_Init(void);
void Motor_DIR_Left_Forward(void);
void Motor_DIR_Left_Backward(void);
void Motor_DIR_Right_Forward(void);
void Motor_DIR_Right_Backward(void);
#endif
这是motor.h#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);
}
void PWM_SetCompare3(uint16_t Compare3)//控制右侧电机的转速
{
TIM_SetCompare3(TIM2, Compare3);
}
void PWM_SetCompare4(uint16_t Compare4)//控制左侧电机的转速
{
TIM_SetCompare4(TIM2, Compare4);
}
这是pwm.c#ifndef __PWM_H
#define __PWM_H
void PWM_Init(void);
void PWM_SetCompare3(uint16_t Compare3);
void PWM_SetCompare4(uint16_t Compare4);
#endif
这是pwm.h#include “stm32f10x.h”
#include “OLED_Font.h”
/引脚配置/
#define OLED_W_SCL(x) GPIO_WriteBit(GPIOB, GPIO_Pin_14, (BitAction)(x))
#define OLED_W_SDA(x) GPIO_WriteBit(GPIOB, GPIO_Pin_15, (BitAction)(x))
/引脚初始化/
void OLED_I2C_Init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; GPIO_Init(GPIOB, &GPIO_InitStructure); OLED_W_SCL(1); OLED_W_SDA(1);
}
/**
@brief I2C开始
@param 无
@retval 无
*/
void OLED_I2C_Start(void)
{
OLED_W_SDA(1);
OLED_W_SCL(1);
OLED_W_SDA(0);
OLED_W_SCL(0);
}
/**
@brief I2C停止
@param 无
@retval 无
*/
void OLED_I2C_Stop(void)
{
OLED_W_SDA(0);
OLED_W_SCL(1);
OLED_W_SDA(1);
}
/**
@brief I2C发送一个字节
@param Byte 要发送的一个字节
@retval 无
*/
void OLED_I2C_SendByte(uint8_t Byte)
{
uint8_t i;
for (i = 0; i < 8; i++)
{
OLED_W_SDA(!!(Byte & (0x80 >> i)));
OLED_W_SCL(1);
OLED_W_SCL(0);
}
OLED_W_SCL(1); //额外的一个时钟,不处理应答信号
OLED_W_SCL(0);
}
/**
@brief OLED写命令
@param Command 要写入的命令
@retval 无
*/
void OLED_WriteCommand(uint8_t Command)
{
OLED_I2C_Start();
OLED_I2C_SendByte(0x78); //从机地址
OLED_I2C_SendByte(0x00); //写命令
OLED_I2C_SendByte(Command);
OLED_I2C_Stop();
}
/**
@brief OLED写数据
@param Data 要写入的数据
@retval 无
*/
void OLED_WriteData(uint8_t Data)
{
OLED_I2C_Start();
OLED_I2C_SendByte(0x78); //从机地址
OLED_I2C_SendByte(0x40); //写数据
OLED_I2C_SendByte(Data);
OLED_I2C_Stop();
}
/**
@brief OLED设置光标位置
@param Y 以左上角为原点,向下方向的坐标,范围:0~7
@param X 以左上角为原点,向右方向的坐标,范围:0~127
@retval 无
*/
void OLED_SetCursor(uint8_t Y, uint8_t X)
{
OLED_WriteCommand(0xB0 | Y); //设置Y位置
OLED_WriteCommand(0x10 | ((X & 0xF0) >> 4)); //设置X位置高4位
OLED_WriteCommand(0x00 | (X & 0x0F)); //设置X位置低4位
}
/**
@brief OLED清屏
@param 无
@retval 无
*/
void OLED_Clear(void)
{
uint8_t i, j;
for (j = 0; j < 8; j++)
{
OLED_SetCursor(j, 0);
for(i = 0; i < 128; i++)
{
OLED_WriteData(0x00);
}
}
}
/**
@brief OLED显示一个字符
@param Line 行位置,范围:1~4
@param Column 列位置,范围:1~16
@param Char 要显示的一个字符,范围:ASCII可见字符
@retval 无
*/
void OLED_ShowChar(uint8_t Line, uint8_t Column, char Char)
{
uint8_t i;
OLED_SetCursor((Line - 1) * 2, (Column - 1) * 8); //设置光标位置在上半部分
for (i = 0; i < 8; i++)
{
OLED_WriteData(OLED_F8x16[Char - ’ '][i]); //显示上半部分内容
}
OLED_SetCursor((Line - 1) * 2 + 1, (Column - 1) * 8); //设置光标位置在下半部分
for (i = 0; i < 8; i++)
{
OLED_WriteData(OLED_F8x16[Char - ’ '][i + 8]); //显示下半部分内容
}
}
/**
@brief OLED显示字符串
@param Line 起始行位置,范围:1~4
@param Column 起始列位置,范围:1~16
@param String 要显示的字符串,范围:ASCII可见字符
@retval 无
*/
void OLED_ShowString(uint8_t Line, uint8_t Column, char *String)
{
uint8_t i;
for (i = 0; String[i] != ‘\0’; i++)
{
OLED_ShowChar(Line, Column + i, String[i]);
}
}
/**
@brief OLED次方函数
@retval 返回值等于X的Y次方
*/
uint32_t OLED_Pow(uint32_t X, uint32_t Y)
{
uint32_t Result = 1;
while (Y–)
{
Result *= X;
}
return Result;
}
/**
@brief OLED显示数字(十进制,正数)
@param Line 起始行位置,范围:1~4
@param Column 起始列位置,范围:1~16
@param Number 要显示的数字,范围:0~4294967295
@param Length 要显示数字的长度,范围:1~10
@retval 无
*/
void OLED_ShowNum(uint8_t Line, uint8_t Column, uint32_t Number, uint8_t Length)
{
uint8_t i;
for (i = 0; i < Length; i++)
{
OLED_ShowChar(Line, Column + i, Number / OLED_Pow(10, Length - i - 1) % 10 + ‘0’);
}
}
/**
@brief OLED显示数字(十进制,带符号数)
@param Line 起始行位置,范围:1~4
@param Column 起始列位置,范围:1~16
@param Number 要显示的数字,范围:-2147483648~2147483647
@param Length 要显示数字的长度,范围:1~10
@retval 无
*/
void OLED_ShowSignedNum(uint8_t Line, uint8_t Column, int32_t Number, uint8_t Length)
{
uint8_t i;
uint32_t Number1;
if (Number >= 0)
{
OLED_ShowChar(Line, Column, ‘+’);
Number1 = Number;
}
else
{
OLED_ShowChar(Line, Column, ‘-’);
Number1 = -Number;
}
for (i = 0; i < Length; i++)
{
OLED_ShowChar(Line, Column + i + 1, Number1 / OLED_Pow(10, Length - i - 1) % 10 + ‘0’);
}
}
/**
@brief OLED显示数字(十六进制,正数)
@param Line 起始行位置,范围:1~4
@param Column 起始列位置,范围:1~16
@param Number 要显示的数字,范围:0~0xFFFFFFFF
@param Length 要显示数字的长度,范围:1~8
@retval 无
*/
void OLED_ShowHexNum(uint8_t Line, uint8_t Column, uint32_t Number, uint8_t Length)
{
uint8_t i, SingleNumber;
for (i = 0; i < Length; i++)
{
SingleNumber = Number / OLED_Pow(16, Length - i - 1) % 16;
if (SingleNumber < 10)
{
OLED_ShowChar(Line, Column + i, SingleNumber + ‘0’);
}
else
{
OLED_ShowChar(Line, Column + i, SingleNumber - 10 + ‘A’);
}
}
}
/**
@brief OLED显示数字(二进制,正数)
@param Line 起始行位置,范围:1~4
@param Column 起始列位置,范围:1~16
@param Number 要显示的数字,范围:0~1111 1111 1111 1111
@param Length 要显示数字的长度,范围:1~16
@retval 无
*/
void OLED_ShowBinNum(uint8_t Line, uint8_t Column, uint32_t Number, uint8_t Length)
{
uint8_t i;
for (i = 0; i < Length; i++)
{
OLED_ShowChar(Line, Column + i, Number / OLED_Pow(2, Length - i - 1) % 2 + ‘0’);
}
}
/**
@brief OLED初始化
@param 无
@retval 无
*/
void OLED_Init(void)
{
uint32_t i, j;
for (i = 0; i < 1000; i++) //上电延时
{
for (j = 0; j < 1000; j++);
}
OLED_I2C_Init(); //端口初始化
OLED_WriteCommand(0xAE); //关闭显示
OLED_WriteCommand(0xD5); //设置显示时钟分频比/振荡器频率
OLED_WriteCommand(0x80);
OLED_WriteCommand(0xA8); //设置多路复用率
OLED_WriteCommand(0x3F);
OLED_WriteCommand(0xD3); //设置显示偏移
OLED_WriteCommand(0x00);
OLED_WriteCommand(0x40); //设置显示开始行
OLED_WriteCommand(0xA1); //设置左右方向,0xA1正常 0xA0左右反置
OLED_WriteCommand(0xC8); //设置上下方向,0xC8正常 0xC0上下反置
OLED_WriteCommand(0xDA); //设置COM引脚硬件配置
OLED_WriteCommand(0x12);
OLED_WriteCommand(0x81); //设置对比度控制
OLED_WriteCommand(0xCF);
OLED_WriteCommand(0xD9); //设置预充电周期
OLED_WriteCommand(0xF1);
OLED_WriteCommand(0xDB); //设置VCOMH取消选择级别
OLED_WriteCommand(0x30);
OLED_WriteCommand(0xA4); //设置整个显示打开/关闭
OLED_WriteCommand(0xA6); //设置正常/倒转显示
OLED_WriteCommand(0x8D); //设置充电泵
OLED_WriteCommand(0x14);
OLED_WriteCommand(0xAF); //开启显示
OLED_Clear(); //OLED清屏
}
这是oled.c#ifndef __OLED_H
#define __OLED_H
void OLED_Init(void);
void OLED_Clear(void);
void OLED_ShowChar(uint8_t Line, uint8_t Column, char Char);
void OLED_ShowString(uint8_t Line, uint8_t Column, char *String);
void OLED_ShowNum(uint8_t Line, uint8_t Column, uint32_t Number, uint8_t Length);
void OLED_ShowSignedNum(uint8_t Line, uint8_t Column, int32_t Number, uint8_t Length);
void OLED_ShowHexNum(uint8_t Line, uint8_t Column, uint32_t Number, uint8_t Length);
void OLED_ShowBinNum(uint8_t Line, uint8_t Column, uint32_t Number, uint8_t Length);
#endif
这是oled.h#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#ifndef __TIMER_H
#define __TIMER_H
void Timer1_Init(void);
void TIM1_UP_IRQHandler(void);
#endif
这是timer.h#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#ifndef __DELAY_H
#define __DELAY_H
#include “stm32f10x.h”
// 函数声明
void Delay_init(void); // 初始化 SysTick 节拍
uint32_t millis(void); // 获取当前毫秒数
void Delay_us(uint32_t xus); // 微秒延时
void Delay_ms(uint32_t ms); // 毫秒延时
void Delay_s(uint32_t xs); // 秒级延时
#endif
这是delay.h#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 500.0f
#define PID_MIN_OUTPUT -500.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]) + Kie[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] - 2pid->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#ifndef __INCREMENTAL_PID_H
#define __INCREMENTAL_PID_H
typedef struct {
float Kp;
float Ki;
float Kd;
float error[3]; // error[0]:当前, [1]:上次, [2]:上上次 float output; // 当前输出(累计值)
} Incremental_PID;
void IncPID_Init(Incremental_PID* pid, float kp, float ki, float kd);
float IncPID_Calc(Incremental_PID* pid, float setpoint, float feedback);
extern Incremental_PID left_speed_pid;
extern Incremental_PID right_speed_pid;
float calculate_weighted_error(void);
#endif
这是incremental_pid.h#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 5.0f // 每 10ms 期望读取到的脉冲数(对应速度)
#define MAX_SPEED_TICKS_PER_10MS 15.0f // 最大允许值
#define MIN_SPEED_TICKS_PER_10MS 2.0f // 最小允许值
#define KP_DIFF 0.8f // 差速比例增益
#define KD_DIFF 0.2f // 差速微分增益
#define WHEELBASE_L_IN_TICKS 12.0f // 虚拟轴距(可用标定法确定等效值)
float target_left_ticks_10ms = 0.0f;
float target_right_ticks_10ms = 0.0f;
/**
@brief 根据灰度偏差 error 计算左右轮目标脉冲数(ticks/10ms)
*/
void Motor_Control(void)
{
// 1. 获取加权平均误差(范围约 [-7, +7])
float error = calculate_weighted_error();
// 2. 将误差映射为期望角速度 omega(可调参数)
const float OMEGA_MAX = 2.0f; // 最大虚拟角速度
float omega = (error / 7.0f) * OMEGA_MAX; // 归一化后缩放 [-2, +2]
// 3. 定义基础线速度(用 ticks/10ms 表示)
const float V_BASE = BASE_SPEED_TICKS_PER_10MS;
// 4. 假设一个等效轴距 L(单位:ticks_per_10ms_per_unit_angle)
// 这是一个需要调试的经验系数,称为“转向灵敏度因子”
const float L_EQUIVALENT = 3.5f; // 实验调参确定
// 5. 根据差速公式计算左右轮目标速度
// v_left = v - (L * ω) / 2
// v_right = v + (L * ω) / 2
float delta = (L_EQUIVALENT * omega) / 2.0f;
float left_target = V_BASE - delta;
float right_target = V_BASE + delta;
// 6. 限幅保护
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;
// 7. 更新全局目标变量(供 PID 使用)
target_left_ticks_10ms = left_target;
target_right_ticks_10ms = right_target;
}
这是mycontrol.c#ifndef __MYCONTROL_H
#define __MYCONTROL_H
void Motor_Control(void);
#endif
这是mycontrol.h。这些是我的全部工程文件,我想问一下,增量式pid计算出的output直接给compare,不用映射吗
最新发布