I’m enhancing the functional capabilities of our four-legged robot.
This time, I’ve implemented a feature that allows the robot to be controlled via browser commands.
Parallelizing browser input and servo control was quite challenging, but we managed to get it working smoothly.
What I did
For parallel processing, I used “Thread” library to physically utilize two cores.
・Core0 for the browser
・Core1 for servo control
Starting with the simple thought that parallelization could be easily achieved with Threads, we encountered an unexpected issue where the browser would freeze mid-operation.
The parallelization on Raspberry Pi Pico is very well explained here(in japanese):
ラズパイPicoマルチコアで並列動作させる方法：MicroPython編
Raspberry Pi Picoのデュアルコアを使用して2つの処理を同時に並列処理させる使い方について詳しく紹介。一定間隔で実行させたいプログラムが簡単に実現できます。
logikara.blog
Solution to the browser freeze
I kept the operation simple, outputting the robot’s state through browser input and making movements like forward or right turns based on the state in servo processing.
Even after implementing variable Locks, the browser would still freeze.
During debugging, when certain parts of the servo code were commented out, the freezing stopped, suggesting that Locks might not be the issue.
It seemed more like a problem with the RP2040 build or MicroPython. Upon further research, I found the following:
Leveraging second core, code hangs - MicroPython Forum (Archive)
forum.micropython.org
It appears to be a similar bug or freezing issue. Some replies mentioned garbage collection.
Since the browser side’s loop does not require real-time performance, I tried incorporating garbage collection, which resolved the freezing issue.
I’m not entirely sure if memory allocation needs to be done in advance, so we plan to continue researching this.
Code
import network # Network library
import socket # Socket library
from time import sleep # Sleep library
from picozero import pico_temp_sensor, pico_led # GPIO library
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#####
# Network(Wifi router) SSID and password setting
ssid = 'SSID'
password = 'PASSWORD'
# Wi-Fi connection
def connect():
wlan = network.WLAN(network.STA_IF) # creating WLAN object
wlan.active(True) # enabling WLAN interface
wlan.connect(ssid, password) # wifi connection with the SSID and password
while wlan.isconnected() == False: # wating Wi-Fi connection
print('Waiting for connection...')
sleep(1)
print(wlan.ifconfig()) # printing Wi-Fi information
ip = wlan.ifconfig()[0] # getting only IP address
print(f'Connected IP: {ip}') # printing IP address
return ip # returning IPaddress
# SOcket open function
def open_socket(ip):
address = (ip, 80) # generating tuple of IP address and port number
connection = socket.socket() # generating socket object
connection.bind(address) # binding IP address and port number
connection.listen(1) # waiting connection
print(connection) # printing socket object
return connection # returning socket object
# Web page for robot control
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 = "Temperature：" + text;
});
</script>
</body>
</html>
'''
return html
# Wi-Fi connection
try:
ip = connect() # getting IP address with WiFi connection
connection = open_socket(ip) # opening socket with IP address
html = webpage() # loading html
except (KeyboardInterrupt, OSError) as e:
machine.reset() # resetting due to error or stop
#####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] # Recieving connection request from client
request = client.recv(1024) # Recieving HTTP request from client
request = str(request) # Converting Byte to Str
try: # Catch the IndexError that may occur if a request does not exist and ignore the exception
request = request.split()[1] # Extract the URL from the received data
except IndexError:
pass
# return header information
client.send('HTTP/1.0 200 OKrnContent-type: text/htmlrnrn')
# return html by root access
if request == '/':
client.send(html) # return Web page
print('Root Access')
# Forwad
elif request == '/moveforward':
state = 1
client.send('move forward!')
print('move forward!')
# Left turn
elif request == '/moveleft':
state = 2
client.send('move left!')
print('move left!')
# Right turn
elif request == '/moveright':
state = 3
client.send('move right!')
print('move right!')
# Back
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!')
# getting robot temperature
elif request == '/get/data':
temperature = pico_temp_sensor.temp # getting temperature from sensor
temperature = "{:.1f}".format(temperature) # convering temperature data to float
client.send(temperature) # return temperature data
print('Temp:', temperature)
with lock:
params[0] = state
client.close() # closing client connection
gc.collect() # doing garbage collection
except KeyboardInterrupt:
machine.reset() # Restart pico when inputing Ctrl-C
except OSError as e:
client.close() # close the connection when an OS error occurs
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)) #Servo control execution on core1
core0(params, servo)
When you access the IP address assigned to the robot from a PC or smartphone on the local network, it will display as follows:
I have only implemented forward and right turn functions so far, but we plan to implement all functions eventually.
コメント