2019年8月12日月曜日

ESP-WROOM-02(ESP8266)で,9軸センサーBMX055から取得した値をパソコンにUDPで送信する.

前の記事で,ESP-WROOM-02 (ESP8266)9軸センサーBMX055からの値を取得できたので,これをUDPでパソコンのPythonプログラムに送信しました. 

IPアドレスの設定は以下の通りです.
  • パソコン側のIPアドレス:172.20.10.4
パソコンの受け側のUDPのポート番号を以下の値とします.
  • パソコン側のUDPポート番号:30000
パソコン側のPythonのプログラムは以下のとおりです.

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# Python 3.7.3 on macOS 10.15 Catalina Beta
# 2019/8/4
# receive JSON data by UDP

import argparse
import threading
import time
from socket import *

class ServerThread(threading.Thread):
    def __init__(self, ADDRESS="127.0.0.1", PORT=30000):
        threading.Thread.__init__(self)
        self.data = None
        self.kill_flag = False
        # line information
        self.ADDRESS = ADDRESS
        self.PORT = PORT
        self.BUFSIZE = 1024
        self.ADDR = (gethostbyname(self.ADDRESS), self.PORT)
        print(self.ADDR)
        # bind
        self.udpServSock = socket(AF_INET, SOCK_DGRAM)
        self.udpServSock.bind(self.ADDR) # HOST, PORTでbinding

    def run(self):
        while True:
            try:
                # データ受信
                data, self.addr = self.udpServSock.recvfrom(self.BUFSIZE)
                if len(data) > 0:
                    print("receive {} bytes".format(len(data)))
                    print("received data: {}".format(data))
                # OSCはデコードできなくてexceptionが起こる
                # JSONだとテキストなのでデコードできる
                self.data = data.decode()
                print("decoded data: {}".format(self.data))
            except:
                print("exception occrurred")
                #pass

def start_UDP_server(ip_address, port_number):
    print("starting on address {}, port {}".format(ip_address, port_number))
    th = ServerThread(ip_address, port_number)
    th.setDaemon(True)
    th.start()

    while True:
        time.sleep(1.0)
        if not th.data:
            continue
        print(th.data)

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", default="127.0.0.1",
                        help="IP address to listen on")
    parser.add_argument("--port", type=int, default=30000,
                        help="port to listen on")
    args = parser.parse_args()
    start_UDP_server(args.ip, args.port)


ESP-WROOM-02側のプログラムは以下のとおりです.

//================================================================//
//  AE-BMX055             ESP8266                            //
//    3V3                    +3.3V(1)                                  //
//    GND                    GND(17)                                  //
//    SDA                    IO4(12)                              //
//    SCL                    IO5(16)                              //
//                                                                //
//   (JP6,JP4,JP5はショートした状態)                              //
//   http://akizukidenshi.com/catalog/g/gK-13010/                 //
//   -> 3.3Vで使うのでJP6はショートしない
//================================================================//

#include 
#include 
#include 
#include 
#include  // sprintf()

WiFiUDP UDP;
IPAddress HOSTIP(172, 20, 10, 4);
const int remoteUdpPort = 30000;
IPAddress myIP(172, 20, 10, 10);
const int localUdpPort = 31000;
const char *ssid = "iPhone7PlusBlack";
const char *password = "qatacygd60y8u";

// BMX055 加速度センサのI2Cアドレス  
#define Addr_Accl 0x19  // (JP1,JP2,JP3 = Openの時)
// BMX055 ジャイロセンサのI2Cアドレス
#define Addr_Gyro 0x69  // (JP1,JP2,JP3 = Openの時)
// BMX055 磁気センサのI2Cアドレス
#define Addr_Mag 0x13   // (JP1,JP2,JP3 = Openの時)

// センサーの値を保存するグローバル関数
float xAccl = 0.00;
float yAccl = 0.00;
float zAccl = 0.00;
float xGyro = 0.00;
float yGyro = 0.00;
float zGyro = 0.00;
short int   xMag  = 0;
short int   yMag  = 0;
short int   zMag  = 0;

void connectWiFi() {
  Serial.println("WiFi setup start");
  WiFi.begin(ssid, password);
  WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask());
  Serial.println("WiFi setup done");
  Serial.println("start_connect");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("CONNECTED!");
}

void sendWiFi(char byteData[]) {
  if (UDP.beginPacket(HOSTIP, remoteUdpPort)) {
    UDP.write(byteData);
    UDP.endPacket();
    Serial.println(byteData);
  }
}

void reconnectWiFi() {
  if (WiFi.status() != WL_CONNECTED) {
    WiFi.disconnect();
    Serial.println("disconnected! reconnect WiFi.");
    connectWiFi();
  }
}

void setup()
{
  // Wire(Arduino-I2C)の初期化
  Wire.begin();
  // デバック用シリアル通信は9600bps
  Serial.begin(9600);
  //BMX055 初期化
  BMX055_Init();
  delay(300);
  //WiFi初期化
  WiFi.mode(WIFI_STA);
  UDP.begin(localUdpPort);
  connectWiFi();
}

void loop()
{
  char buf[128];
  Serial.println("--------------------------------------"); 

  //BMX055 加速度の読み取り
  BMX055_Accl();
  sprintf( buf, "Accel= %f,%f,%f", xAccl, yAccl, zAccl);
  sendWiFi(buf);
  delay(500);
  reconnectWiFi();

  //BMX055 ジャイロの読み取り
  BMX055_Gyro();
  sprintf( buf, "Gyro= %f,%f,%f", xGyro, yGyro, zGyro);
  sendWiFi(buf);
  delay(500);
  reconnectWiFi();
  
  //BMX055 磁気の読み取り
  BMX055_Mag();
  sprintf( buf, "Mag= %d,%d,%d", xMag, yMag, zMag);
  sendWiFi(buf);
  delay(500);
  reconnectWiFi();  
}

//=====================================================================================//
void BMX055_Init()
{
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x0F); // Select PMU_Range register
  Wire.write(0x03);   // Range = +/- 2g
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x10);  // Select PMU_BW register
  Wire.write(0x08);  // Bandwidth = 7.81 Hz
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x11);  // Select PMU_LPW register
  Wire.write(0x00);  // Normal mode, Sleep duration = 0.5ms
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x0F);  // Select Range register
  Wire.write(0x04);  // Full scale = +/- 125 degree/s
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x10);  // Select Bandwidth register
  Wire.write(0x07);  // ODR = 100 Hz
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x11);  // Select LPM1 register
  Wire.write(0x00);  // Normal mode, Sleep duration = 2ms
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x83);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x01);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4C);  // Select Mag register
  Wire.write(0x00);  // Normal Mode, ODR = 10 Hz
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4E);  // Select Mag register
  Wire.write(0x84);  // X, Y, Z-Axis enabled
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x51);  // Select Mag register
  Wire.write(0x04);  // No. of Repetitions for X-Y Axis = 9
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x52);  // Select Mag register
  Wire.write(0x16);  // No. of Repetitions for Z-Axis = 15
  Wire.endTransmission();
}
//=====================================================================================//
void BMX055_Accl()
{
  unsigned char data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Accl);
    Wire.write((2 + i));// Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
    // Read 6 bytes of data
    // xAccl lsb, xAccl msb, yAccl lsb, yAccl msb, zAccl lsb, zAccl msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data to 12-bits
  xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (xAccl > 2047)  xAccl -= 4096;
  yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (yAccl > 2047)  yAccl -= 4096;
  zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (zAccl > 2047)  zAccl -= 4096;
  xAccl = xAccl * 0.0098; // renge +-2g
  yAccl = yAccl * 0.0098; // renge +-2g
  zAccl = zAccl * 0.0098; // renge +-2g
}
//=====================================================================================//
void BMX055_Gyro()
{
  unsigned char data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Gyro);
    Wire.write((2 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Gyro, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  xGyro = (data[1] * 256) + data[0];
  if (xGyro > 32767)  xGyro -= 65536;
  yGyro = (data[3] * 256) + data[2];
  if (yGyro > 32767)  yGyro -= 65536;
  zGyro = (data[5] * 256) + data[4];
  if (zGyro > 32767)  zGyro -= 65536;

  xGyro = xGyro * 0.0038; //  Full scale = +/- 125 degree/s
  yGyro = yGyro * 0.0038; //  Full scale = +/- 125 degree/s
  zGyro = zGyro * 0.0038; //  Full scale = +/- 125 degree/s
}
//=====================================================================================//
void BMX055_Mag()
{
  unsigned short int data[8];
  for (int i = 0; i < 8; i++)
  {
    Wire.beginTransmission(Addr_Mag);
    Wire.write((0x42 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Mag, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  xMag = ((data[1] <<8 data="">>3));
  if (xMag > 4095)  xMag -= 8192;
  yMag = ((data[3] <<8 data="">>3));
  if (yMag > 4095)  yMag -= 8192;
  zMag = ((data[5] <<8 data="">>3));
  if (zMag > 16383)  zMag -= 32768;
}


パソコン側で以下のようにPythonプログラムを起動します.

% ./UDPserver1.py --ip 172.20.10.4 --port 30000


ESP-WROOM-02側のシリアルモニタでは,以下のようにデータが送信できています.

--------------------------------------
Accel= -0.098000,0.098000,9.780400
Gyro= -0.049400,-0.262200,0.095000
Mag= -503,11,-498
--------------------------------------


パソコン側では以下のようにデータが受信できています.

receive 34 bytes
received data: b'Accel= -0.098000,0.098000,9.780400'
decoded data: Accel= -0.098000,0.098000,9.780400
Accel= -0.098000,0.098000,9.780400
receive 34 bytes
received data: b'Gyro= -0.049400,-0.262200,0.095000'
decoded data: Gyro= -0.049400,-0.262200,0.095000
receive 17 bytes
received data: b'Mag= -503,11,-498'
decoded data: Mag= -503,11,-498
Mag= -503,11,-498


0 件のコメント:

コメントを投稿