マイクロビット（e_44）2WDカー
スマートカー?
以前に2WDロボットカーの本体（シャーシ）を購入していたので組み立てて動かしてみようと思います。
「ロボットカー」と言うようなレベルではありません。
フォトインタラプタモジュール(GR-YM-227)の記事を具現化したような感じです。距離センサなども搭載してロボットカー的にしたいと思ったのですが、micro:bit V1だとMemorryエラーになってしまうので無線のコントロールだけにしました。
外観
2WDカーのシャーシ組み立てた外観です。いろんなセンサーなどを取付られるように板に穴が沢山あります。
DCモータの速度制御用にフォトインタラプタを取り付けました。両面テープで固定しました。
付属していたエンコーダをモータ軸に取り付けています。フォトインタラプタのLEDが外側になるように取り付けています。
接続例
DCモータの制御用に、micro:bitのpin6、pin7、pin8、pin9出力をレベルコンバータを介してモータードライバー(TA7291P)の制御ピンIN1、IN2へそれぞれ接続します。
フォトインタラプタ（GR-YM-227）の出力をmicro:bitのpin1、pin10に入力します。
pin6、pin7、pin9、pin10はmicro:bitのLED Displayに使われているので、使用の際にはdisplay.off()が必要になります。
赤のLEDは無線動作の確認用です。
※TA7291Pの説明は記事e_41を参照してください。他の詳細はデータシート等を参照ください。
スクリプト
インタラプタセンサーモジュール(GR-YM-227)の記事e_42とMOVE miniをRadio通信の記事e_39を合わせたような感じになってします。詳細はそれぞれの記事を参照ください。
※thonny-microbitのMicroPythonを使っています。
送信側（コントローラー側）
コントローラー側のmicro:bitの前後の傾きから前後の向きと速度を設定し、左右の傾きから左右のモータ速度を調整し、受信側で使うTGTデータ（目標period値）を送信しています。
無線に関しては、MOVEminiの時はややこしい方法で文字列を送受信していましたが、別の方法に気付きました。
送信する文字列は’F,15,15’のようにコンマ区切りにし、受信側でsplitでリストデータにしました。
この方が簡単です。
’F,15,15’の場合はF:前進、15,15:左右モータの目標period値です。他はB:後進 S:停止にしています。
scar_send_tes_02b.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 から TGTデータ（目標period値）を作成
def M_TGT(x_acc,y_acc):
# 10ms-80ms/(800-128) で目標値を計算するが、15msで制限、~128は無視
# TGT target period speed msec
tgt_msec=int((abs(y_acc)-128)/(800-128)*(10-80)+80)
if tgt_msec < 15:
tgt_msec=15 # tgt_msec mimimal(max speed)
# 左右の傾き率分を加算して遅くする
# 0%-100%/(0-1024)
corr=abs(x_acc)/1024*2.5
if corr <= 0.2: # 20% は不感帯に corr=0 corr=corr+1 # 遅くする> periodを長くする>曲がる
tgt_corr=int(tgt_msec*corr) # 倍率したTGT(period)
if tgt_corr>=80: # 制限 80msec (mimimal speed) 無くてもよい
tgt_corr=80
#print("tgt",tgt_msec,corr,tgt_corr)
# x:R(+)L(-) Y:B(+)F(-) Z:DN(+)UP(-)
# 右傾斜なら右曲進、右periodを増やす=>遅くなる
if x_acc>=0:
# Turn R
# 補正後のPWM(L,R)
d_tgt=(tgt_msec, tgt_corr )
# 左傾斜
else:
# Turn L
d_tgt=(tgt_corr, tgt_msec )
# カウント、元PWM値、補正値、補正後のPWM(L,R)
#print(cnt,'pwm,corr,s_pwm',d_pwm,corr,s_pwm)
return d_tgt # TGT period ms (R,L)
radio.on()
cnt=0
while True:
# A+B button でブレーク
if button_a.is_pressed() & button_b.is_pressed():
cnt=cnt+1
print('A+B Pushed STOP')
radio.send('S')
radio.off()
break
# x:R(+)L(-) Y:B(+)F(-) Z:DN(+)UP(-)
r_acc=accelerometer.get_values() # accデータ値を取得
#print(cnt,r_acc[0],r_acc[1])
# y-axis FWD/BWD
if r_acc[1]>0:
drc='B' # 後進文字'B'
else:
drc='F' # 前進文字'F'
# 前後傾きでモーター回転（前後）0~255 STOP
if abs(r_acc[1])>=255:
cnt=cnt+1
r_tgt = M_TGT(r_acc[0],r_acc[1]) # 左右の傾きを加味したtgt計算
#print(cnt,r_acc[0],r_acc[1],r_tgt)
# 送信する文字列 ここで左右を入れ替えしてる（受信側に合わせた）
msg=drc +','+str(r_tgt[1])+','+str(r_tgt[0])
radio.send(msg)
print(cnt, msg)
#sleep(100) # 少しある方が良いかも？
RS() # 返信待
else:
print('y_acc <255 Pass STOP')
#pass
msg='S' +','+'500'+','+'500'
radio.send(msg)
print(cnt, msg)
RS() # 返信待
#sleep(50)
実行例
Thoonyのshellに表示されます
>>> %Run scar_send_tes_02b.py
1 B,77,77 # 後進、period(77,77)
2 B,77,77
y_acc <255 Pass STOP # 傾きが小さいので
2 S,500,500 # STOP 500,500はダミー値
3 F,15,15 # 前進、period(15,15)
4 F,72,72
y_acc <255 Pass STOP
4 S,500,500
y_acc <255 Pass STOP
・・
・・
y_acc <255 Pass STOP
189 S,500,500
190 B,80,80
A+B Pushed STOP # ブレーク停止
受信側（2WDカー側）
’F,15,15’のようにコンマ区切り文字を受信するのでsplit(‘,’)を使って[F,15,15]のリストデータにしました。
エンコーダで測定した現在のperiod値と送信されてきた目標のperiod値を比較、差分からモータ駆動するpwmを計算しています。
他の処理動作も加えたかったのですが、さらに1~2行追加するとMemorryエラーになってしまうので諦めました。
scar_05_fback_radio_02b.py
from microbit import *
import machine
import radio
# TA7291P TP=[IN1, IN2]
R=[pin6,pin7] #[TP7291P IN1,IN2]
L=[pin9,pin8] # モータの極性を入れ替えておく
EN=[pin1,pin10] # エンコーダ出力 input pin
# pin6,7,9,10を使うのでdisplayをオフ
display.off()
# 応答を返信
def RS_msg():
s_msg='recived'
radio.send(s_msg)
# FWD(pwm value, TP=(R/L)) R=[pin6,pin7] L=[pin9,pin8]
def FWD(pwm,TP):
TP[0].write_analog(0) # analogモード、出力は’0’V
TP[1].set_analog_period(20) # 周期を設定 20msec
TP[1].write_analog(pwm) # 0 ~ 1023 128~
sleep(5) # TP7921P 切替100us~
# BWD IN1(L/0)IN2(H/PWM)
def BWD(pwm,TP):
TP[1].write_analog(0)
TP[0].set_analog_period(20)
TP[0].write_analog(pwm)
sleep(5)
def STP(TP):
TP[0].write_digital(0) # IN1
TP[1].write_digital(0) # IN1
sleep(5)
# 現在のエンコーダ値(period)から 変更するdat(pwm値)を返す
def period(TGT,DCM): #DCM R(0)L(1)
global dat, c_ng, EN
add=4 # 初期の加算値
min_pls=192 # モータがギリ動くpwm-value
# 加減の倍率を周期20msecで分ける
if TGT/1000 <= 22:
mag=5 # 4 ~ 10 調整
else:
mag=1/2 # 1/5 ~ 1.0
# エンコーダ(period)測定 time_pulse_us
d = machine.time_pulse_us(EN[DCM],1) # Ton時間測定
# 目標値とのmsec差分
dif=int((TGT - d)/1000) # usec/1000 -> msecに
# 差分に対して加減するPWM値
add=int( abs(dif) * mag) # mag 減算の倍率
if ((d==-2 ) or (d==-1)) and c_ng[0]<=3: # 回転してない PWM値を増やす
dat[DCM]=min_pls+128*c_ng[DCM] # エラー回数でPWM値を増やす
c_ng[DCM]=c_ng[DCM]+1
# 回数、コメント、period(usec)、PWM値、加減値、差分 ----①
print(cnt, DCM, c_ng, 'error', d, dat, dif, add)
return dat
if abs(dif) < 1: # 2msec未満なら無視
# 回数、コメント、period(usec)、PWM値、加減値、差分
print(cnt, DCM, c_ng, 'cont', d, dat, dif, add) ----②
return dat
# 測定値の方が小(早い)ので遅くする・pwm値を下げる・減算する
if dif > 0 :
dat[DCM]=dat[DCM]-add
if dat[DCM] < min_pls: # 減算後、下限値以下なら下限値にする
dat[DCM] = min_pls # ギリ動く
c_ng=[0,0]
print(cnt, DCM, c_ng, 'dif<0', d, dat, dif, add) ----③
# 測定値の方が遅いので早くする・pwm値を上げる・加算する
else:
dat[DCM]=dat[DCM]+add
if dat[DCM] > 1023: # 加算後がPWM値1023超えなら上限1023にする
dat[DCM]=1023
return dat
print(cnt, DCM, c_ng, 'dif<0',d, dat, dif, add) ----④
return dat
# period 測定からpwm値を算出してdat[R,L]に入れる
def get_encord(TGT):
# period(tgt_ms,dcm_num)
dat[0]=period(TGT[0]*1000, 0)[0] # TGT(ms)を(us)に
dat[1]=period(TGT[1]*1000, 1)[1]
return dat
## MAIN ---------------------------------------------------
# time_pulse_usを使うpinをPULL設定して予約する
EN[0].set_pull(EN[0].PULL_DOWN) #EN[0]:R
EN[1].set_pull(EN[1].PULL_DOWN) #EN[1]:L
c_ng=[0,0] # 回転エラー回数 time_pulse_usのエラー
dat=[0,0] # PWM値[R, L]
TGT=[0,0] # 目標period値 [R, L] 15~70msec
radio.on() # 無線ON
#STP(R) #（①～④printをコメントアウトして有効化）
#STP(L) #（①～④printをコメントアウトして有効化）
while True:
#pin0.write_digital(1) # 繰返し動作開始LEDをON（①～④printをコメントアウトして有効化）
r_msg = radio.receive() # 受信
# 受信データがあれば以下を処理
if r_msg:
cnt=cnt+1
#pin0.write_digital(0) # 受信あればLEDをOFF（①～④printをコメントアウトして有効化）
m_msg=r_msg.split(',') # r_msg splitを使うと簡単
if m_msg[0]=='S': # STOP
STP(R)
STP(L)
# 返信
RS_msg()
continue
# TGT(period) >> dat(pwm)
TGT=[int(m_msg[1]),int(m_msg[2])] #TGT[R,L]period
dat=get_encord(TGT)
# 前進
if m_msg[0]=='F':
FWD(dat[0],R)
FWD(dat[1],L)
# 後進
if m_msg[0]=='B':
BWD(dat[0],R)
BWD(dat[1],L)
# 返信
RS_msg()
実行例
Thoonyのshellに表示されます。
print(cnt, DCM, c_ng, 'dif<0',d, dat, dif, add)
回数、モータ番号、エラー数、コメント、period(us)、dat[R,L]、差分、加減値
>>> %Run scar_05_fback_radio_02b.py
1 0 [1, 0] error -2 [192, 0] 68 34 # 初動時のエラー（回転してない） pwm=192から開始
1 1 [1, 1] error -1 [192, 192] 68 34
2 0 [2, 1] error -2 [320, 192] 68 34
2 1 [2, 2] error -1 [320, 320] 68 34 # 加算ステップ128 192+128*1=320
3 0 [3, 2] error -2 [448, 320] 68 34
3 1 [3, 3] error -1 [448, 448] 68 34 # 192+128*2=448
4 0 [3, 3] dif<0 53094 [441, 448] 14 7 # 回転
4 1 [3, 3] dif<0 42072 [441, 436] 25 12
5 0 [3, 3] dif<0 21558 [418, 436] 46 23 # 441-23=418
ちなみに、2WDカーに付属していたモーターの特性を(GR-YM-227)の記事と同じように測定しました。
無負荷状態です。
動作確認
送信文字列を’F,15,80’で固定して動作を確認しました。
先のようにThonnyのshellに表示されたデータを表計算でまとめてみました。
個別データは大きくばらついていますが、平均的には15，80msec近くになっているように見えます。それなりに制御できているようです。
また、コントローラを前後、左右に傾けると前後、左右の回転数も変化する事を確認しました。机上ではモータの制御はそれなりに上手く動いているようです。
実動結果
タイヤも付けて実際に動かしてみました。
micro:bitやTA2917Pなどのブレッドボードを無理矢理載せた状態です。フォトインタラプタはmicro:bitの下にあり隠れています。電源はmicro:bit用とモーター用の2つが載ってます。
送受信それぞれスクリプトをmain.pyとしてアップロードして実動作を確認しました。受信側のprintはコメントアウトしてます。
三輪のコマが動かない、滑るようで動作がパッとしません。そのため、コマを固定して動作させた動画です。
どこかが上手く動いていないような気もするのですが・・とりあえず動いてます。
もっと広いところなら、もっと早く、左右に曲がったり、回転など上手く動かせたのではないかと思います。
まとめ
micro:bitの無線機能を使って2WDカーを動作させました。前後、左右、速度調整の動作が出来たと思います。
