程小奔是使用的直流电机驱动,由于电机或者结构的一些细微偏差,程小奔在做直线运动时,有可能会遇到走不了直线的问题。我们该怎么去解决这个问题呢? 一般来说,有两种方法
    1. 使用编程语句块,然后根据自己电机的实际运转情况,微调左右轮的速度。以此达到两轮行进速度一致的目的。
         driver语句块.jpg
    2. 也就是本贴所使用的方式,利用陀螺仪的偏航角来对程小奔的行进路线进行比例调节或者PID调节。        
        程序代码请参见附件。

    因为陀螺仪的偏航角获取并没有 图形化语句块的支持,所以这里只有 python的接口。以下是这个如何使用 main.py 的方法和步骤。
【使用步骤】
1. 打开mblock5-Alpha8的程序,设计一个你自己要执行的程序,比如走一个正方形,或者走一个等边三角形什么的。请注意,把你的程序写在按键事件块中。因为主事件块已经被我用来处理角度数据更新和速度矫正算法了。例如写一个按下A按键后,走正方形的程序

走正方形程序.jpg

2. 点击 切换到 代码模式
切换到代码模式.jpg

3. 下载附件中的main.py程序,复制其中的内容,并覆盖当前 mblock5 正式代码以外的代码。本示例中,即覆盖第三行以上的全部代码,因为我的代码文件已经有
import codey
import rocky

代码前后.jpg

4. 确保codey是连接到mblock5的,把所有直线运动的函数按照以下的映射关系进行替换。
rocky.forward(50, 1)         ==》 my_forward(50 ,1)
rocky.backward(50, 1)      ==》 my_backward(50 ,1)
rocky.forward(50)             ==》 my_forward(50)
rocky.backward(50)          ==》 my_backward(50)

替换前后的对比如下:
代码替换前后.jpg

5. 上传代码,并检查结果。

6. 唯一可能需要调整修改的是  update_speed 里面的 比例 P的参数。


【程序的简单说明】
1. 引入头文件除了 codey rocky,还需要引入数学运算库:
import codey
import rocky
import math

2. 引入陀螺仪的库

from makeblock import gyro_board
m_gyro = gyro_board()

3. 几个全局变量,在main.py的注释里面有说明。

4. 这个函数就是获取到陀螺仪的偏航角,并利用三角函数公式算出程小奔的偏航角
def get_rocky_rotation_angle():
    global codey_inclination
    angle = m_gyro.value(2)
    angle = angle / (math.cos(codey_inclination / 180 * 3.1415926))
    return angle

5. 这个函数仅仅用于获取角度偏差
def get_angle_deviation():
    global angle_deviation
    return angle_deviation

6. 这个函数就是利用程小奔的偏航角偏差的比例算出速度矫正的数值。这个p,大家可以根据自己的测试结果进行一些调整。
def update_speed():
    global Integral   
    global target_speed
    p = 0.82
    error = round(p * get_angle_deviation())
    offset = p * error
    rocky.drive(target_speed + offset, target_speed - offset)

7. 这个函数用于重置角度偏差计算时的历史值,之所以需要这个函数,是因为在执行拐弯的语句块之后,我们需要更新角度值。否则转弯函数会失效。
def set_rocky_previous_angle(angle):
    global previous_angle
    previous_angle = angle

8. 紧跟着的就是最重要的两个函数,用于替代 mblock5 里面的
前后运动语句块.jpg

9. 下面这段代码,相当于单独使用一个主事件来处理角度函数以及速度的更新。所以这个事件是不能阻塞的。
def on_start_callback():
    global codey_inclination
    global angle_deviation
    global previous_angle
    previous_angle = get_rocky_rotation_angle()
    codey.reset_time()
    previous_time = codey.time()
    while True:
        current_time = codey.time()
        if(current_time - previous_time > 0.1):
            previous_time = codey.time()
            current_angle = get_rocky_rotation_angle()
            angle_deviation = current_angle - previous_angle
            codey.show(get_angle_deviation())
            if Linear_move_flag == True:
                update_speed()

codey.on_start(on_start_callback)


如果大家有什么使用疑问或者建议,欢迎大家在评论里面告知我。

main.py

2.43 KB, 下载次数: 281

收藏
9 条回帖
sunkf新手会员2018-2-28 23:33:23
程小奔的直线运动校正
1.png
2.png
3.png

程小奔的直线运动校正.json

9.9 KB, 下载次数: 225

lthirty注册会员2018-3-2 22:37:44
不错,里面顶一个
蓝色小星星注册会员2018-3-7 10:10:53
内涵贴,支持一个!
木果创客中级会员2018-3-26 14:41:50
试试看,感谢分享~!
ganli2018新手会员2018-6-4 13:01:49
怎样才能知道makeblock包里有哪些函数,比如gyro_board,有没有完整的文档说明?或者可以看到makeblock包 的源码?
刘刚_huRgd新手会员2018-10-22 11:08:16
不错,生硬地人为调节两边的速度差,遇到地面不完全平坦就抓瞎了。
需要登陆后才可进行回复 登录

返回顶部
现在加入我们,注册一个账号 账号登陆 QQ账号登陆 微博账号登陆