AngleCalculate:车模倾角计算函数。根据采集到的陀螺仪和重力加速度传感器的数
值计算车模角度和角速度。如果这部分的算法由外部一个运放实现,那么采集得到
的直接是车模的角度和角速度,这部分算法可以省略。该函数是每 5 毫秒调用一次。
2. AngelControl:车模直立控制函数。根据车模角度和角速度计算车模电机的控制量。
直立控制是 5 毫秒调用一次。
3. SpeedControl:车模速度控制函数。根据车模采集到的电机转速和速度设定值,计算
电机的控制量。该函数是 100 毫秒调用一次。
4. SpeedControlOutput:速度输出平滑函数。由于速度是每 100 毫秒进行一次计算。为
了使得速度控制更加平滑,该函数将速度输出变化量平均分配到 20 步 5 毫秒的控制
周期中。
5. DirectionControl:方向控制函数。根据车模采集到的左右两个电磁传感器的数值计
算出角度控制的量。该函数每 10 毫秒调用一次