「M5Atomで作る歩行ロボット」という本の犬型ロボットの製作記です。
本の内容からそれてしまっていますが、Ubuntuが必要なROSを使わずに、Windowsで完結するシステムにしたいと考え、ヌンチャクとPCの通信をUDPで行おうと思っています。
ここでは、Wiiのヌンチャクの信号をUDPでPCのUnityに送り、それに応じた描画をするという実験をしました。
WiiヌンチャクのM5Atom LITE 側
Wiiヌンチャクの信号をI2C通信で取得し、WiFiを使ってUDPでPCに送るプログラムです。ヌンチャクの信号は、zボタンとcボタンのオン/オフ(zbtn, cbtn)、十字キーのx値(joyx)、y値(joyy)、ヌンチャクの傾き3方向(accx, accy, accy) の7種類あります。
// ヌンチャクの情報をUDPで送信する
#include "WiFi.h"
#include "WiFiUdp.h"
#include "M5Atom.h"
#include <Wire.h>
static uint8_t nunchuck_buf[6]; // ヌンチャクの状態保存用バッファ
// WiFiの設定
const char *ssid = "your_ssid"; // 適切なものを記載
const char *password = "passward"; //適切なものを記載
const char *destination_ip = "xxx.xxx.xxx.xxx"; // PCのIPアドレスを記載
unsigned int destination_port = 5000;
// UDPの設定
WiFiUDP udp;
void setup() {
// M5Atomの初期化
M5.begin(true, false, true);
// I2Cの初期化
Serial.begin(19200);
Wire.begin(19, 22);
Wire.beginTransmission(0x52);
Wire.write((uint8_t)0x40);
Wire.write((uint8_t)0x00);
Wire.endTransmission();
// WiFiの接続開始, LEDを緑に点灯
WiFi.begin(ssid, password);
while (WiFi.status() != WL_CONNECTED) {
M5.dis.drawpix(0, 0x00FF00);
delay(500);
}
M5.dis.drawpix(0, 0x000000);
}
// ヌンチャクからデータを受信する
static int nunchuck_get_data()
{
int cnt=0;
Wire.requestFrom (0x52, 6);
while (Wire.available ()) {
nunchuck_buf[cnt] = nunchuk_decode_byte( Wire.read() );
cnt++;
}
nunchuck_send_request();
if (cnt >= 5) {
return 0; // success
}
return 1; // failure
}
// データのデコーディング
static char nunchuk_decode_byte (char x)
{
// x = (x ^ 0x17) + 0x17; // 純正品のヌンチャクの場合はコメントを外す
return x;
}
// ヌンチャクにデータをリクエストする
static void nunchuck_send_request()
{
Wire.beginTransmission(0x52);
Wire.write((uint8_t)0x00);
Wire.endTransmission();
}
static int nunchuck_zbutton()
{
return ((nunchuck_buf[5] >> 0) & 1) ? 0 : 1;
}
static int nunchuck_cbutton()
{
return ((nunchuck_buf[5] >> 1) & 1) ? 0 : 1;
}
static int nunchuck_joyx()
{
return nunchuck_buf[0];
}
static int nunchuck_joyy()
{
return nunchuck_buf[1];
}
static int nunchuck_accelx()
{
return nunchuck_buf[2];
}
static int nunchuck_accely()
{
return nunchuck_buf[3];
}
static int nunchuck_accelz()
{
return nunchuck_buf[4];
}
void loop() {
// ヌンチャクの情報を取得
if (nunchuck_get_data()) {
M5.dis.drawpix(0, 0xFF0000); // 取得失敗時はLEDを赤色に点灯
delay(500);
M5.dis.drawpix(0, 0x000000);
delay(500);
return;
}
int zbut = nunchuck_zbutton();
int cbut = nunchuck_cbutton();
int joyx = map(nunchuck_joyx(),3,252,-15,15); // -15, 15の範囲に変換
int joyy = map(nunchuck_joyy(),2,253,-15,15);
int accx = map(nunchuck_accelx(),0,255,-15,15);
int accy = map(nunchuck_accely(),0,255,-15,15);
int accz = map(nunchuck_accelz(),0,255,-15,15);
// シリアルモニタに表示
Serial.print("zbut:"); Serial.print(zbut * 10); Serial.print(",");
Serial.print("cbut:"); Serial.print(cbut * -10); Serial.print(",");
Serial.print("joyx:"); Serial.print(joyx); Serial.print(",");
Serial.print("joyy:"); Serial.print(joyy); Serial.print(",");
Serial.print("accx:"); Serial.print(accx); Serial.print(",");
Serial.print("accy:"); Serial.print(accy); Serial.print(",");
// Serial.print("accz:"); Serial.print(accz); Serial.print(",");
Serial.print("min:"); Serial.print(-20); Serial.print(","); // グラフ表示用
Serial.print("max:"); Serial.print(20); //グラフ表示用
Serial.print("\n");
// データを送信
String message =
+ "," + String(zbut)
+ "," + String(cbut)
+ "," + String(joyx)
+ "," + String(joyy)
+ "," + String(accx)
+ "," + String(accy)
+ "," + String(accz);
udp.beginPacket(destination_ip, destination_port);
udp.write((const uint8_t*)message.c_str(), message.length());
udp.endPacket();
delay(20);
}Code language: C++ (cpp)Unity
UnityでUDPをひろい、描画に反映する部分のプログラムはこんな感じです。初めなかなかうまくいかなかったのですが、ファイヤーウォールの設定が必要でした。こちらのブログを参考にさせていただきました。
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Net;
using System.Net.Sockets;
using System.Text;
using TMPro;
public class UDP_receiver : MonoBehaviour
{
private UdpClient udpClient;
private int port = 5000;
public GameObject cube;
public GameObject zbut_obj;
public GameObject cbut_obj;
public TMPro.TextMeshProUGUI text_board;
private int zbut = 0;
private int cbut = 0;
private int joyx = 0;
private int joyy = 0;
private int accx = 0;
private int accy = 0;
private int accz = 0;
void Start()
{
try
{
udpClient = new UdpClient(port);
udpClient.BeginReceive(ReceiveCallback, null);
Debug.Log("UDP Receiver started on port " + port);
}
catch (Exception e)
{
Debug.LogError("Failed to start UDP Receiver: " + e.Message);
}
}
void Update()
{
string msg = string.Format("zbut: {0}\tcbut: {1}\tjoyx: {2}\tjoyy: {3}\naccx: {4}\taccy: {5}\taccz: {6}", zbut, cbut, joyx, joyy, accx, accy, accz);
Debug.Log(msg);
text_board.text = msg;
// Existing code
float scale = 0.2f;
cube.transform.position = new Vector3(joyx * scale, 1.5f, joyy * scale);
float rot_scale = 8.0f;
cube.transform.rotation = Quaternion.Euler(accy * rot_scale, 0.0f, -accx * rot_scale);
if (cbut == 1)
{
cbut_obj.GetComponent<Renderer>().material.color = Color.green;
}
else
{
cbut_obj.GetComponent<Renderer>().material.color = Color.gray;
}
if (zbut == 1)
{
zbut_obj.GetComponent<Renderer>().material.color = Color.green;
}
else
{
zbut_obj.GetComponent<Renderer>().material.color = Color.gray;
}
}
private void OnDisable()
{
if (udpClient != null)
{
udpClient.Close();
}
}
private void ReceiveCallback(IAsyncResult ar)
{
try
{
IPEndPoint endPoint = new IPEndPoint(IPAddress.Any, port);
byte[] receivedBytes = udpClient.EndReceive(ar, ref endPoint);
string receivedMessage = Encoding.ASCII.GetString(receivedBytes);
udpClient.BeginReceive(ReceiveCallback, null);
Controllcube(receivedMessage);
}
catch (Exception e)
{
Debug.LogError("Failed to receive UDP message: " + e.Message);
}
}
private void Controllcube(string message)
{
try
{
string[] parts = message.Split(new string[] { "," }, StringSplitOptions.RemoveEmptyEntries);
zbut = int.Parse(parts[0]);
cbut = int.Parse(parts[1]);
joyx = int.Parse(parts[2]);
joyy = int.Parse(parts[3]);
accx = int.Parse(parts[4]);
accy = int.Parse(parts[5]);
accz = int.Parse(parts[6]);
}
catch (Exception e)
{
Debug.LogError("Failed to parse message: " + e.Message);
}
}
}
Code language: C# (cs)完成図
こんな感じです。けっこう反応は良いです。
コメントを残す