第一步：任务创建 ：在 FreeRTOS 中，系统功能由任务（Task）组成。在系统启动时，你需要创建各个任务并指定它们的任务频率 、优先级 、堆栈大小等参数。
xTaskCreate() 来创建开始任务并定义任务的执行函数、优先级、堆栈大小等参数。
接着，创建六个并行任务
第一个Balance_task任务是：小车控制任务.
点开参数BALANCE_TASK_PRIO为优先级。可以看到是4的优先级。在freeRTOS系统里面优先级数字越大，它的优先级越高。
但是单片机自带的外部中断和定时中断的优先级肯定是大于FreeRTOS任务优先级
进入到Balance_task任务里看，可以看到设置了100HZ的运行频率，也就代表10ms控制一次。
这是一个绝对的10ms，就是它从任务执行开始时开始计算，从这个lastWakeTime这里开始计算。10ms之后它会执行下一次任务。（使用了绝对延时函数）
第二个任务是mpu9250的任务，它是陀螺仪、加速度计读取数据的任务。
第三个任务是显示屏任务
。。。
如果说任务是例如像LED灯闪烁这种简单任务，那么它对频率的精度要求就会很低，然后它的执行时间也会很短。那么可以使用相对延时函数
编码器设计
这个就是计算编码器的精度。公式在代码已经给了
编码器精度的概念：车轮转一圈，编码器读取到数据的一个大小。
这个EncoderMultiples对应的就是编码器的频率，4倍频
EncoderAccuracy就是对应编码器的线数
GearRatio就是电机减速比30
比如光电编码器： 4*500*3=60000
所以Encoder_precision编码器的线速度为6万的时候，就代表我们的小车转了一圈
ok，接下来要将编码器进行数据转换了。将编码器原始数据转换成国际单位m/s
公式如下：
Encoder_A_pr/Encoder_precision
整体公式就是：
10ms内转了多少圈，10ms因为在100HZ的控制频率下，控制频率在blance_task那个任务写在里面
Encoder_A_pr就是编码器的原始数值
Encoder_precision代表小车车轮转一圈
然后我们要换算成s，即每秒转多少圈。那么就在公式的基础上乘以CONTROL_FREQUENVY,就是100HZ
（Encoder_A_pr/Encoder_precision）* CONTROL_FREQUENVY
也就是1s内我们转了多少圈
然后还要乘以一个周长，因为我们现在是弧度值（rad/s）的一个单位。所以我们要转换成线速度（m/s）
（Encoder_A_pr/Encoder_precision）* CONTROL_FREQUENVY*Wheel_perimeter
逆运动学解算
通过机器人X轴、Y轴、Z轴方向的速度来分别求出机器人各轮的速度，
速度平滑控制
重点在于step步进的一个设置，在这里设置的是0.01，也就是以0.01m/s的步伐前进。
看到函数参数，这里输入一个目标速度，最后会给一个平滑速度赋值。
假设我们当前速度是0，然后我们突然给一个目标速度是1m/s，那么就是说1/0.01=100.
所以说它要进入函数100次，它才会加速到1m/s的速度。对应上我们的控制频率是100HZ，
也就是说1秒之后它才会加速到1m/s的速度。
你也许会担心到不了1m/s，这个代码就是如果到了1m/s的目标速度，那么它就是1m/s不变。因为有限幅。
增量式PI控制器
Blas就是目标速度减去当前速度的值（计算偏差）
如果我们当前的目标速度是2m/s的速度，然后当前的实际速度它是1m/s，那Blas就是1，last_bias也是1
第二次如果当前实际速度达到了1.5m/s，那么Blas就是0.5。Bias - Last_bias=0.5 - 1 = -0.5
就是说我们现在速度增加了，我这个值就返回了一个负的值，那么它就是负反馈的作用，那么就是防止我们加速太快。我们加速越快，那么它给一个负反馈。我们加速越快，那么它给一个负反馈就越大
整个算法最重要就是：（动态调整）
pwm = pwm + Velocity_Kp*（Blas-Last_bias）+Velocity_KI*Bias
限幅
这个7200是pwm初始化来的。电机pwm的初始化是10KHz的频率.0到7199，就是7200个数。
如果没有这个限幅，那么它可能会加到6万的pwm值，但是7200与60000的PWM值，它实际上给到的一个转速是一样大的，因为7200是满幅了。但是当我们从6万像回到7200或者零的时候，那么它要减速很久。限幅就是为了防止这个回归时间太长的问题。
