Gyrod　【ソフトウェア編】 - The Fool In The Valleyの雑記帳

GyrodはLEGOがMindstorms EV3のための高度なプログラミング環境として用意したMicropythonを使用しています。
Micropythonについては、本体、周辺デバイスを使うためのライブラリがあり、下記のサイトで参照することができます。
pybricks.com

またそこにはライブラリの使い方を紹介したサンプル・プログラムがいくつか公開されていて、そのなかにGyroセンサーを用いて二輪で走行するGyroboyというプログラムがあります。
pybricks.com

Gyrodはそれにインスパイアされて創ったもので、ソフトウェアの基本的な枠組みはそのGyroboyを参考にしています。

Gyrodのソフトウェアでキーになるのは、①バランスを取って直立させるルーチンと②外部からのコマンドを受け付けるルーチンです。
①はメインのルーチンで、一定の周期で、姿勢と位置を測定し、その値を使って直立を保持するための制御入力を計算し、それに基づいて左右のモーターを駆動します。②はPythonのジェネレータとして生成され、コマンドを生成するイテレータとして動作します。赤外線送信機からのコマンド信号を常に監視していて、①のメインからの要求があった時にコマンドをyieldで送ることにより処理の並列性を実現しています。下図は①、②の関係を図示したものです。

実際のコードは300行程度のものですが、少し冗長になるので、その概要を下に疑似コードで示します。

#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.iodevices import I2CDevice
from pybricks.ev3devices import Motor, UltrasonicSensor, TouchSensor, GyroSensor
from pybricks.parameters import Port, Color, ImageFile, SoundFile
from pybricks.tools import wait, StopWatch
from ucollections import namedtuple
import urandom

本体、デバイス（モーター、ジャイロセンサ、赤外線レシーバー、超音波センサ）の初期化
各種タイマーの初期化

Action = namedtuple('Action ', ['speed', 'steering'])　# 名前付きtupleを定義
使いたい複数のActionの種類を 'speed', 'steering' の値を入れて定義する

def get_command():
    　While True:
　　　　赤外線送信機からのコマンドを受信したとき、new_commandにそのActionを代入する
　　　　yield new_command　　# メインの next(sction_task)に値を送る
　　　　new_command に変更があれば腕を振る　

while True:   #ここからがメインプログラム
　　モーター、タイマー、制御変数( speed, steering, ... etc.   )の初期化
　　action_task = get_command()
　　ジャイロセンサのドリフトの測定、ロボットが静止していることを確認

　　While True:
　　　　処理ループのタイマーのリセット
　　　　制御ループの平均処理時間を計算

　　　　ジャイロセンサの値からロボットの傾き角θ、回転の角速度dθ/dtを計算
　　　　モーターのエンコーダーの値から車輪の回転角ξ、角速度dξ/dtを計算

　　　　制御入力量を計算 （θ、dθ/dt、ξ、dξ/dtの線形結合）

　　　　制御入力とspeed、steeringから左右モーターの回転させる

　　　　モーターの回転が飽和した状態が一定時間続いた場合は制御不能と判断し、ループから出る

　　　　action = next(action_task)　　# get_command() からyield で new_command を得る
　　　　speed, steering = action 　　 # speedとsteeringの値を新しいactionでアップデート

　　　　while ループが一定時間（20msec）になるように時間を調整

　　制御不能の場合モーターを停止し、メインプログラムの最初に戻す