マイクロビット（e_38）MOVEminiを動かす Route55Go

micro:bitでMOVE miniを動かす

KitronikのMOVE miniの組立が出来たので自走させてみました。
サーボモータなどの制御をmicro:bitで行います。
MOVE miniのサーボモータ（FS90R）を周期20msecで使うとPWM値は77±41（+ccw、-cw）です。77でニュートラル（静止）します。
右側サーボの制御にmicro:bitのPin1、左側サーボの制御にPin2を使っています。※前回の記事を参照ください。
pin1.set_analog_period(20)で周期20msecに設定します。
pin1.write_analog(pwm値)でサーボモータを動かします。

スクリプト

スクリプト例は以下のようにしてみました。※thonny-microbitのMicroPythonを使っています。


move_mini_run_03.py
from microbit import *
# pins
RS=pin1       # servo right(pin1)
LS=pin2       # servo left (pin2)
# right servo
def RSV(pwm):
    RS.set_analog_period(20)      #周期を設定　20msec
    RS.write_analog(pwm)
    #print('pwm_L %3d' %pwm)
# left servo
def LSV(pwm):
    LS.set_analog_period(20)      #周期を設定　20msec
    LS.write_analog(pwm)
    #print('pwm_L %3d' %pwm)
# pwm off
def M_STOP():
    RS.write_analog(0)
    LS.write_analog(0)
# neutral pwm=77
N_pos=77
# neutral position
def M_NPOS():
    RSV(N_pos) #neutral
    LSV(N_pos) #neutral
#----------------------------------------FRONT-BACKWARD
# 前進
def M_FWD(DAT=[41,41],tm=2000,wt=1000):
    LSV(N_pos+DAT[0])   # CW
    RSV(N_pos-DAT[1])   # CCW
    sleep(tm)           # move_time
    M_NPOS()            # neutral stop
    sleep(wt)           # wait_time
# 後進
def M_BWD(DAT=[41,41],tm=2000,wt=1000):
    LSV(N_pos-DAT[0]) # CW
    RSV(N_pos+DAT[1]) # CCW   
    sleep(tm) 
    M_NPOS()
    sleep(wt) 
#----------------------------------------FRONT TURN
# 前右回
def M_FTR(DAT=[41,41],tm=850,wt=1000):
    LSV(N_pos+DAT[0])
    #RSV(N_pos+DAT[1])
    sleep(tm)
    M_NPOS()
    sleep(wt)   
# 前左回
def M_FTL(DAT=[41,41],tm=850,wt=1000):
    #LSV(N_pos-DAT[0])
    RSV(N_pos-DAT[1])
    sleep(tm)
    M_NPOS()
    sleep(wt)
#----------------------------------------SPIN TURN
# 前左小回
def M_FSL(DAT=[41,41],tm=1000,wt=1000):
    LSV(N_pos-DAT[0])
    RSV(N_pos-DAT[1])
    sleep(tm)
    M_NPOS()
    sleep(wt)
# 前右小回
def M_FSR(DAT=[41,41],tm=1000,wt=1000):
    LSV(N_pos+DAT[0])
    RSV(N_pos+DAT[1])
    sleep(tm)
    M_NPOS()
    sleep(wt)

# PWM値
FHSP=(15,14)    # 前進 L,R SPEED  HI(15,14) LO(7,7) 
FLSP=(7,7)
BHSP=(15,12)    # 後進 L,R SPEED  HI(15,12) LO(10,6)
BLSP=(10,6)
# 動作時間の目安
#  90deg Turn LOW  880msec
# 180deg Turn LOW 1650msec 
# 動作テスト
M_STOP()
M_NPOS()
print('Neutral')
sleep(5000)

# 前後 TEST
print('前後')
M_FWD(FHSP,1000)
M_BWD(BHSP,1000)
# TURN TEST
for n in range(4):
    print('右90')
    M_FWD(FHSP,1000)
    M_FTR(FLSP,830)
for n in range(4):
    print('左90')
    M_FWD(FHSP,1000)
    M_FTL(FLSP,830)
for n in range(2):
    print('右180')
    M_FWD(FHSP,1000)
    M_FTR(FLSP,1650)
for n in range(2):
    print('左180')
    M_FWD(FHSP,1000)
    M_FTL(FLSP,1650)
for n in range(2):   
    M_FWD(FHSP,1000)
    M_FSL()
    M_FWD(FHSP,1000)
    M_FSR()    
M_STOP()            # 停止

スクリプトの説明

スクリプトの部分説明です。

MOVEminiの動作

前進 >> 後進 >>
（前進>右回90°）x4回 >>（前進>左回90°）x4回 >>
（前進>右回180°）x2回 >>（前進>左回180°）x2回 >>
（前進>左回スピン>前進>右回スピン）x2回
なります。

関数の説明

M_FWDを例に説明します。


# 前進
def M_FWD(DAT=[41,41],tm=2000,wt=1000):
    LSV(N_pos+DAT[0])   # CW
    RSV(N_pos-DAT[1])   # CCW
    sleep(tm)           # move_time
    M_NPOS()            # neutral stop
    sleep(wt)           # wait_time

DAT=[41,41]
左右サーボのPWM値をneutral値(77)から加減する値です。デフォルトは41にしました。
かなりいい加減ですが、PWM値と30秒間の回転数を目視でカウントし下図を作成しました。かなりアバウトです。
左右のサーボの回転数が違ったので、FHSP=(15,14)のように動作によって変更できるようにしました。実際には左右同じになるポイントが無く上手く調整できませんでした。
tm=2000
動作時間(msec)です。上記では前進する時間はデフォルト値で2secです。回転の場合は動作時間で回転角が変わります。
かなりいい加減な計算です。
車輪円周17.3cm、車軸間の円周44cm、FLSP=(7,7)で23回転だったとして、360度回頭は44/17.3*30/23=3.32secになります。そのため180degは1650msec、90degは830msecにしています。
wt＝1000
待機時間(msec)です。動作後に待機時間を設けています。デフォルトでは前進後に1sec待ちます。

実行結果

Thonnyのshellに表示された結果です。問題無く動いているようです。確認ができたのでスクリプトをmicro:bitにアップロードしてMove miniを単独で動作させます。具体的にはThonnyのメニューから「Device>Upload current script as main script」を選択します。
>>> %Run move_mini_run_03.py
Neutral
前後
右90
右90
右90
右90
左90
左90
左90
左90
右180
右180
左180
左180
左SPIN
右SPIN
左SPIN
右SPIN
>>>
動作の映像です。
だいたいは動いてます。サーボの回転数が左右で違うので直進、後進で曲がってます。車輪が滑っていることにも原因ありますが、左右の回転角を合わすにはサーボ動作時間の調整が必要なようです。