1、自定义模块RST
这个模块完成了电机端口、陀螺仪传感器、计时器2的重置，以及数值变量mSum、mPos、mD、mDp1、mDp2、mDp3、Cdrv、cLo、gAng、pwr、st、Cstr、Cdrv，逻辑变量ok（初始化为否）的初始化设置，变量较多，其中：cLo 指循环BAL执行次数（未声明的变量有：gOs 陀螺仪角度的平均偏移量、tInt 循环BAL执行一次需要的时间、gSpd 平均角速度）
2、屏幕显示睡眠表情
3、自定义模块gOS
file:///C:/Users/Lenovo/AppData/Local/Temp/msohtmlclip1/01/clip_image016.jpg
很长的一段，我们分段进行解读
定义gMn、gMx、gSum的值分别为1000、-1000、0
陀螺仪速率写入变量gyro，gSum=gyro+gSum，gSum是gyro的累计
比较gMx和gyro的大小，如果gyro大于gMx，则将gyro的写入gMx，如果小于或等于则不采取任何操作。
这一段和上一段程序比较类似，比较gMn和gyro的大小，如果gyro小于gMn，则将gyro的写入gMn，如果大于或等于则不采取任何操作。
完成上述的操作后等待0.004秒，然后重复执行200次。
解读：这段程序的意思是每隔0.004秒进行一次陀螺仪角度的采样，重复采样200次，也就是进行了0.8秒（官方程序解读中的次数是250次），执行这个循环后，角度gyro偏移的最大值存于变量gMx、gMn中，这也就能解释这两个变量的初始值为什么最大值写入了最小的而最小值写入了最大值，因为只有如此才能实现新值得替换。
计算gMn、gMx两者之间的差值，如果两者的差值小于2则跳出循环。
角度偏移的累计gSum除以200，得到角度偏移的平均值，将这个值写入变量gOS中。
这段程序的意义在于检测结构是否放置平稳，搭建卡中会有一个底座的搭建，用来确保启动前的平稳和每次启动位置趋于一致。
官方程序解读译文：自定义模块g0S使陀螺仪传感器的偏移值保持在稳定范围内，如果陀螺仪略微偏移，便会通过控制机器人移动的方式保持值的稳定，在机器人运行时也会动态计算偏移量。为了计算偏移量，陀螺仪值被读取加在一起200次，在每次累加中，偏移值都会和最大、最小值比较，以确保机器人稳定。在循环gchk结束后，检查gmax和gmin之间的差值，如果该值小于2（小于2则跳出循环gChk，运行后续序列），则计算将继续进行，否则机器人将再次检查陀螺仪的偏移值。
4、变量赋值及发声、显示
变量gAng中写入-0.25，发出声音speed up，显示Awake表情，变量st中写入1。
检测机器放置平稳，并且陀螺仪没有零漂的时候机器人正常唤醒。
5、自定义模块GT
模块说明：比较cLo和0的关系，如果等于零，则在tInt中写入0.014，并重置计时器1，如果不等于0，则令tInt等于计时器1的值除以cLo，最后将cLo增大1。
自定义模块GT的功能在于取得循环BAL执行一次的时间，cLo表示循环执行的次数。第一次执行循环BAL时，时间间隔取0.014，将计时器1重置，后续的循环中，tInt的值通过计时器1除以循环次数获得。
6、时间控制
控制运行完这段程序的时间是0.005秒，当然，两个计时器之间的所有模块运行时间总和不会超过0.005秒
7、自定义模块GG
把0.0005*速率+（1-0.0005）* gOS的值重新写入gOS中，通过陀螺仪实测的速率修正1秒内（250*0.004=1）角度的平均偏移量gOS。
速率（看作速率乘1，表示一秒内的偏移）和偏移量gOS之间的差值写入gSpd（表示1秒内的平均角速度）中，gAng+tInt*gSpd写入gAng中
自定义模块GG从陀螺仪传感器获取信息，变量gOS用于创建动态偏移。 陀螺仪传感器读取速率用于得到新的偏移量gOS。将陀螺仪读取速率和变量gOS之间的差值存在变量gSpd中。当机器人开始运动时，通过计算的方式获取角度偏移比从陀螺仪中直接读取更快，并且可以补偿陀螺仪中的漂移。
8、自定义模块GM
这个运算关系就……太复杂了吧，不怕，我们一步步去看。
将端口A、D上电机的角度和写入mSum中，电机D和电机A的角度差写入mDiff中，计算上一次mSum和这一次mSum的差值，并且将差值写入mD中，mD+mPos写入mPos中。
赋值关系：
mSum=angleA+angleD //角度和
mDiff=angleD-angleA //左右轮角度偏差
mD=mSum-mSum //车体前进的角度
mPos=mD+mPos //电机当前的角度
运算：
mSpd=(mD+mDP1+ mDP2+ mDP3)/4*tInt //计算得到的电机前进速度
这段程序很有意思，mDP2写入mDP3中，mDP1写入mDP2中，mD写入mDP1中，可以知道mD、mDP1、mDP2、mDP3中存的的依次是最近一次、上一次、上上次、上上上次的mD值，这样的操作可以完成指定的四个数值的顺次更新，
自定义模块GM从电机获取信息。通过计算电机A和D的度数和mSum前后两次的差值（存在mD中）来计算电机位置。变量mD是初始差异，它被添加到旧的mPos并生成新的mPos。表示电机速度的变量mSpd，是通过取4个电动机差异的平均值并除以变量tInt来创建的（平均变化量除以时间等于平均速度），最后一个块将运动差异移动到其他变量中，实现只记录最近四次运动差异的效果。
9、自定义模块EQ
把mPos-tInt*Cdrv的运算结果再次写入mPos，
pwr=-0.01*Cdrv+gAng*15+gSpd*0.8+0.08*mSpd+0.12*mPos
车体直行的实时功率pwr是设定功率Cdrv，车体倾角gAng，角速度gSpd，计算得到的电机速度mSpd，以及电机当前角度mPos的函数。大部分参数的获取不是通过传感器直接获取，而是计算获取，准确度和稳定性
如果pwr的值大于100，则在pwr中写入100，如果pwr的值小于-100，则在pwr中写入-100。
Cdrv的值通过下方的操作程序控制，初始为40，4秒后为0。
据此可知pwr为电机功率，因为经过算法得出的调整功率范围不能超出电机的功率范围-100到100。
10、功率控制
把mPos-Cdrv*tInt的值存入mPos中
左轮的功率为：pwr+Cstr*0.1
右轮的功率为：pwr-Cstr*0.1
变量Cstr的存在是为了下面并行程序中实现颜色控制的效果，只要Cstr不为零，车体就会转弯
11、自定义模块CHK
功率pwr的绝对值小于100，则重置计时器2，如果计时器2大于1秒（官方程序解读是跟2比较），就在ok中写入逻辑“是”。
12、机器人停止工作
当机器人以最大功率运行超过一秒时，结束内部的循环，停车、st中写入0，亮红灯，显示Knocked out表情，发出Speed down的声音，按下并送开3号端口的触动传感器则重置屏幕，进入下一次循环。
（二）动作控制程序
循环BHV控制机器人的行为。状态变量s的值用来实现三种状态的切换：如果变量s为0，则变量Cdrv和Cstr设置为0，这是机器人的空闲状态。如果s为1，机器人执行启动序列，变量Cdrv设置为40持续4秒，然后降为0，接着在变量s中写入2，以便开始进入新的运动状态。
当变量s为2时，它是机器人的主要操作和交互状态。检查颜色传感器，对于每种可用颜色，变量Cstr和Cdrv有不同的值。
在超声波模式下，检查是否有障碍物在机器人前面。如果有，机器人停止并保存其最后的驾驶条件，然后稍微向后移动并挥动它的手臂转动，接着机器人随机向左或向右移动几秒钟并返回其先前的驾驶状态。
三、写在最后
（一）、计算真的比实测的还要快、准、稳吗？
程序中有两个关键的参数（陀螺仪实测速率和gSpd、实测电机功率和mSpd）是通过计算或者是计算和实测结合的方式代替了直接冲陀螺仪或者电机中读取，并且强调这样的方式获取的参数更快，更稳定，后续将利用EV3文件读写功能进行参数的对比，是否更快、更稳、更准，我们拭目以待。
（二）、你可以做做下面这些尝试
1、机器人的直行的快慢受变量Cdrv控制，转弯速度差的大小受变量Cstr控制，可以尝试固定一个颜色传感器做成平衡车巡线的效果，也可以做成遥控版的平衡小车（亲测有效）；
2、官方程序中的启动条件太过苛刻，如果想放宽启动条件（至少用手能扶着启动），可以更改哪些参数；
3、官方程序冗长难懂，是否可以有更好的编写方式，写得易懂一些；
4、官方程序在实测过程中，推动幅度稍大就会因为保护程序而倾倒，相关参数的设定是否可以写成角速度的分段函数，以应对不同的外界干扰。
|手机版|中文乐高
( 桂ICP备13001575号-7 )
IP卡
狗仔卡
发表于 2019-4-4 21:13:12
）
提升卡
置顶卡
沉默卡
变色卡
千斤顶
显身卡
