本帖最后由 玩富大师兄 于 2018-11-13 15:34 编辑
粘上来排版不太好看，可以回复下载word版
线程1：保持平衡 //M循环开始 1.RST模块： 电机A复位 电机D复位陀螺仪复位 计数器复位 变量置0 ： mSum,mPos,mD,mDP1,mDP2,mDP3,Cdrv,cLo,gAng,ok,pwr,st,Cstr,Cdrv
2.屏幕显示sleeping表情
3.gOS模块 //取200次陀螺仪测速平均值，直到机器人处于稳定状态 //循环OSL开始 gMn=1000 gMx=-1000 gSum=0 //循环gChk开始 gSum=gSum+陀螺仪速率 //累加200次 陀螺仪速率写入gyro if gyro>gMx gMx=gyro if gyro<gMn gMn=gyro //当gyro在-1000~1000之间时，比如500，gMx=gyro=500，gMn=gyro=500 下一次，gyro=400，gMx仍然为500，gMn=400 下一次，gyro=600，gMx为600，gMn仍然为400 所以，gMx是200次中gyro的最大值，gMn是200次中gyro的最小值
等待0.004s gChk循环200次 //循环gChk结束 直至gMx-gMn<2跳出OSL循环 //循环OSL结束
要求200次中陀螺仪速率的最大值和最小值之差小于2 也就是陀螺仪比较稳定的时候跳出 如果陀螺仪不稳定就继续循环 gOS=gSum/200 //陀螺仪速率累加200次之后再除以200 //此时gOS基本为0
4.gAng=-0.25 5.播放声音speed up 6.屏幕显示 Awake表情
7.st=1 //BAL循环开始 8.GT模块 //计算每轮所需时间tInt if cLo=0 //只有第一次进入时，clo=0 //如果clo=0，clo不能作为除数，所以要单独定义 tInt=0.014 计时器1重置 else tInt=计时器1测量时间/cLo //计算开始循环到本次循环的平均时间 cLo=cLo+1 9.计时器1,测量时间timeA 10.GG模块 //计算本轮角速率gSpd及角度gAng //陀螺仪速率是带符号的整数，gSpd和gOS是带符号的小数 //gSpd的数值比较大，整数部分有时也会达到十几 //gOS的数值比较小，基本在±0.1以下 //因为计算gOS时，陀螺仪速率乘以0.0005 //gSpd是在关键公式里要用的参数 gOS=0.0005*陀螺仪速率+(1-0.0005)*gOS //第一次进入时，gOS为200次陀螺仪速率取平均，基本为0 //以后每次进入时，都用新的陀螺仪速率加权更新gOS gSpd=陀螺仪速率-gOS //gSpd=0.9995*（陀螺仪速率-gOS_old） //gSpd基本表示当前陀螺仪的瞬时速率，只是用gOS值进行了微调 为什么用gOS值来微调？理解的还不太透彻 gOS为负，表示之前所有的陀螺仪速率，为负的时候多，或者为负的值比较大 计算gSpd时，减掉负的gOS，相当于是加了一个正值 如果陀螺仪速率为负，减掉负的gOS，相当于gSpd是比当前陀螺仪速率绝对值小的负值 如果陀螺仪速率为正，减掉负的gOS，相当于gSpd是比当前陀螺仪速率绝对值大的正值 可见，gOS是用历史累积数据，对当前的运动进行补偿，如果历史经验，陀螺仪速率倾向于为负，那么当前负向的陀螺仪速率就削弱一些，当前正向的陀螺仪速率就增强一些
gAng=gAng+gSpd*tInt //tInt是平均每次BAL循环的时间 //速率*时间=角度 陀螺仪速率*本次循环的时间，得到本次循环陀螺仪角度的变化 陀螺仪速率有正负，计算得到的角度gAng也有正负
gAng表示累积的角度，为什么不用陀螺仪来测量角度？ 经过试验，测得用陀螺仪测量的角度和gAng测量值基本接近 不过gAng是小数，陀螺仪测量的角度是整数
11.GM模块 // PS: 计算马达速度修正量mSpd以及位置修正量mPos temp=mSum mSum=D电机旋转度数+A电机旋转度数 //往前走的越远mSum越大，且为正 mDiff=D电机旋转度数-A电机旋转度数 //mDiff后面没用到 mD=mSum-temp //mD是mSum的变化值，表示此次循环，电机转了多少度 用电机的旋转度数来表示离平衡位置多远 mPos=mPos+mD 实际试验，发现mPos和mSum不同，因为在EQ模块和cntrl模块中，mPos=mPos-Cdrv*tInt，mPos的值会被Cdrv改变，而程序运行前4s的时间里，Cdrv为40 验证了一下，把前4s的 Cdrv改为0，mPos就等于mSum了 mPos表示当前离平衡位置的距离，之所以不直接用mSum就是因为，需要用Cdrv来调整。如果控制机器人向前走了一段，然后停止。此时机器人应该认为平衡位置是刚停下的位置，而不是最开始出发的位置。
转弯的时候，A和D的角度不一样，简单的加法就能表示位置吗？ 当有转弯指令时，控制A和D的功率计算为rpwr=pwr+Cstr*0.1，lpwr=pwr- Cstr*0.1 两个电机的角度相加，恰好把控制转动的Cstr抵消了
mSpd=(mDP1+mDP2+mDP3+mD)/4tInt //tInt是一次BAL循环的时间 //四次mD的平均值再除以时间，即电机旋转平均角速率 mDP3=mDP2 mDP2=mDP1 mDP1=mD
12.EQ模块 //计算马达功率 pwr mPos=mPos-tInt*Cdrv //Cdrv用于控制前后运动，在控制运动的线程里，根据颜色传感器识别到的颜色来改变Cdrv的值 pwr=-0.01*Cdrv+(0.08*mSpd+0.12*mPos)+(0.8*gSpd+15*gAng) //这个公式是关键 mSpd是电机旋转平均角速率，mPos表示机器人偏离平衡位置的距离，gSpd表示陀螺仪瞬时速率，gAng表示陀螺仪角度 这些系数很关键，不知道怎么选的，是算出来的，还是试出来的？ if pwr>100 pwr=100 if pwr<-100 pwr=-100 13.cntrl模块 mPos=mPos-Cdrv*tInt //Cdrv在初始化时置0，在控制运动的线程里，st=1之后4s的时间内，Cdrv=40，之后又置0 //Cdrv用于控制前后运动，在控制运动的线程里，根据颜色传感器识别到的颜色来改变Cdrv的值 为什么，在EQ模块里，做一次mPos=mPos-tInt*Cdrv，在cntrl模块里又做一次mPos=mPos-Cdrv*tInt？ 需要减掉两倍的Cdrv*tInt，因为A和D电机的控制参数都加了Cdrv 那为什么在计算pwr的时候，使用只减掉了一倍Cdrv*tInt的mPos？ 实际上，循环起来之后，mPos是减掉了两倍的Cdrv*tInt，只是计算pwr的时候，只减掉了一倍的Cdrv*tInt，
mPos是使用mSum的变化值累加得到的，mSum是两个电机的转动角度相加，那么mPos中包含了两个电机的运动数据，如果人为控制了前后运动，两个电机都会进行相应转动，为了更新平衡位置，让机器人在新位置保持平衡，而不是再回到最开始出发的位置，需要把两个电机的转动数据都减掉
暂时想不清楚，就把这当成一种参数调节 因为pwr=-0.01*Cdrv+(0.08*mSpd+0.12*mPos)+(0.8*gSpd+15*gAng) 本来就是要用Cdrv来调整pwr的，而mPos=mPos- tInt*Cdrv 可以分解为 pwr=-0.01*Cdrv+(0.08*mSpd+0.12*(mPos-tInt*Cdrv)+(0.8*gSpd+15*gAng) pwr=-(0.01+0.12*tInt)Cdrv+(0.08*mSpd+0.12*mPos)+(0.8*gSpd+15*gAng)
rpwr=pwr+Cstr*0.1 lpwr=pwr- Cstr*0.1 //Cstr用于控制左右旋转，在控制运动的线程里，由颜色传感器识别到的颜色控制
14.未校准电机A 使用lpwr //根据功率控制电机 //未校准电机和普通电机的区别：未校准电机不带PID控制、缓启动、缓停止，未校准电机就是傻转 15.未校准电机D使用rpwr
16.CHK模块 //检测机器人是否处于平衡状态 如果pwr的绝对值小于100 计时器2重置 如果计时器2大于1s 在EQ模块中， if pwr>100,pwr=100,ifpwr<-100,pwr=-100 pwr=100或-100时，是不符合pwr绝对值小于100的 只有pwr达到100或-100时，计时器才不重置 pwr的绝对值保持在100达到1s ok为真
17.计时器1，测量时间timeB 18.计算 time_delta=0.005-(timeB-timeA) 19.等待时间 time_delta //保持从（9.计时器1，timeA）到（19. 等待时间 time_delta ）循环的时间为0.005s 实际测量值，tInt维持在0.019左右 可见，BAL循环的其他部分（主要为GT模块），占用时间为0.014s
20.如果变量ok为真 跳出循环BAL 如果变量ok为伪 继续循环BAL //ok为真就跳出，跳出之后，后面显示的表情也是异常状态 //如果ok为伪，继续循环，保持正常状态 //BAL循环结束 21.电机A和D停止运转 22.st=0 23.程序块状态等闪红灯 24.屏幕显示knoced out 表情
//表示无法保持平衡了 25.播放声音speed down 26.等待触碰传感器按下并抬起 27.程序块状态灯重置 M循环，无条件循环 //循环M结束
线程2：控制运动 1.st=0 //正常工作之前，和出现异常之后，st=0 //BHV循环开始 2.st多分支 2.1如果st=0 Cdrv=0 Cstr=0 2.2如果 st=1 //在平衡线程里，开始正常工作时会把st置1 Cdrv=40 等待4s //只有4s的时间Cdrv=40，之后就置0 //机器人向前走一小段 Cdrv=0 播放音调 频率1000 st=2 //4s之后，st自动变为2，下一个循环就进入st=2状态 2.3如果st=2 //启用颜色传感器 //分支“st=2”开始 使用色块来控制Gyro Boy的行进： - 红色 - 停止 - 绿色 - 前进 - 黄色 - 左转 - 蓝色 - 右转 - 白色 - 后退 颜色传感器测量颜色 如果为红色 //停止 播放音调 频率2000 Cdrv=0 Cstr=0 //Cdrv控制前进、后退 Cstr控制转弯 如果为绿色 //前进 播放音调 频率2000 Cdrv=150 Cstr=0 如果为蓝色 //右转 播放音调 频率2000 Cstr=70 如果为黄色 //左转 播放音调 频率2000 Cstr=-70 如果为白色 //后退 播放音调 频率2000 Cdrv=-75 //仍然在分支“st=2”里面 超声波传感器测量距离 如果小于25cm //分支“距离小于25cm”开始 Cstr=0 oldDr=Cdrv Cdrv=-10 //缓慢后退 中型电机开启30度 功率30 中型电机开启60度 功率-30 中型电机开启30度 功率30 //两条手臂摆动 产生-1~1之间的随机数num_random 如果num_random大于等于1 //随机数是-1~1，大于等于1的情况就是为1 “数字”模式输出随机整数值。 值会设置为处于 下限和 上限指定的范围内的随机整数。此范围内的每个整数值的选择概率是相等的。 产生的随机数为-1、0、1，所以，num_random≥1的概率是1/3，num_random＜1的概率是2/3 Cstr=70 否则 Cstr=-70 //仍然在分支“距离小于25cm”里 等待4s 播放音调 频率2000 Cstr=0 Cdrv=oldDr //继续原来的前进或后退运动 //分支“距离小于25cm”结束 //分支“st=2”结束 等待0.1s 循环BHV无条件循环 //BHV循环结束
| 
|手机版|中文乐高
( 桂ICP备13001575号-7 )
IP卡
狗仔卡
发表于 2018-11-13 15:18:39
提升卡
置顶卡
沉默卡
变色卡
千斤顶
显身卡
