M5Stackで2足歩行ロボット”スタックチャン”の作成④プログラムの書き込み、完成（仮）

前回、足のパーツまで作成いたしました。↓

M5Stackで2足歩行ロボット"スタックチャン"の作成③足のCADスケッチ、印...

https://denkenmusic.com/m5stackで2足歩行ロボットスタックチャンの作成③足

　前回、ボディ部を作成しました。↓　今回は足の作成に入りたいと思います。足は、本体の回転を伝える「支え」の部分と、本体を傾けさせる「足」の部分に分けてパーツを印刷します。ここ、結構躓きまして（足だけに。。）、かなりの試行錯誤を繰り返しました(-_-;)その辺の詳細はまた後日まとめるとして、今回は足のパーツ印刷、そして本体組立てまでをまとめたいと思います。　　足のCADスケッチ支え部分本体と足のパーツを接続させる支えの部品になります。上側に本体のサーボを繋いで、下側に足のサーボを繋ぐように設計しています...

今回はプログラムの書き込み、歩行テスト、仮完成までをまとめようかと思います。

実は歩行テストの際に中々うまく行かない状況にハマりまして、何度も何度も試行錯誤したのですが、その辺りは要点を絞って記しておこうと思います。

それでは先ずプログラムから。

1 M5Stack core2（コントローラー側）
- 1.1 micropython
2 M5Stack側（本体側）
- 2.1 micropython
3 余談
- 3.1 ハマったところ
4 閑話休題
- 4.1 紆余曲折あって完成（仮）！

M5Stack core2（コントローラー側）

micropython

from m5stack_ui import *
from socket import socket, AF_INET, SOCK_DGRAM
import time
import network
from m5stack import *
#from m5stack import lcd
from machine import Pin
from uiflow import *
 
screen = M5Screen()
screen.clean_screen()
screen.set_screen_bg_color(0xFFFFFF)
 
dstip = "m5stackのIP"
dstport = ポート番号
 
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
if not wlan.isconnected():
    print('connecting to network...')
    wlan.connect('SSID', 'PASS')
    while not wlan.isconnected():
        pass
    print('network config:', wlan.ifconfig())
print('network config:', wlan.ifconfig())
s = socket(AF_INET, SOCK_DGRAM)
#s.bind((dstip, dstport))
 
touch_button0 = M5Btn(text='Button', x=110, y=10, w=100, h=80, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)
touch_button1 = M5Btn(text='Button', x=10, y=80, w=100, h=80, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)
touch_button2 = M5Btn(text='Button', x=210, y=80, w=100, h=80, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)
touch_button3 = M5Btn(text='Button', x=110, y=150, w=100, h=80, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)
touch_button4 = M5Btn(text='Button', x=116, y=100, w=40, h=40, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)
touch_button5 = M5Btn(text='Button', x=165, y=100, w=40, h=40, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)
 
 
def touch_button0_pressed():
    global data
    data = "F"
    
def touch_button3_pressed():
    global data
    data = "B"
    
def touch_button1_pressed():
    global data
    data = "L"
    
def touch_button2_pressed():
    global data
    data = "R"
    
 
#touch_button0.pressed(touch_button0_pressed)
# ボタン"A" が「離された」時の処理
def touch_button0_released():
    global data
    data = "S"
    
def touch_button4_pressed():
    global data
    data = "on"
    
def touch_button5_pressed():
    global data
    data = "off"
    
#data = touch_button0.released(touch_button0_released)
data = "nane"
stamp = 0
 
while(True):
    #global data
    touch_button0.pressed(touch_button0_pressed)
    touch_button1.pressed(touch_button1_pressed)
    touch_button2.pressed(touch_button2_pressed)
    touch_button3.pressed(touch_button3_pressed)
    touch_button4.pressed(touch_button4_pressed)
    touch_button5.pressed(touch_button5_pressed)
        #data = "A"
    touch_button0.released(touch_button0_released)
    touch_button1.released(touch_button0_released)
    touch_button2.released(touch_button0_released)
    touch_button3.released(touch_button0_released)
    
    s.sendto(data, (dstip, dstport))
    print(data)
    #print(Slider2.get_value())
    time.sleep(0.1)

from m5stack_ui import *

from socket import socket, AF_INET, SOCK_DGRAM

import time

import network

from m5stack import *

#from m5stack import lcd

from machine import Pin

from uiflow import *

screen = M5Screen()

screen.clean_screen()

screen.set_screen_bg_color(0xFFFFFF)

dstip = "m5stackのIP"

dstport = ポート番号

wlan = network.WLAN(network.STA_IF)

wlan.active(True)

if not wlan.isconnected():

print('connecting to network...')

wlan.connect('SSID', 'PASS')

while not wlan.isconnected():

pass

print('network config:', wlan.ifconfig())

s = socket(AF_INET, SOCK_DGRAM)

#s.bind((dstip, dstport))

touch_button0 = M5Btn(text='Button', x=110, y=10, w=100, h=80, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)

touch_button1 = M5Btn(text='Button', x=10, y=80, w=100, h=80, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)

touch_button2 = M5Btn(text='Button', x=210, y=80, w=100, h=80, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)

touch_button3 = M5Btn(text='Button', x=110, y=150, w=100, h=80, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)

touch_button4 = M5Btn(text='Button', x=116, y=100, w=40, h=40, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)

touch_button5 = M5Btn(text='Button', x=165, y=100, w=40, h=40, bg_c=0xFFFFFF, text_c=0x000000, font=FONT_MONT_14, parent=None)

def touch_button0_pressed():

global data

data = "F"

def touch_button3_pressed():

global data

data = "B"

def touch_button1_pressed():

global data

data = "L"

def touch_button2_pressed():

global data

data = "R"

#touch_button0.pressed(touch_button0_pressed)

# ボタン"A" が「離された」時の処理

def touch_button0_released():

global data

data = "S"

def touch_button4_pressed():

global data

data = "on"

def touch_button5_pressed():

global data

data = "off"

#data = touch_button0.released(touch_button0_released)

data = "nane"

stamp = 0

while(True):

#global data

touch_button0.pressed(touch_button0_pressed)

touch_button1.pressed(touch_button1_pressed)

touch_button2.pressed(touch_button2_pressed)

touch_button3.pressed(touch_button3_pressed)

touch_button4.pressed(touch_button4_pressed)

touch_button5.pressed(touch_button5_pressed)

#data = "A"

touch_button0.released(touch_button0_released)

touch_button1.released(touch_button0_released)

touch_button2.released(touch_button0_released)

touch_button3.released(touch_button0_released)

s.sendto(data, (dstip, dstport))

print(data)

#print(Slider2.get_value())

time.sleep(0.1)

操作ボタンを配置し、ボタン押下でボタンに紐づいた文字列をUDPでM5Stack側へ送信しています。

２つ余分にボタンが配置されていますが、これは追々使用する予定です。

M5Stack側（本体側）

micropython

from socket import socket, AF_INET, SOCK_DGRAM
import time
import network
from machine import Pin, I2C
import pca9685
import servo
import _thread
from m5stack import *
from m5stack import lcd
#from machine import Pin
 
posi = {"lx":90, "ly":90, "lr":10, "rx":230, "ry":90, "rr":10, "ux":110, "uy":170}
lcd.clear(lcd.BLACK)
 
lcd.circle(posi["lx"], posi["ly"], posi["lr"], lcd.WHITE, lcd.WHITE)
lcd.circle(posi["rx"], posi["ry"], posi["rr"], lcd.WHITE, lcd.WHITE)
lcd.rect(posi["ux"], posi["uy"], 100, 5, lcd.WHITE, lcd.WHITE)
 
a = 92
b = 83
c = 83
d = 95
 
 
i2c = I2C(0, scl=Pin(22), sda=Pin(21))
sev = servo.Servos(i2c)
 
#p0 = Pin(19, Pin.OUT)
#p1 = Pin(23, Pin.OUT)
 
HOST = "IP"
PORT = ポート番号
 
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
if not wlan.isconnected():
    print('connecting to network...')
    wlan.connect('Buffalo-G-29CE', 'AKANE0918')
    while not wlan.isconnected():
        pass
    print('network config:', wlan.ifconfig())
 
s = socket(AF_INET, SOCK_DGRAM)
 
s.bind((HOST, PORT))
print("bind")
 
msg = "nane"
 
def recieve():
    while(True):
        global msg
        msg = s.recv(64)    
        msg = msg.decode()
        print(msg)
        time.sleep(0.01)
 
_thread.start_new_thread(recieve, ())

while(True):
    if msg =="F":
        while(b <= 125):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b + 1
            if d <= 125:
                d = d + 1
        while(c >= 75):
            sev.position(2, c)
            time.sleep(0.009)
            c = c - 1
        while(b >= 95):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b - 1
            if d >= 95:
                d = d - 1
        while(c <= 95):
            sev.position(2, c)
            time.sleep(0.009)
            c = c + 1
        while(d >= 65):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            d = d - 1
            if b >= 65:
                b = b - 1
        while(a <= 110):
            sev.position(0, a)
            time.sleep(0.009)
            a = a + 1
        while(b <= 95):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b + 1
            if d <= 95:
                d = d + 1
        while(a >= 90):
            sev.position(0, a)
            time.sleep(0.009)
            a = a - 1
    elif msg == "B":
        while(b <= 125):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b + 1
            if d <= 125:
                d = d + 1
        while(c <= 115):
            sev.position(2, c)
            time.sleep(0.009)
            c = c + 1
        while(b >= 95):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b - 1
            if d >= 95:
                d = d - 1
        while(c >= 95):
            sev.position(2, c)
            time.sleep(0.009)
            c = c - 1          
        while(d >= 65):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            d = d - 1
            if b >= 65:
                b = b - 1
        while(a >= 70):
            sev.position(0, a)
            time.sleep(0.009)
            a = a - 1
        while(b <= 95):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b + 1
            if d <= 95:
                d = d + 1
        while(a <= 90):
            sev.position(0, a)
            time.sleep(0.009)
            a = a + 1
    elif msg == "L":
        while(d >= 65):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            d = d - 1
            if b >= 65:
                b = b - 1
        while(a >= 70):
            sev.position(0, a)
            time.sleep(0.009)
            a = a - 1
        while(b <= 95):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b + 1
            if d <= 95:
                d = d + 1
        while(a <= 90):
            sev.position(0, a)
            time.sleep(0.009)
            a = a + 1
    elif msg == "R":
        while(b <= 125):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b + 1
            if d <= 125:
                d = d + 1
        while(c <= 115):
            sev.position(2, c)
            time.sleep(0.009)
            c = c + 1
        while(b >= 95):
            sev.position(1, b)
            sev.position(3, d)
            time.sleep(0.009)
            b = b - 1
            if d >= 95:
                d = d - 1
        while(c >= 95):
            sev.position(2, c)
            time.sleep(0.009)
            c = c - 1
    time.sleep(0.01)

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

from socket import socket, AF_INET, SOCK_DGRAM

import time

import network

from machine import Pin, I2C

import pca9685

import servo

import _thread

from m5stack import *

from m5stack import lcd

#from machine import Pin

posi = {"lx":90, "ly":90, "lr":10, "rx":230, "ry":90, "rr":10, "ux":110, "uy":170}

lcd.clear(lcd.BLACK)

lcd.circle(posi["lx"], posi["ly"], posi["lr"], lcd.WHITE, lcd.WHITE)

lcd.circle(posi["rx"], posi["ry"], posi["rr"], lcd.WHITE, lcd.WHITE)

lcd.rect(posi["ux"], posi["uy"], 100, 5, lcd.WHITE, lcd.WHITE)

a = 92

b = 83

c = 83

d = 95

i2c = I2C(0, scl=Pin(22), sda=Pin(21))

sev = servo.Servos(i2c)

#p0 = Pin(19, Pin.OUT)

#p1 = Pin(23, Pin.OUT)

HOST = "IP"

PORT = ポート番号

wlan = network.WLAN(network.STA_IF)

wlan.active(True)

if not wlan.isconnected():

print('connecting to network...')

wlan.connect('Buffalo-G-29CE', 'AKANE0918')

while not wlan.isconnected():

pass

print('network config:', wlan.ifconfig())

s = socket(AF_INET, SOCK_DGRAM)

s.bind((HOST, PORT))

print("bind")

msg = "nane"

def recieve():

while(True):

global msg

msg = s.recv(64)

msg = msg.decode()

print(msg)

time.sleep(0.01)

_thread.start_new_thread(recieve, ())

while(True):

if msg =="F":

while(b <= 125):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b + 1

if d <= 125:

d = d + 1

while(c >= 75):

sev.position(2, c)

time.sleep(0.009)

c = c - 1

while(b >= 95):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b - 1

if d >= 95:

d = d - 1

while(c <= 95):

sev.position(2, c)

time.sleep(0.009)

c = c + 1

while(d >= 65):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

d = d - 1

if b >= 65:

b = b - 1

while(a <= 110):

sev.position(0, a)

time.sleep(0.009)

a = a + 1

while(b <= 95):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b + 1

if d <= 95:

d = d + 1

while(a >= 90):

sev.position(0, a)

time.sleep(0.009)

a = a - 1

elif msg == "B":

while(b <= 125):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b + 1

if d <= 125:

d = d + 1

while(c <= 115):

sev.position(2, c)

time.sleep(0.009)

c = c + 1

while(b >= 95):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b - 1

if d >= 95:

d = d - 1

while(c >= 95):

sev.position(2, c)

time.sleep(0.009)

c = c - 1

while(d >= 65):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

d = d - 1

if b >= 65:

b = b - 1

while(a >= 70):

sev.position(0, a)

time.sleep(0.009)

a = a - 1

while(b <= 95):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b + 1

if d <= 95:

d = d + 1

while(a <= 90):

sev.position(0, a)

time.sleep(0.009)

a = a + 1

elif msg == "L":

while(d >= 65):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

d = d - 1

if b >= 65:

b = b - 1

while(a >= 70):

sev.position(0, a)

time.sleep(0.009)

a = a - 1

while(b <= 95):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b + 1

if d <= 95:

d = d + 1

while(a <= 90):

sev.position(0, a)

time.sleep(0.009)

a = a + 1

elif msg == "R":

while(b <= 125):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b + 1

if d <= 125:

d = d + 1

while(c <= 115):

sev.position(2, c)

time.sleep(0.009)

c = c + 1

while(b >= 95):

sev.position(1, b)

sev.position(3, d)

time.sleep(0.009)

b = b - 1

if d >= 95:

d = d - 1

while(c >= 95):

sev.position(2, c)

time.sleep(0.009)

c = c - 1

time.sleep(0.01)

UDPで文字列を受信するループと、サーボを駆動させるループを並列で処理させています。（まとめて処理させると遅延が発生する為）

ちなみに、pca9685のライブラリを使っているので、M5Stackのフラッシュにpca9685関連のファイルを保存させておく必要があります。ファイルはgitで公開されています。

余談

ハマったところ

https://twitter.com/ketunorobio/status/1543967105061490688

歩行テスト時に、足が全く動かないトラブルに見舞われました。

宙に浮かせると動くのですが、床に置くと動かない。。つまりサーボの動力が足りてないんですよね。

結論から言えば、もともと足のサーボの軸を本体の外側に配置していたので、重心を外れて力が伝わりづらかったんですよね。こんな感じで、だいぶ外れてた(;^ω^)↓

これの軸を中心に持ってくるように修正した所、足に動力が伝わるようになりました。

閑話休題

紆余曲折あって完成（仮）！

https://twitter.com/ketunorobio/status/1545166926451658752

なかなかいいんじゃないでしょうか、私にしては上出来です。

これでほぼ完成なのですが、とりあえずケースが破損していているのと、ツートンカラーがあれなので、次回はケースを再度印刷し直し、ブラックでペイントして、再度作り直しをしてみようと思います。

M5Stack core2（コントローラー側）

micropython

M5Stack側（本体側）

micropython

余談

ハマったところ

閑話休題

紆余曲折あって完成（仮）！

コメントを残す コメントをキャンセル

コメントを残すコメントをキャンセル