マイクロビット(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の傾斜で左右の車輪の回転数を変えて曲がることが出来ました。もっといい方法があるかも知れません。