「M5Atomで作る歩行ロボット」という本の犬型ロボットの製作記です。

Wiiのヌンチャクがロボットのコントローラーになります。前回では、Wiiのヌンチャクの信号をM5AtomLITEで取るところまで進めましたが、ここからはその信号をPCに転送することを考えます。

本では、コントローラーとPCの通信にROSを使っているので、PCはUbuntu が必要になります。Ubuntuを新たに用意しなくとも、ROSの環境が入ったUbuntuをDockerでWindowsに構築することもできますが、それでも少し大掛かりな気がします。

そこで、ROSは使わずにシンプルにUDPを使ってみようと思いました。UDPならWindowsのPCでも問題なく使えますし、PythonやUnity、C#でも使えます。またROSでは通信を統括するROSCoreをPCで立ち上げないといけないと思いますが、UDPの構想ならその必要はありません。

では、M5Atom LITE （以下LITE）から、WiFi を使ってUDPで情報をPCに送るところを作っていきます。

PythonでUDPの送受信

まず、PythonでUDPの送受信ができることを確立してから、送信側を、LITEに置き換える手順で進めたいと思います。

まず、UDPを送信するpythonプログラム sender.pyを作りました。1秒ごとに、カウントアップした整数二つ”cnt, cnt+1″ を送ります。

import random
import time
from socket import socket, AF_INET, SOCK_DGRAM

HOST = ''
PORT = 5000
ADDRESS = "127.0.0.1" # 自分に送信

s = socket(AF_INET, SOCK_DGRAM)

cnt = 0
try:
    while True:
        msg = str(f"{cnt}, {cnt+1}")
        print(f"send: {msg}")
        s.sendto(msg.encode(), (ADDRESS, PORT))
        time.sleep(1)
        cnt += 1

except KeyboardInterrupt:
    print("CTRL+Cを検出。終了します。")
    s.close()Code language: Python (python)

このプログラムをプロンプトで実行すると以下のように表示されます。

> python .\sender_udp.py
send: 0, 1
send: 1, 2
send: 2, 3
send: 3, 4
send: 4, 5
send: 5, 6
send: 6, 7
...Code language: HTML, XML (xml)

次に、受け取る側、receiver.py を作ります。信号が来ていないと5秒でタイムアウトするように作っています。

from socket import socket, AF_INET, SOCK_DGRAM

HOST = ''   
PORT = 5000

s = socket(AF_INET, SOCK_DGRAM)
s.bind((HOST, PORT))
s.settimeout(5) # タイムアウトを設定

try:
    while True:
        msg, address = s.recvfrom(8192) # 8192=2**13 のバイト数を受信
        msg = msg.decode('utf-8')
        arg0, arg1 = map(int, msg.split(", "))
        print(f"from {address}: {arg0}, {arg1}")
except KeyboardInterrupt:
    print("CTRL+Cを検出。終了します。")
except Exception as e:
    print("エラーが発生。終了します。")
    print(e)
finally:
    s.close()
Code language: Python (python)

receiver.pyが動いている状態で、新しいプロンプトを立ち上げて実行すると、receiver.pyが送信している数値を受け取って以下のように表示します。いい感じです。

(base) PS D:\myWorks\p22_udp> python .\receiver_udp.py
from ('127.0.0.1', 53690): 6, 7
from ('127.0.0.1', 53690): 7, 8
from ('127.0.0.1', 53690): 8, 9
from ('127.0.0.1', 53690): 9, 10
from ('127.0.0.1', 53690): 10, 11Code language: HTML, XML (xml)

これで、UDPの信号を受け取る準備ができました。

M5AtomLITE でWiFiを使ってUDPを送信

次に、LITEでUDPを送信することを試みます。sender.py と同じように、WiFiの接続を確立した後、1秒ごとにカウントアップの数値を2つ送り続けます。コードには、WiFiのSSID、パスワード、受け取る側のPCのIPアドレスを記入するようにします。PCのIPアドレスは”ipconfig” とコンソールでタイプすることで調べることができます。

#include "WiFi.h"
#include "WiFiUdp.h"
#include "M5Atom.h"

const char *ssid = "your_ssid";
const char *password = "your_passward";
const char *destination_ip = "xxx.xxx.xxx.xxx"; // 受け取るPCのIPアドレス
unsigned int destination_port = 5000;

WiFiUDP udp;
int count = 0;

void setup() {
  M5.begin(true, true, true);
  // WiFiの接続開始
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    // WiFiの接続中
    // LEDが緑で点滅
    M5.dis.drawpix(0, 0x00FF00);
    delay(250);
    M5.dis.drawpix(0, 0x000000);
    delay(250);
  }
}

void loop() {
  // UDPでデータを送信
  // LEDが青で点滅
  String message = String(count) + ", " + String(count + 1);
  Serial.println(message);
  udp.beginPacket(destination_ip, destination_port);
  udp.write((const uint8_t*)message.c_str(), message.length());
  udp.endPacket();

  count++;
  M5.dis.drawpix(0, 0x0000FF);
  delay(500);
  M5.dis.drawpix(0, 0x000000);
  delay(500);
}
Code language: C++ (cpp)

実行すると、WiFiの接続が確立するまでLEDが緑色で点滅します。WiFiが確立すると、UDPで情報を送り始めます。このとこには、LEDが青で点滅します。

LITEが青の点滅を始めてから、先ほど作ったsender.pyを起動すると、LITEからの情報が表示されます。

> python .\receiver_udp.py
from ('192.168.0.111', 54994): 48, 49
from ('192.168.0.111', 54994): 49, 50
from ('192.168.0.111', 54994): 50, 51
from ('192.168.0.111', 54994): 51, 52
from ('192.168.0.111', 54994): 52, 53
from ('192.168.0.111', 54994): 53, 54
from ('192.168.0.111', 54994): 54, 55
...Code language: HTML, XML (xml)

これでLITEからPCへのUDP通信ができました。

LITEは、PCにつながなくても、USBのtype-Cの電源供給さえすれば、WiFiでUDPを飛ばしてくれます。こんなにちっちゃいのにすごいです。

Post Views: 453