“KEA”通常指的是 NXP 的 S9KEA 系列单片机（如 S9KEA128），这款芯片在国内的智能车竞赛和平衡车项目中非常流行。
一个完整的“KEA 直立程序”通常包含四大核心部分：传感器数据读取（MPU6050）、姿态角融合（滤波）、直立 PID 控制环、以及电机 PWM 输出。
基于 S9KEA128 + MPU6050 + 直流减速电机 的通用直立车程序
1. 核心控制逻辑与宏定义 (main.c)
这部分是程序的骨架，包含了初始化流程和主循环中的控制逻辑。
#include "common.h" // KEA128 官方或第三方库头文件
#include "MKKEA128.h" // 芯片寄存器头文件
#include "MPU6050.h" // MPU6050 驱动
#include "motor.h" // 电机PWM驱动
// ==================== 全局变量定义 ====================
float Pitch = 0; // 最终融合后的俯仰角 (小车前倾/后仰)
float Gyro_Y = 0; // Y轴陀螺仪原始数据 (角速度)
float Accel_X = 0; // X轴加速度计原始数据
// PID 相关变量
float Balance_PWM = 0; // 直立环输出的PWM
float Motor_Left = 0; // 左电机最终PWM
float Motor_Right = 0; // 右电机最终PWM
// ==================== 宏定义 (根据你的机械结构修改) ====================
#define MIDDLE_ANGLE 0.0f // 机械中值角度 (小车完全直立时为0度)
#define BALANCE_KP 42.0f // 直立环 P 系数 (正数)
#define BALANCE_KD 26.0f // 直立环 D 系数 (正数)
/* 简要说明:
Balance_PWM = KP * (Pitch - MIDDLE_ANGLE) + KD * Gyro_Y;
注意: 如果你的小车往哪边倒，电机就往哪边转，说明极性反了，把 KP/KD 改为负数即可。
*/
// ==================== 直立 PD 控制函数 ====================
float Balance_Control(float Angle, float Gyro) {
float Bias;
float Pwm;
Bias = Angle - MIDDLE_ANGLE; // 计算角度偏差
Pwm = BALANCE_KP * Bias + BALANCE_KD * Gyro; // 经典 PD 公式
return Pwm;
}
// ==================== 主函数 ====================
int main(void) {
// 1. 系统初始化
SystemCoreClockUpdate(); // 更新系统核心时钟
Delay_Init(); // 延时初始化
LED_Init(); // LED 初始化 (用于状态指示)
// 2. 外设初始化
I2C_Init(); // 初始化 I2C 总线 (用于通信 MPU6050)
MPU6050_Init(); // 初始化 MPU6050 (设置量程、滤波器等)
Motor_PWM_Init(); // 初始化电机 PWM (FTM模块)
// 3. 编码器初始化 (如果需要速度环，在这里初始化 FTM 输入捕获)
// Encoder_Init();
while (1) {
// --- 步骤 1: 读取传感器数据 ---
MPU6050_ReadData(&Pitch, &Gyro_Y, &Accel_X);
// --- 步骤 2: 直立环 PID 计算 ---
Balance_PWM = Balance_Control(Pitch, Gyro_Y);
// --- 步骤 3: 电机输出 ---
// 将直立环的结果分配给左右电机
// 如果加上转向环或速度环，就在后面加减相应的PWM值
Motor_Left = Balance_PWM;
Motor_Right = Balance_PWM;
// 限幅保护 (防止 PWM 超出 100%)
if (Motor_Left > 100) Motor_Left = 100;
if (Motor_Left < -100) Motor_Left = -100;
if (Motor_Right > 100) Motor_Right = 100;
if (Motor_Right < -100) Motor_Right = -100;
// 真正将 PWM 写入寄存器，驱动电机
Motor_Set_PWM(Motor_Left, Motor_Right);
// --- 步骤 4: 循环延时控制 (控制周期建议 5ms - 10ms) ---
Delay_ms(5);
}
}
2. MPU6050 驱动与滤波处理 (MPU6050.c)
MPU6050 返回的是原始 ADC 值，我们需要将其转化为物理量，并进行简单的互补滤波（一阶低通+高通）来消除加速度计的噪声和陀螺仪的漂移。
#include "MPU6050.h"
// MPU6050 内部寄存器地址
#define MPU6050_ADDR 0xD0 // 器件写地址 (AD0 接地)
#define PWR_MGMT_1 0x6B
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_XOUT_H 0x3B
#define GYRO_YOUT_H 0x45 // 读取 Y 轴陀螺仪的高字节
// 陀螺仪和加速度计量程缩放因子
#define GYRO_SCALER 16.4f // ±2000 dps 量程下的缩放因子 (32768 / 2000)
#define ACCEL_SCALER 16384.0f// ±2g 量程下的缩放因子
static float GyroOffset_Y = 0;
static float AccelOffset_X = 0;
// 一阶互补滤波参数 (α 越大，陀螺仪信任度越高，响应越快但对加速度计噪声抑制变差)
#define ALPHA 0.98f
// ==================== 向 MPU6050 写一个字节 ====================
void MPU6050_WriteReg(uint8_t reg, uint8_t data) {
I2C_Start();
I2C_SendByte(MPU6050_ADDR); // 发送写地址
I2C_WaitAck();
I2C_SendByte(reg); // 发送寄存器地址
I2C_WaitAck();
I2C_SendByte(data); // 发送要写入的数据
I2C_WaitAck();
I2C_Stop();
}
// ==================== 从 MPU6050 读一个字节 ====================
uint8_t MPU6050_ReadReg(uint8_t reg) {
uint8_t data;
I2C_Start();
I2C_SendByte(MPU6050_ADDR);
I2C_WaitAck();
I2C_SendByte(reg);
I2C_WaitAck();
I2C_Start(); // 重启总线，准备读取
I2C_SendByte(MPU6050_ADDR | 0x01); // 发送读地址
I2C_WaitAck();
data = I2C_ReadByte();
I2C_NoAck(); // 读最后一个字节发送 NAck
I2C_Stop();
return data;
}
// ==================== MPU6050 初始化 ====================
void MPU6050_Init(void) {
MPU6050_WriteReg(PWR_MGMT_1, 0x00); // 唤醒设备
MPU6050_WriteReg(SMPLRT_DIV, 0x07); // 采样率分频，1kHz / (1+7) = 125Hz
MPU6050_WriteReg(CONFIG, 0x00); // 配置低通滤波器
MPU6050_WriteReg(GYRO_CONFIG, 0x18); // +-2000度/秒 量程
MPU6050_WriteReg(ACCEL_CONFIG, 0x00); // +-2g 量程
// 注意：实际项目中，这里应该加入“上电静态校准”代码，
// 通过连续读取几百次数据求平均，计算出 GyroOffset 和 AccelOffset。
}
// ==================== 读取并处理传感器数据 ====================
void MPU6050_ReadData(float *pitch, float *gyro_y, float *accel_x) {
int16_t raw_ax, raw_ay, raw_az;
int16_t raw_gy, dt = 5; // dt 为每次计算的间隔时间(ms)，这里与main中的Delay_ms(5)对应
// 读取加速度计原始值
raw_ax = (int16_t)((MPU6050_ReadReg(ACCEL_XOUT_H) << 8) | MPU6050_ReadReg(ACCEL_XOUT_H + 1));
raw_ay = (int16_t)((MPU6050_ReadReg(ACCEL_XOUT_H + 2) << 8) | MPU6050_ReadReg(ACCEL_XOUT_H + 3));
raw_az = (int16_t)((MPU6050_ReadReg(ACCEL_XOUT_H + 4) << 8) | MPU6050_ReadReg(ACCEL_XOUT_H + 5));
// 读取陀螺仪原始值
raw_gy = (int16_t)((MPU6050_ReadReg(GYRO_YOUT_H) << 8) | MPU6050_ReadReg(GYRO_YOUT_H + 1));
// 减去零点偏移量
raw_gy -= GyroOffset_Y;
raw_ax -= AccelOffset_X;
// 转换为物理量
float f_gy = (float)raw_gy / GYRO_SCALER; // 单位：度/秒
float f_ax = (float)raw_ax / ACCEL_SCALER; // 单位：g
float f_ay = (float)raw_ay / ACCEL_SCALER;
float f_az = (float)raw_az / ACCEL_SCALER;
// --- 互补滤波计算俯仰角 (Pitch) ---
// 1. 通过加速度计计算角度 (arctan2 得到的单位是弧度，需转为度)
float angle_acc = atan2(f_ax, sqrt(f_ay*f_ay + f_az*f_az)) * 180.0f / 3.1415926f;
// 2. 通过陀螺仪积分计算角度
float angle_gy = *pitch + f_gy * dt / 1000.0f; // 度/秒 * 秒 = 度
// 3. 互补滤波融合
*pitch = ALPHA * angle_gy + (1 - ALPHA) * angle_acc;
// 传出数据
*gyro_y = f_gy;
*accel_x = f_ax;
}
3. 电机 PWM 驱动 (motor.c)
KEA128 通常使用 FTM (FlexTimer Module) 来产生 PWM 波。
#include "motor.h"
// 假设使用 FTM2 模块，通道 0 和 1 控制左电机，通道 2 和 3 控制右电机
// 具体引脚需根据你自己的原理图修改
#define MOTOR_FTM FTM2
#define LEFT_FWD FTM_Ch0
#define LEFT_REV FTM_Ch1
#define RIGHT_FWD FTM_Ch2
#define RIGHT_REV FTM_Ch3
void Motor_PWM_Init(void) {
// 使能 FTM2 时钟
SIM->SCGC |= SIM_SCGC_FTM2_MASK;
// 配置 FTM2 为边沿对齐 PWM 模式
MOTOR_FTM->MODE = FTM_MODE_WPDIS_MASK; // 禁用写保护
MOTOR_FTM->CONF = 0xC0; // 设置BDM为11，允许在调试模式下运行
MOTOR_FTM->MOD = 10000; // 设置 PWM 频率 = 总线频率 / MOD值 (例如 10kHz)
// 配置通道为 PWM 模式
MOTOR_FTM->CONTROLS[LEFT_FWD].CnSC = FTM_CnSC_MSB_MASK | FTM_CnSC_ELSB_MASK;
MOTOR_FTM->CONTROLS[LEFT_REV].CnSC = FTM_CnSC_MSB_MASK | FTM_CnSC_ELSB_MASK;
MOTOR_FTM->CONTROLS[RIGHT_FWD].CnSC = FTM_CnSC_MSB_MASK | FTM_CnSC_ELSB_MASK;
MOTOR_FTM->CONTROLS[RIGHT_REV].CnSC = FTM_CnSC_MSB_MASK | FTM_CnSC_ELSB_MASK;
// 初始化占空比为 0
MOTOR_FTM->CONTROLS[LEFT_FWD].CnV = 0;
MOTOR_FTM->CONTROLS[LEFT_REV].CnV = 0;
MOTOR_FTM->CONTROLS[RIGHT_FWD].CnV = 0;
MOTOR_FTM->CONTROLS[RIGHT_REV].CnV = 0;
// 启动计数器
MOTOR_FTM->SC = FTM_SC_CLKS(1) | FTM_SC_PS(0); // 选择系统时钟，不分频
}
void Motor_Set_PWM(float left_pwm, float right_pwm) {
// 限制 PWM 占空比在 0 - 10000 之间
if(left_pwm > 0) {
MOTOR_FTM->CONTROLS[LEFT_FWD].CnV = (uint16_t)left_pwm * 100; // 假设 100% 对应 10000
MOTOR_FTM->CONTROLS[LEFT_REV].CnV = 0;
} else {
MOTOR_FTM->CONTROLS[LEFT_FWD].CnV = 0;
MOTOR_FTM->CONTROLS[LEFT_REV].CnV = (uint16_t)(-left_pwm) * 100;
}
if(right_pwm > 0) {
MOTOR_FTM->CONTROLS[RIGHT_FWD].CnV = (uint16_t)right_pwm * 100;
MOTOR_FTM->CONTROLS[RIGHT_REV].CnV = 0;
} else {
MOTOR_FTM->CONTROLS[RIGHT_FWD].CnV = 0;
MOTOR_FTM->CONTROLS[RIGHT_REV].CnV = (uint16_t)(-right_pwm) * 100;
}
}
参考代码 KEA_直立程序 www.youwenfan.com/contentcsu/70191.html
调参指南
-
先调直立环（PD）：
- 把小车拿在手里，上电，不要放下。
- 先设
KD = 0，慢慢增大KP，直到小车有“反抗”的趋势（你往后倒它往后转，你往前倒它往前转）。 - 固定
KP，慢慢增大KD，直到小车能够比较平稳地抵抗你的推拉，并且不会剧烈震荡。 - 极性判断：如果小车往哪边倒，轮子就往相反方向转（即越倒越快），说明
KP或KD的正负号反了，直接改成负数即可。
-
MPU6050 的安装方向：
- 代码中的俯仰角计算是基于 MPU6050 的 X 轴和 Z 轴加速度计。如果你的板子安装方向不同（例如平放或侧放），你需要修改
atan2函数里的输入参数（比如改用 Y 轴和 Z 轴）。
- 代码中的俯仰角计算是基于 MPU6050 的 X 轴和 Z 轴加速度计。如果你的板子安装方向不同（例如平放或侧放），你需要修改
-
控制周期：
- 直立车的控制周期非常关键。上面的代码使用了
Delay_ms(5)，即 200Hz。如果你的控制周期变慢（比如 50ms），陀螺仪的积分项*pitch = angle_gy + f_gy * dt中的dt就必须跟着改，否则角度会疯狂发散。
- 直立车的控制周期非常关键。上面的代码使用了
这个框架实现了最基本的自平衡功能。在实际的智能车比赛中，你还需要在此基础上加入速度环（PI控制，用来让小车保持静止不走动）和转向环（PD控制，用来循迹或保持方向）。
