核心硬件驱动模块
1.驱动电机用motor相关函数
2.读取速度用encoder相关函数
3.获取姿态数据，用MPU6050相关函数
4.调试时调用两个串口相关函数
平衡方案
本项目使用两轮平衡小车，仅需关注俯仰角，只有精准实时获取车身前倾 / 后倾角度，后续才能通过 PID 算法控制电机正反转，抵消倾斜、维持车身直立，是整个控制系统的 “感知基础”。
实现方案：因此利用 MPU6050计算小车的俯仰角，并通过互补滤波算法解析。
单独用加速度计计算俯仰角：
测加速度就是测每个面受到的重力，F=ma，成正比。根据MPU6050在小车上安装的方向和位置，这里我们根据x轴和z轴的夹角来计算俯仰角
优点：静态精度高，小车静止时测出的角度稳定、无漂移；
缺点：抗干扰极差，小车电机振动、车身移动、加减速都会严重干扰数据（加速时会受到力测量出加速度值，再用上面的公式计算会有偏移）。测绝对角度
单独使用陀螺仪计算俯仰角：
测角速度方法简单来说就是两个球绕一根杆旋转，转动速度越快力越大，圆周运动时，角速度的积分就是角度，所以只要简单的对角速度进行积分就能测出角度。而这里的角度不是绝对值，而是变化的角度。由于小车绕xoz平面旋转，因此只要测绕y轴的角速度就能测出俯仰角。
由于只能隔一段时间采样角速度，是离散系统，因此只能计算矩形面积的和，还要知道采样间隔T
优点：动态响应快，车身倾斜瞬间就能检测到角速度，短期测角精度高，不受振动影响；
缺点：存在积分漂移问题。陀螺仪只能输出角速度，需要不断积分得到角度，随着时间推移，微小误差会持续累积。测相对角度，且要明确初始值。
核心算法：互补滤波（两者结合）
思路就是以陀螺仪为主，加速度计为辅。通过参考加速度计角度，来修正陀螺仪角度的漂移，同时为陀螺仪提供绝对角度参考值。
下面的式子就是在陀螺仪算出的角度上加一个偏移量，使陀螺仪算出的角度不断偏向加速度计算出的角度，同时这之间的差值越大，偏移量越大。
这里α的值大了，算出的值易受冲击影响更明显（加速度计的缺点），小了，算出的值在稳定时的稳态误差越大（陀螺仪的缺点）。一般给0.01或0.02。
最后还要抑制零度漂移，得到小车平放时的y轴角速度，然后让其减去这个值使y轴角速度归零。
PID控制结构
这里只包括了角度环和转向环
当差分PWM给零时，左右轮转动速度一样，小车直行。当平均PWM给10，差分PWM给4时，左轮PWM为12，右轮PWM为8，左轮转速快，小车右转。
因此只要控制平均PWM和差分PWM就可以控制小车运动，同时前后和左右控制能实现解耦，前后只要控制平均PWM，左右只要控制差分PWM。
主要代码：计算俯仰角，平均PWM，差分PWM
MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ); GY -= 37; AngleAcc = -atan2(AX , AZ) / 3.14159 * 180; AngleGyro = Angle + GY / 32768.0 * 2000 *0.01; float Alpha = 0.01; Angle = Alpha * AngleAcc + (1 - Alpha) * AngleGyro;//angle为俯仰角 if(Angle > 50 || Angle < -50) { RunFlag = 0; } if(RunFlag) { AnglePID.Actual = Angle; PID_Update(&AnglePID); AvePWM = -AnglePID.Out; LeftPWM = AvePWM + DifPWM / 2; RightPWM = AvePWM - DifPWM / 2; if(LeftPWM > 100) {LeftPWM = 100;} else if(LeftPWM < -100) {LeftPWM = -100;} if(RightPWM > 100) {RightPWM = 100;} else if(RightPWM < -100) {RightPWM = -100;} Motor_SetPWM(1, LeftPWM); Motor_SetPWM(2, RightPWM); } else { Motor_SetPWM(1, 0); Motor_SetPWM(2, 0); }PID的参数如下
PID_t AnglePID = { .Kp = 3.1, .Ki = 0.1, .Kd = 3.7, .OutMax = 100, .OutMin = -100, };
