稼働中

マイクロビット(e_40)MOVEminiをRadioで操作(2)

MOVE miniをRadio通信で操作2

MOVE miniをRadio通信で制御した前記事のスクリプトを改良してみました。前記事と同様に、micro:bitに搭載されているI2Cデバイスの加速度センサの値を使って無線で操作します。

スクリプト

前進、後進時の左右のサーボモータの回転数を変えて左右に曲がるようしました。後進でも曲がることができます。
左右回転の関数は不要になったので、Movemini(受信)側のスクリプトはやや短くなりました。
コントローラ(送信)側はやや複雑になりました。送信側で加速度センサ値を加工して、サーボ動作時間、左右サーボPWM値などを送信するようにしました。その関連でボタン操作時の送信値を前記事(e-39)では’B’+数文字(’1’、’2’、’3’)値でしたが、’B’+文字(’A’,’B’,’C’)としました。受信側も合わせて変更しました。

スクリプト例は以下のようにしてみました。※thonny-microbitのMicroPythonを使っています。
radioのchannel、queueなどconfigはデフォルト状態で使用しています。
※ThonnyのメニューからGeneral>「Allow only single Thonny instance」のチェックを外せば複数のThonnyを起動できます。送信用と受信用(MOVE mini)にThonnyを2つ起動してスクリプトの評価ができます。

送信側(コントローラー)


radio_send_13bb.py
from microbit import *
import radio

# 受信済みの返信待ち CMD送信したら返信あるまで待機する 前回と同じ
def RS():
    while True:
        r_msg = radio.receive()
        if r_msg:
            # from Move recieved
            print(r_msg)
            break
# acc x,y  新規追加
def M_SVM(x_acc,y_acc):
    t_dat=int(abs(800/1023*y_acc))
    d_pwm=int((abs(y_acc)-255)/96 + 8 )
    # 左右の傾きでSVMのPWM減算値 直進_d_pwmに率を掛けて半分
    corr=int(abs((x_acc/1023)*d_pwm/2))
    # x:R(+)L(-) Y:B(+)F(-) Z:DN(+)UP(-)
    # 右傾斜なら右曲進、右サーボを遅くする(減算する)補正correction
    if x_acc>=0:
        # Turn  L 
        # s_pwm:補正後のPWM(L,R)
        s_pwm=(d_pwm, d_pwm-corr)
    # 左傾斜
    else:
        # Turn  R 
        s_pwm=(d_pwm-corr,d_pwm)
    # カウント、元PWM値、補正値、補正後のPWM(L,R)
    #print(cnt,'pwm,corr,s_pwm',d_pwm,corr,s_pwm)
    return t_dat,s_pwm         # servo on-time,pwm(L,R)
# --MAIN--
radio.on()
# PUSH Aボタンで開始、イメージ矢印を表示 A+Bで中止  数文字と文字に変更しただけ
while True:
    print('START Push A')
    display.show('A')    # 乾電池電源で単独動作の時に’A'表示あればわかりやすい
    sleep(1000)
    display.clear()
    sleep(500)
    if button_a.is_pressed() & button_b.is_pressed():
        display.clear()
        print('A+B STOP')
        radio.send('BA')    # 開始してから停止させる(受信側の都合)
        sleep(10)
        radio.send('BC')    # 停止送信
        break               # 中止

    if button_a.is_pressed():
        print('GO')         # イメージ矢印を表示、頭文字+数文字'BA'送信
        display.show(Image.ARROW_N)
        radio.send('BA')
        RS()                # 返信待
        break               # 待機から抜けて 操作 開始
cnt=0
btn='NON'
while True:    
    #cnt=cnt+1
    # x:R(+)L(-) Y:B(+)F(-) Z:DN(+)UP(-)
    r_acc=accelerometer.get_values()        # accデータ値を取得
    #print(cnt,r_acc)   
    if button_a.is_pressed() & button_b.is_pressed():
        btn='C'
    elif button_b.is_pressed():
        btn='B'        
    elif button_a.is_pressed():
        btn='A'    
    else :
        btn='NON'
    #print(cnt,msg,btn)   
    # ボタン 
    if btn != 'NON':
        msg='B'+btn  # ボタン操作の頭文字’B’+文字'A''B''C' 変更 
        radio.send(msg)
        cnt=cnt+1
        print(cnt,'btn=', btn, msg)
        RS()   # 返信待
        sleep(200) # チャタリング防止        
        if btn=='C':
            display.clear()
            print('Break-END')
            radio.off()
            break
    # 大幅に変更した部分(傾きから左右PWM値補正、動作時間)  
    if abs(r_acc[1])>=255:
        cnt=cnt+1
        if r_acc[1]>0:
            drc='G'    # 後進文字'G'
        else:
            drc='F'    # 前進文字'F'
        # M_SVMへ送る、サーボ動作時間、補正後のPWM値(L,R)とを返す
        DT= M_SVM(r_acc[0],r_acc[1])
        #print(cnt,r_acc,'DT=',DT) # e.g. DT= (675, (14, 12))
        # COMMAND 頭文字+数文字
        s_msc=drc+str(DT[0])      # direc+servo on time
        l_pwm='L'+str(DT[1][0])   # PWM-L
        r_pwm='R'+str(DT[1][1])   # PWM-R        
        # 送信する文字列    e.g. F337L9R9  G225L8R5
        msg=s_msc+l_pwm+r_pwm
        radio.send(msg)
        print(cnt,'send-msg=',s_msc,l_pwm,r_pwm)
        #sleep(100)
        RS()   # 返信待
    else:
        #print('y_acc <255 Pass')
        pass

受信側(MOVE mini)


move_radio_recieve_13-2bb.py
from microbit import *
import radio

# 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 position pwm=77
N_pos=77
def M_NPOS():
    RSV(N_pos) #neutral
    LSV(N_pos) #neutral   
#----------------------------------------FRONT-BACKWARD
# 前進
def M_FWD(DAT,tm,wt=0):
    LSV(N_pos+DAT[0]) # CW
    RSV(N_pos-DAT[1]) # CCW
    sleep(tm)  # move_time
    M_NPOS()
    sleep(wt)  # wait_time
# 後進
def M_BWD(DAT,tm,wt=0):
    LSV(N_pos-DAT[0]) # CW
    RSV(N_pos+DAT[1]) # CCW   
    sleep(tm) 
    M_NPOS()
    sleep(wt) 
# 大幅に変更した部分  頭文字'L''R'からPWMデータを分離
def SEP(dat): 
    pos=0
    for i in dat:
        if i=='L':
            # 'L'no ichi
            sl=pos
        if i=='R':
            # 'R'no ichi
            sr=pos   
        pos=pos+1
    t_msc=int(dat[1: sl])       # str>>int slice
    l_pwm=int(dat[sl+1: sr])
    r_pwm=int(dat[sr+1: ])
    d_pwm=(l_pwm, r_pwm)        # PWM(L,R)
    #print(d_pwm,t_msc)         #servo PWM(L,R),on-time
    return d_pwm,t_msc
# 返信  追加
def RS_msg():
    s_msg='recived'
    radio.send(s_msg)      
# -- Main ---
M_STOP()
M_NPOS()   # Neutral
sleep(1000)
print('START')

radio.on()
cnt=0
while True: # ボタンAが押されるまで待機 数文字と文字に変更しただけ
    r_msg = radio.receive()
    print('Ready A',r_msg)
    sleep(1000)
    if r_msg=='BA': #'A'(1)
        RS_msg() 
        display.show(Image.ARROW_S)
        sleep(200)
        break
while True:
    r_msg = radio.receive()
    if r_msg:
        cnt=cnt+1        
        #print(cnt,r_msg)
        # Button   ボタン関連は数文字と文字に変更、返信追加
        if r_msg[0]=='B':
            print('button')
            if r_msg=='BC':  #'C'
                print('buttonA+B END')
                M_STOP()
                RS_msg()                
                display.clear()
                radio.off()
                break
            if r_msg=='BB':   #'B'
                print('buttonB')
                display.clear()
                RS_msg()
                continue
            if r_msg=='BA':   #'A'
                print('buttonA')
                display.show(Image.HAPPY)
                RS_msg()
                continue      
        # 前後進
        # 大幅に変更した部分 これだけになった
        # SEP(r_msg) >> return d_pwm, t_dat
        m_dat=SEP(r_msg)     
        print(cnt, r_msg, r_msg[0], m_dat)
        if r_msg[0]=='F':
            print('前')
            M_FWD(m_dat[0], m_dat[1], 0) # d_pwm, t_dat, 0
        else:
            print('後')
            M_BWD(m_dat[0], m_dat[1], 0)
        # 返信
        RS_msg()
    else:
        pass
        #sleep(1000)

スクリプトの説明

スクリプトの部分説明です。
前記事から変わった部分の説明です。※他の部分は前記事(e-39)を参照ください。

送信側(コントローラー)

def M_SVM(x_acc,y_acc):の部分
加速度センサのデータのy値からサーボ動作時間(t_dat)とPWM値(d_pwm)、x値からPWM値の補正値(corr)を計算しています。
t_dat、d_pwm
前記事と同じです。※前記事(e-39)を参照ください。
corr
引き算しても負にならないように。最大値はd_pwmの最低値(8)にしました。
s_pwm
場合分けして補正値をd_pwm値から減算しています。
x値が正なら前進しながら右曲げさせたいので、右側の回転数を下げます。PWM(L,R)のRを補正値を引いて小さくします。「s_pwm=(d_pwm, d_pwm-corr)」
x値が負でも、同じように計算して補正したPWM値(s_pwm)を返しています。

# 大幅に変更した部分(傾きから左右PWM値補正、動作時間)の部分
加速度センサのデータのy値から前進、後進を判断して前進なら’F’、後進なら’G’を頭文字にします。
続けてサーボ動作時間(t_dat)をつなぎます。
s_msc=drc+str(DT[0]) #’F’or’G’+servo on time
補正したPWM値(s_pwm)は左右で頭文字’L’’R’を付けてつなぎます。
l_pwm=’L’+str(DT[1][0]) # PWM-L
r_pwm=’R’+str(DT[1][1]) # PWM-R
先の文字列をつないで送信する文字列にしています。
msg=s_msc+l_pwm+r_pwm
F337L9R9なら前進、サーボ時間337msec、PWM(9,9)の意味になります。

受信側(MOVE mini)

def SEP(dat):の部分
受信側で使うPWM値(d_pwm)、動作時間(t_msc)を返します。
例のように「F337L9R9」の形で送信されて来るので、’L’、’R’の位置を特定してデータを抜き出しています。
G375L10R10のデータなら((10, 10), 375)で返ります。

大幅に変更した部分
m_dat=SEP(r_msg)でサーボで使う値を得ます。
G375L10R10のデータなら ((10, 10), 375)が返るので、m_dat[0]がPWM値、m_dat[1]が動作時間になります。各値を M_FWD、M_BWDで使います。
r_msg[0]で前後の頭文字’F’、’G’を得ます。前進、後進の場合分けに使っています。

サーボモータの制御の行数がかなり簡素になったように思います。

実行結果

Thonnyのshellに表示された結果です。

送信側(コントローラー)


>>> %Run radio_send_13bb.py
START Push A
START Push A
GO                                      # ボタンAでSTART
recived                                 # 返信受
1 (384, -256, -944) DT= (200, (8, 7))   # accデータ ON時間,PWM(L,R)
1 send-msg= F200 L8 R7                  # 送信データ
recived                                 # 返信受
2 (416, -368, -832) DT= (287, (9, 8))
2 send-msg= F287 L9 R8
recived
省略
7 (720, -576, -352) DT= (450, (11, 8))
7 send-msg= F450 L11 R8
recived
省略
19 (48, 480, -912) DT= (375, (10, 10))
19 send-msg= G375 L10 R10
recived
省略
35 btn= A BA                # ボタンA
recived
36 btn= B BB                # ボタンB
recived
37 btn= C BC                # ボタンA+B
recived
Break-END                   # 停止
>>> 

受信側(MOVE mini)


>>> %Run move_radio_recieve_13-2bb.py
START
Ready A None
Ready A None
Ready A BA                      # ボタンAでSTART
1 F200L8R7 F ((8, 7), 200)      # 受信データ 加工データ
前                              # 前進
2 F287L9R8 F ((9, 8), 287)
前
省略
7 F450L11R8 F ((11, 8), 450)
前
省略
19 G375L10R10 G ((10, 10), 375)
後                              # 後進
省略
button
buttonA
button
buttonB
button
buttonA+B END                   # 停止
>>>

※メモリーのエラーが発生する場合、ThonnyのメニューからDevice>soft reboot後に実行すると解消できることがあります。

実動作の動画

電池駆動で独立させて動かしてみます。
送信側(コントローラー)、受信側(MOVE mini)のスクリプトを各々main.pyとしてuploadします。micro:bitにuploadするにはThonnyのメニューのDevice>Upload current script as main scriptを選択します。

まあまあ動いていると思いますが、なかなか上手く操作できません。車輪が滑ることも影響しているかも知れません。特に回転時の車輪の回転が早いと滑ります。慣れればもっとスムースに動かせそうな気がします。

まとめ

micro:bitのRadio通信機能を使ってMOVE miniのリモート動作が出来ました。送信側のmicro:bitの傾斜で左右の車輪の回転数を変えて曲がることが出来ました。もっといい方法があるかも知れません。