EV3用MicroPythonでジャイロボーイのプログラミング（失敗編） ～レゴマインドストームEV3～
コンテンツ
MicroPythonでジャイロボーイ
教育用レゴマインドストームEV3でMicroPythonでジャイロボーイのプログラミングをしてみた時の備忘録。
EV3用のMicroPythonについてはこちらの記事を参照してほしい。
尚、最初に断っておくがPythonでのジャイロボーイのプログラミングは上手く行かなかった。
理由は後述するがレゴマインドストームソフトウェアと同じロジックをPython上でプログラミングしても数秒は直立するのだが安定して直立する事は無かった。
その原因の考察の意味も込めて記事として残しておく。
尚、記事の最後に実際に動かしてみた時の動画を載せている。
プログラム
本来のジャイロボーイのプログラム（こちらの記事を参照）は直立のロジックの他にカラーセンサーからの読み取りや超音波センサーでの距離の測定に関するロジックがあるのだが、とりあえずPythonでは直立のロジックだけを移植した。
ソースコード
Pythonのソースコードは以下の通り。
#!/usr/bin/env pybricks-micropython
from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
import time
import os
POWER_RATE = 9.6
class GyroBoyClass: # ジャイロボーイのクラス
def __init__(self): # コンストラクタ
self.D_motor = Motor(Port.D, Direction.CLOCKWISE)
self.A_motor = Motor(Port.A, Direction.CLOCKWISE)
self.gyro_sensor = GyroSensor(Port.S2, Direction.CLOCKWISE)
def Reset(self): # リセット
self.A_motor.reset_angle(0)
self.D_motor.reset_angle(0)
self.gyro_sensor.reset_angle(0) # ジャイロセンサーのリセット
self.timer2 = time.time()
self.mSum = 0
self.mPos = 0
self.mD = 0
self.mDP1 = 0
self.mDP2 = 0
self.mDP3 = 0
self.Cdrv = 0
self.cLo = 0
self.gAng = 0
self.pwr = 0
self.st = 0
self.Cstr = 0
def GyroOffset(self): # ジャイロオフセット
# gyroの平均角速度の算出
# gyro(角速度）前方：プラス、後方：マイナス
__dif = 2000
while __dif >= 2: # 角速度の最大、最小の差分が２以下になるまで繰り返す
__gMn = 1000
__gMx = -1000
__dif = __gMn - __gMx
__gSum = 0
for _ in range(200):
__angle_rate = self.gyro_sensor.speed() # ジャイロセンサーの角速度
__gyro = __angle_rate
__gSum = __angle_rate + __gSum
if __gyro > __gMx:
__gMx = __gyro
if __gyro < __gMn:
__gMn = __gyro
time.sleep(0.004)
__dif = __gMx - __gMn
self.gOS = __gSum / 200 # １ループ平均
def SetgAng(self, value): # gAngに値をセットする
self.gAng = value
def GyroTime(self): # ループ時間
if self.cLo == 0: # 初回
self.tInt = 0.014
self.timer1 = time.time()
else: # ２回目以降
__elap_time = gyro_boy.ElapTime(self.timer1) # 経過時間を計測
self.tInt = __elap_time / self.cLo
self.cLo += 1
def ElapTime(self, start_time): # 経過時間を測定する
__elap_time = time.time() - start_time
return __elap_time
def GyroGyro(self): # ジャイロセンサーの値を計算する
__angle_rate = self.gyro_sensor.speed() # ジャイロセンサーの角速度
self.gOS = (0.0005 * __angle_rate) + (( 1 - 0.0005) * self.gOS)
self.gSpd = __angle_rate - self.gOS
self.gAng = self.gAng + (self.gSpd * self.tInt)
def GyroMotor(self): # モーターの値を計算する
__mSumSave = self.mSum
self.mSum = self.A_motor.angle() + self.D_motor.angle()
# mDiff = self.D_motor.angle() - self.A_motor.angle() # これは恐らく使っていない
self.mD = self.mSum - __mSumSave # mD 前回の回転角度との差異
self.mPos = self.mPos + self.mD # mPos 累積の回転角度
self.mSpd = ((self.mDP1 + self.mDP2 + self.mDP3 + self.mD) / 4) / self.tInt # 過去４回の平均スピード
self.mDP3 = self.mDP2
self.mDP2 = self.mDP1
self.mDP1 = self.mD
def Equation(self): # ジャイロセンサーとモーターの値から計算する
self.mPos = self.mPos - (self.tInt * self.Cdrv)
self.pwr = ((-0.01 * self.Cdrv) + ((0.08 * self.mSpd) + (0.12* self.mPos)) + ((0.8 * self.gSpd) + (15 * self.gAng))) * POWER_RATE
if self.pwr > 960: # 最大角速度を超えた時 160rpm=160*360/60=960度/秒
self.pwr = 960
if self.pwr < -960:
self.pwr = -960
def Control(self): # 左右のパワーのコントロール
self.mPos = self.mPos - (self.Cdrv * self.tInt)
self.D_motor.run(self.pwr + (self.Cstr * 0.1))
self.A_motor.run(self.pwr - (self.Cstr * 0.1))
def Check(self): # ループの終了チェック
__ok = False
if abs(self.pwr) < 960:
self.timer2 = time.time()
elap_time = gyro_boy.ElapTime(self.timer2)
if elap_time > 1: # １秒以上最大角速度で動作した時
__ok = True
return __ok
def MotorZero(self): # Lモーターの速度をゼロにする
self.D_motor.run(0)
self.A_motor.run(0)
#main
try:
if __name__ == "__main__":
os.chdir(os.path.dirname(os.path.abspath(__file__))) # カレントディレクトリをプログラムのあるディレクトリに移動する
gyro_boy = GyroBoyClass()
while True:
gyro_boy.Reset()
gyro_boy.GyroOffset()
gyro_boy.SetgAng(-0.25)
check_ok = False
while check_ok == False:
gyro_boy.GyroTime()
loop_time = time.time()
gyro_boy.GyroGyro()
gyro_boy.GyroMotor()
gyro_boy.Equation()
gyro_boy.Control()
loop_sec = gyro_boy.ElapTime(loop_time) # 経過時間を測定する
# print('loop_sec =', loop_sec)
check_ok = gyro_boy.Check()
loop_intvl = 0.005-loop_sec
if loop_intvl < 0:
loop_intvl = 0
time.sleep(loop_intvl)
except KeyboardInterrupt:
pass
gyro_boy.MotorZero()違い
コメントを記入しているのでロジックの詳細な説明は省くが”基本的に”レゴマインドストーム・ソフトウェアでGUI形式でプログラミングしたものと同じロジックになっている（はず）。
ただ全く違いが無いかというと、そうではなく特にモーターのパワーの指定方法を変更している。
レゴマインドストーム・ソフトウェアでは０～１００（％）のパワーで指定をしているがPythonでは角速度（度／秒）で指定することになる（パワー指定が出来ない）
アフレル社のホームページのLモータ仕様によるとLモータのスペックは１６０～１７０ｒｐｍとある。
つまりパワー１００（％）の時に１分間に１６０～１７０回転するという事なのでこれを角速度に置き換えてみる。
１６０✕３６０（度）÷６０（秒）＝９６０度／秒
の前提プログラミングしている。
※１３行目の POWER_RATE = 9.6 で調整している
状況
このプログラムを動かしてみた所、最初の何秒かは直立するのだが数秒で倒れてしまう。
倒れる様子を見ると車体が傾いた時の補正が間に合っていないように見える。
ジャイロボーイが直立する基本的な仕組みは、
- 車体が前に傾く→ジャイロセンサーの値がプラス→モーターを前に駆動する→前への傾きが補正される
- 車体が後ろに傾く→ジャイロセンサーの値がマイナス→モーターを後ろに駆動する→後ろへの傾きが補正される
上記を素早く繰り返して直立しているだが、どうも傾きの補正が間に合わずに倒れてしまっている様に見える。
検証
上記の状況からプログラムの処理速度に違いがあるのではないかと推定してループ時間を計測してみた。
レゴマインドストーム・ソフトウェアの場合
下記の様に表示ブロックでBALのループ時間を計測した所、０．００５～０．０１０秒／１ループだった。
Pythonの場合
Pythonの場合は上記１３７行目のprint文で計測した所０．４９０～０．６６０／１ループと、約１０倍の処理時間がかかっていた。
考察
Pythonの方は処理時間がかかりジャイロセンサー傾きの補正が間に合わずに数秒しか直立できないのだと推測した。
Pythonはインタープリター言語で１行ずつ逐次機械語に翻訳するのでどうしても実行に時間が掛かってしまいジャイロボーイの様なプログラミングには向かないのかも知れない。
ただ、今はかなり冗長なロジックなので時間が掛かる処理を工夫すればもう少しマシになりそうなので今後の課題としたい。
しかしレゴマインドストーム・ソフトウェアの０．００５秒（５ミリ秒）は早すぎると思う。
コンパイルしているのだろうか。
どなたかご存知のかたは教えて頂けるとありがたい。
動画
実際に動かしてみた時の動画
最近のコメント