第一步：任务创建：在 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或者零的时候，那么它要减速很久。限幅就是为了防止这个回归时间太长的问题。