The Fool In The Valleyの雑記帳

-- 好奇心いっぱいのおじいちゃんが綴るよしなし事 --

EV3用ジャイロセンサを使う

先週購入したMindstorms EV3用のジャイロセンサをPythonで動かすことができました。
センサを傾けると、角速度と角度がそれらしい値を示しているのでよさそうです。

だから何だと言われそうですが、角速度と角度が検出できるとうれしいのです。(笑)

下は実験の様子のビデオです。
センサを垂直にした場合
youtu.be

センサを水平にした場合
youtu.be

実験用のプログラム

#!/usr/bin/env pybricks-micropython

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import GyroSensor
from pybricks.parameters import Port, Color, ImageFile, SoundFile
from pybricks.tools import wait, StopWatch
from pybricks.media.ev3dev import Font

# Initialize the EV3
ev3 = EV3Brick()
big_font = Font(size=24, bold=True)
# Initialize the gyro sensor. It is used to provide feedback for balancing the
# robot.
gyro = GyroSensor(Port.S2)

single_loop_timer = StopWatch()
accum_loop_timer = StopWatch()

CALIB_LOOP_COUNT = 200
OFFSET_FACTOR = 0.0005
TARGET_LOOP_PERIOD = 15  # ms

while True:
    gyro_value_min, gyro_value_max = 440, -440
    gyro_sum = 0
    for _ in range(CALIB_LOOP_COUNT):
        gyro_value = gyro.speed()
        gyro_sum += gyro_value
        if gyro_value > gyro_value_max:
            gyro_value_max = gyro_value
        if gyro_value < gyro_value_min:
            gyro_value_min = gyro_value
        wait(5)      # msec
    if gyro_value_max - gyro_value_min < 2:
        break
gyro_drift = gyro_sum / CALIB_LOOP_COUNT

ev3.screen.clear()
ev3.screen.draw_text(10, 50,'os = '+str(gyro_sum))
ev3.screen.draw_text(10, 70,'gs = '+str(gyro_drift))
wait(2000)
ev3.speaker.play_file(SoundFile.READY)

control_loop_count = 0
gyro_angle = 0.0
ev3.screen.set_font(big_font)

while True:
    single_loop_timer.reset()
    
    if control_loop_count == 0:
        average_loop_period = TARGET_LOOP_PERIOD / 1000
        accum_loop_timer.reset()
    else:
        average_loop_period = (accum_loop_timer.time() / 1000 /
                                    control_loop_count)
    control_loop_count += 1
    
    # calculate robot body angle and speed
    gyro_value = gyro.speed()
    gyro_drift *= (1 - OFFSET_FACTOR)
    gyro_drift += OFFSET_FACTOR * gyro_value
    ang_vel = gyro_value - gyro_drift
    gyro_angle += ang_vel * average_loop_period

    if control_loop_count % 20 == 0:
        ev3.screen.clear()
        y = round(ang_vel, 1)
        ev3.screen.draw_text(10, 30,'a.vel = '+str(y)) 
        x = round(gyro_angle, 1)
        ev3.screen.draw_text(10, 60,'angle = '+str(x))
    
    wait(TARGET_LOOP_PERIOD - single_loop_timer.time())

ここ2年程、Mindstormsから遠ざかっていて、いろんなことをすっかり忘れてしまっていて、時間がかかってしまいました。
でもなんとかここまではできたので、これからビールでお祝いします。(笑)