4脚ロボットの機能面の強化を図っています。
今回はブラウザから指示してロボを操作する機能を実装してみました。
ブラウザ入力とサーボ制御の並列化で結構はまりましたが、うまく動かすところまでいけました。
やったこと
並列処理をするということで、今回はThreadを使って物理的にコアを２つ使っています。
・Core0でブラウザ
・Core1でサーボ制御
です。
Threadを使うことでこんな簡単に並列化できるのか！という安易な考えで始めたのですが、
何故か途中でブラウザがフリーズするという現象が起きてました。
Raspberry pi picoの並列化は下記がとてもわかりやすいです。ありがとうございます。
ラズパイPicoマルチコアで並列動作させる方法：MicroPython編
Raspberry Pi Picoのデュアルコアを使用して2つの処理を同時に並列処理させる使い方について詳しく紹介。一定間隔で実行させたいプログラムが簡単に実現できます。
logikara.blog
ブラウザフリーズの解決手段
ブラウザ入力でロボットのStateを出力して、サーボ処理でStateをもとに前進やら右折やらをするというシンプルな動作にしていたのですが、変数のLockなどを入れてもフリーズしていました。
デバッグ過程で、サーボ側の一部の処理をコメントアウトすると、フリーズしなかったため、Lockは関係なさそうでRP2040の作りやらMicroPythonの問題？ということで調べてみると、下記を見つけました。
Leveraging second core, code hangs - MicroPython Forum (Archive)
forum.micropython.org
どうも同じようなバグ？なのか、フリーズ現象が出ていたようで、一部のリプライにガーベージコレクションの話がありました。
ブラウザ側のループはリアルタイム性能は要求されないので、試しにガーベージコレクションを入れ込んだらフリーズがなくなりました。
メモリのアロケーションとかを事前にやる必要があるのかどうかよくわかってないので引き続き調べようと思います。
プログラム
import network # ネットワーク処理用ライブラリ
import socket # ソケット処理用ライブラリ
from time import sleep # スリープ処理用ライブラリ
from picozero import pico_temp_sensor, pico_led # GPIO制御用ライブラリ
import machine
from machine import Pin, PWM
import utime
import time
import _thread
import gc
gc.enable()
lock = _thread.allocate_lock()
# sharing parameters
params = [0]
#####Network setting#####
# ネットワーク（Wi-Fiルーター）の SSID とパスワードを設定
ssid = 'SSID'
password = 'PASSWORD'
# Wi-Fi 接続実行関数
def connect():
wlan = network.WLAN(network.STA_IF) # WLANオブジェクトを作成
wlan.active(True) # WLANインタフェースを有効化
wlan.connect(ssid, password) # 指定されたSSIDとパスワードでWi-Fiに接続する
while wlan.isconnected() == False: # Wi-Fi接続が確立されるまで待機
print('Waiting for connection...')
sleep(1)
print(wlan.ifconfig()) # Wi-Fi 接続情報を全て出力（IPアドレス、サブネットマスク、 ゲートウェイ、DNS）
ip = wlan.ifconfig()[0] # IPアドレスのみを取得
print(f'Connected IP: {ip}') # IPアドレスを出力
return ip # IPアドレスを返す
# ソケットを開く関数
def open_socket(ip):
address = (ip, 80) # IPアドレスとポート番号のタプルを作成
connection = socket.socket() # ソケットオブジェクトを作成
connection.bind(address) # IPアドレスとポート番号をバインド
connection.listen(1) # 接続待機
print(connection) # ソケットオブジェクトを出力
return connection # ソケットオブジェクトを返す
# Webページを準備
def webpage():
html = '''
<!DOCTYPE html>
<html>
<head>
<meta charset="UTF-8">
<title>Robot Controller</title>
<style>
#container {display: flex;justify-content: center;align-items: center;flex-direction: column;margin-top: 50px;}
h1 {color: #666;}
.button {width: 100px;height: 50px;border-radius: 5px;margin: 10px;cursor: pointer;font-size: 30px;font-weight: bold;color: white;text-align: center;line-height: 50px;}
#movef-button {background-color: green;}
#mover-button {background-color: green;}
#movel-button {background-color: green;}
#moveb-button {background-color: green;}
#standalone-button {background-color: red;}
#temp-button {background-color: blue;}
input[type="text"] {width: 200px;padding: 5px;font-size: 20px;text-align: center;margin: 20px;}
</style>
</head>
<body>
<div id="container">
<h1>Robot Controller</h1>
<hr>
<div>
<center>
<button id="movef-button" class="button">↑</button><br>
<button id="mover-button" class="button">←</button>
<button id="movel-button" class="button">→</button><br>
<button id="moveb-button" class="button">↓</button><br><br>
<button id="standalone-button" class="button">SA</button><br><br>
</center>
</div>
<button id="temp-button" class="button">T</button><br>
<input type="text" placeholder="Temperature" />
</div>
<script>
const mfButton = document.getElementById("movef-button");
const mrButton = document.getElementById("mover-button");
const mlButton = document.getElementById("movel-button");
const mbButton = document.getElementById("moveb-button");
const saButton = document.getElementById("standalone-button");
const tpButton = document.getElementById("temp-button");
const textBox = document.querySelector("input[type='text']");
mfButton.addEventListener("click", async () => {
const response = await fetch("/moveforward");
const text = await response.text();
console.log(text);
});
mrButton.addEventListener("click", async () => {
const response = await fetch("/moveleft");
const text = await response.text();
console.log(text);
});
mlButton.addEventListener("click", async () => {
const response = await fetch("/moveright");
const text = await response.text();
console.log(text);
});
mbButton.addEventListener("click", async () => {
const response = await fetch("/moveback");
const text = await response.text();
console.log(text);
});
saButton.addEventListener("click", async () => {
const response = await fetch("/standalone");
const text = await response.text();
console.log(text);
});
tpButton.addEventListener("click", async () => {
const response = await fetch("/get/data");
const text = await response.text();
textBox.value = "本体温度：" + text;
});
</script>
</body>
</html>
'''
return html
# Wi-Fi接続実行
try:
ip = connect() # Wi-Fi接続を実行してIPアドレスを取得
connection = open_socket(ip) # IPアドレスでソケットを開く
html = webpage() # htmlデータ読み込み
except (KeyboardInterrupt, OSError) as e:
machine.reset() # 停止またはOSエラーで本体リセット
#####Network setting#####
servo = []
servo.append(PWM(Pin(2))) #Front Right Leg
servo.append(PWM(Pin(3))) #Front Right Shoulder
servo.append(PWM(Pin(4))) #Front Left Leg
servo.append(PWM(Pin(5))) #Front Left Shoulder
servo.append(PWM(Pin(6))) #Rear Right Leg
servo.append(PWM(Pin(7))) #Rear Right Shoulder
servo.append(PWM(Pin(8))) #Rear Left Leg
servo.append(PWM(Pin(9))) #Rear Left Shoulder
def core0(params, servo):
# サーバー応答処理
state = 0
i = 0
while True:
#Network action#
try:
print("request ",str(i))
i += 1
client = connection.accept()[0] # クライアントからの接続要求を受け入れる
request = client.recv(1024) # クライアントからのHTTPリクエストを受信する
request = str(request) # バイト文字列を文字列に変換する
try: # リクエストが存在しない場合に発生する可能性のあるIndexErrorをキャッチし、例外を無視する
request = request.split()[1] # 受信したデータからURLを抽出
except IndexError:
pass
# ヘッダー情報を返す
client.send('HTTP/1.0 200 OKrnContent-type: text/htmlrnrn')
# ルートアクセスでhtmlデータを返す
if request == '/': # 抽出したURLが「/」なら
client.send(html) # Webページを返す
print('Root Access')
# 前進
elif request == '/moveforward':
state = 1
client.send('move forward!')
print('move forward!')
# 左折
elif request == '/moveleft':
state = 2
client.send('move left!')
print('move left!')
# 右折
elif request == '/moveright':
state = 3
client.send('move right!')
print('move right!')
# 後進
elif request == '/moveback':
state = 4
client.send('move back!')
print('move back!')
# stand alone mode
elif request == '/standalone':
state = 5
client.send('Stand alone!')
print('Stand alone!')
# 本体温度を取得
elif request == '/get/data':
temperature = pico_temp_sensor.temp # 温度センサーから温度データを取得する
temperature = "{:.1f}".format(temperature) # 温度データを小数点1桁の文字列に変換
client.send(temperature) # 温度データを返す
print('Temp:', temperature)
with lock:
params[0] = state
client.close() # クライアント接続を閉じる
gc.collect()
except KeyboardInterrupt:
machine.reset() # 停止（Ctrl-C）が押されたときにPicoを再起動する
except OSError as e:
client.close() # OSエラーで接続を閉じる
print('connection closed')
#Network action#
def core1(params, servo):
####Servo initialize####
SV_FREQ = 50.0 #Hz
MAX_DUTY = 65535.0 #uint16 max
MIN_SV_PULSE = 0.5 #ms
MAX_SV_PULSE = 2.5 #ms
trigger = Pin(16, Pin.OUT)
echo = Pin(17, Pin.IN)
def get_pulse_width(angle):
pulse_ms = MIN_SV_PULSE + (MAX_SV_PULSE - MIN_SV_PULSE)*angle/180.0
x = (int)(MAX_DUTY * (pulse_ms*SV_FREQ/1000.0))
return x
def read_distance():
trigger.low()
utime.sleep_us(2)
trigger.high()
utime.sleep(0.00001)
trigger.low()
while echo.value() == 0:
signaloff = utime.ticks_us()
while echo.value() == 1:
signalon = utime.ticks_us()
timepassed = signalon - signaloff
distance = (timepassed * 0.0343) / 2
return distance
#print("dinstance: ",distance,"cm")
correction = [0, 0, 10, 0, 0, 0, -10, -5]
temp_angle = [90, 90, 90, 90, 90, 90, 90, 90]
angle_i = [ 90, 90, 90, 90, 90, 90, 90, 90]# Initial position
angle_s = [[ 90, 90, 90, 90, 90, 90, 90, 90],
[ 90, 90, 90, 90, 90, 90, 90, 90]]
angle_f = [ [ 60, 90, 90, 90, 90, 90, 60, 90],
[ 60, 60, 90, 60, 90, 120, 60, 120],
[ 90, 60, 120, 60, 120, 120, 90, 120],
[ 90, 90, 120, 90, 120, 90, 90, 90],
[ 90, 120, 120, 120, 120, 60, 90, 60],
[ 60, 120, 90, 120, 90, 60, 60, 60] ]
angle_r = [ [ 60, 90, 90, 90, 90, 90, 60, 90],
[ 60, 150, 90, 90, 90, 90, 60, 150],
[ 90, 150, 90, 90, 90, 90, 90, 150],
[ 90, 150, 120, 90, 120, 90, 90, 150],
[ 90, 150, 120, 150, 120, 150, 90, 150],
[ 90, 150, 90, 150, 90, 150, 90, 150],
[ 90, 90, 90, 90, 90, 90, 90, 90] ]
snum = len(servo)
divide = 5 # Number of divisions between frames
div_counter = 0 # Counting divisions
key_frame = 0 # Current keyframe
next_key_frame = 1 # Next frame
# Move to initial position
for i in range(snum):
servo[i].freq(int(SV_FREQ))
servo[i].duty_u16(get_pulse_width(angle_i[i] + correction[i]))
time.sleep(1.0)
d = list(0 for i in range(0, 5))
angle = angle_s
fnum = 1
j = 0
####Servo initialize####
state = 0
while True:
with lock:
state = params[0]
#print(state)
#time.sleep(1)
##Servo control##
#for j in range(len(d)):
# d[j] = read_distance()
#dave = sum(d)/len(d)
#print(dave)
#if (dave >= 20.0) & (next_key_frame == 1):
if (state == 1) & (next_key_frame == 1):
angle = angle_f
fnum = 5
#elif (dave < 20.0) & (next_key_frame == 1):
elif (state == 3) & (next_key_frame == 1):
angle = angle_r
fnum = 6
#Update keyframe
div_counter += 1
if div_counter >= divide:
div_counter = 0
key_frame = next_key_frame
next_key_frame += 1
if next_key_frame > fnum:
next_key_frame = 0 # Back to angle[0]
#Degree calculation
for i in range(snum):
temp_angle[i] = angle[key_frame][i] + (angle[next_key_frame][i] - angle[key_frame][i])*div_counter/divide
#print(key_frame)
# Servo drive
for i in range(snum):
with lock:
servo[i].duty_u16(get_pulse_width(int(temp_angle[i])+correction[i]))
time.sleep(0.03)
_thread.start_new_thread(core1, (params,servo))
core0(params, servo)ローカルネットワーク上にあるPCなりスマホからロボットに割り当てられたIPアドレスにアクセスすると下記のような表示になります。
まだ前進と右折しか実装してませんが、ゆくゆくは全部実装するつもりです。
コメント