第一步:任务创建 :在 FreeRTOS 中,系统功能由任务(Task)组成。在系统启动时,你需要创建各个任务并指定它们的任务频率 、优先级 、堆栈大小等参数。
xTaskCreate()
来创建开始任务并定义任务的执行函数、优先级、堆栈大小等参数。
![](https://img-blog.csdnimg.cn/direct/5f568d9d0a844672ad587ebf6b964a7b.png)
接着,创建六个并行任务
![](https://img-blog.csdnimg.cn/direct/54b0ed5137de44d7822a23bda18099e3.png)
第一个Balance_task任务是:小车控制任务.
点开参数BALANCE_TASK_PRIO为优先级。可以看到是4的优先级。在freeRTOS系统里面优先级数字越大,它的优先级越高。
但是单片机自带的外部中断和定时中断的优先级肯定是大于FreeRTOS任务优先级
![](https://img-blog.csdnimg.cn/direct/d524c10f5a2f45cf83021bc185ce2379.png)
进入到Balance_task任务里看,可以看到设置了100HZ的运行频率,也就代表10ms控制一次。
这是一个绝对的10ms,就是它从任务执行开始时开始计算,从这个lastWakeTime这里开始计算。10ms之后它会执行下一次任务。(使用了绝对延时函数)
![](https://img-blog.csdnimg.cn/direct/9d84642f588a4d8eb44a543a17edf056.png)
第二个任务是mpu9250的任务,它是陀螺仪、加速度计读取数据的任务。
第三个任务是显示屏任务
。。。
如果说任务是例如像LED灯闪烁这种简单任务,那么它对频率的精度要求就会很低,然后它的执行时间也会很短。那么可以使用相对延时函数
编码器设计
这个就是计算编码器的精度。公式在代码已经给了
![](https://img-blog.csdnimg.cn/direct/0f8bb7e57d8142c6a7458b9c4c00a73e.png)
编码器精度的概念:车轮转一圈,编码器读取到数据的一个大小。
![](https://img-blog.csdnimg.cn/direct/ba552612fc254b198eaeb2dccb8a5a1a.png)
这个EncoderMultiples对应的就是编码器的频率,4倍频
![](https://img-blog.csdnimg.cn/direct/3245aefccc9c49fbac4aec30d47a909c.png)
EncoderAccuracy就是对应编码器的线数
![](https://img-blog.csdnimg.cn/direct/5a7b9a5074d04f518a95139682ef5510.png)
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
![](https://img-blog.csdnimg.cn/direct/aceaa075ed2e47fbb5a1f5e8a2b3aaa7.png)
逆运动学解算
通过机器人X轴、Y轴、Z轴方向的速度来分别求出机器人各轮的速度,
速度平滑控制
重点在于step步进的一个设置,在这里设置的是0.01,也就是以0.01m/s的步伐前进。
看到函数参数,这里输入一个目标速度,最后会给一个平滑速度赋值。
假设我们当前速度是0,然后我们突然给一个目标速度是1m/s,那么就是说1/0.01=100.
所以说它要进入函数100次,它才会加速到1m/s的速度。对应上我们的控制频率是100HZ,
也就是说1秒之后它才会加速到1m/s的速度。
![](https://img-blog.csdnimg.cn/direct/93f580eae2da4cd9ab580ab467f7fe3f.png)
你也许会担心到不了1m/s,这个代码就是如果到了1m/s的目标速度,那么它就是1m/s不变。因为有限幅。
![](https://img-blog.csdnimg.cn/direct/41c530d816cc417c8210ad227bf0e803.png)
增量式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
![](https://img-blog.csdnimg.cn/direct/a726f87f79064b6f9bf338691a6014de.png)
限幅
![](https://img-blog.csdnimg.cn/direct/5d6ce13b5a9248b3a5e6c2852bf28327.png)
这个7200是pwm初始化来的。电机pwm的初始化是10KHz的频率.0到7199,就是7200个数。
如果没有这个限幅,那么它可能会加到6万的pwm值,但是7200与60000的PWM值,它实际上给到的一个转速是一样大的,因为7200是满幅了。但是当我们从6万像回到7200或者零的时候,那么它要减速很久。限幅就是为了防止这个回归时间太长的问题。
![](https://img-blog.csdnimg.cn/direct/88ad716b66134d4c8c364457f5b350d7.png)