五、PID算法
控制相关的软件函数包括：
1. AngleControl：小车倾角计算函数。根据采集到的陀螺仪和重力加速度传感器的数值计算小车角度和角速度。如果这部分的算法由外部一个运放实现，那么采集得到的直接是小车的角度和角速度，这部分算法可以省略。该函数是每5毫秒调用一次。
2. SpeedControl：小车速度控制函数。根据小车采集到的电机转速和速度设定值，计算电机的控制量。该函数是50毫秒调用一次。
3. chaoshengbo：超声波函数。将超声波距离获取频率平均分配到2步5毫秒的控制周期中。
4. MotorOutput：电机输出量汇集函数。根据前面的直立控制、速度控制和方向控制所得到的控制量进行叠加，分别得到左右两个电极的输出电压控制量。对叠加后的输出量进行饱和处理。函数调用周期5毫秒。在此请大家注意速度控制量叠加的极性是负。
5. SetMotorVoltage：PWM输出函数：根据两个电机的输出量，计算出PWM控制寄存器的数值，设置四个PWM控制寄存器的数值。函数调用周期1毫秒。
以上9个函数都是在1毫秒中断服务中进行被相互调用的。下图显示了这些函数之间的调用与参数传递关系。在个函数附近也表明了调用的周期。
