Wio Terminal のIMUでProcessingのモデルをぐりぐりしてみた。

前回、M5Stackで３Dモデルをグリグリしたのに味を占めて、Wio terminal でもやってみました。
M5では、Bluetoothのシリアル接続でIMUのデータをProcessingにアップしていましたけど、今回は、WiFiを通して、TCPのソケット通信で加速度センサーのデータをアップしています。
また、Wio terminalには、ジャイロセンサーは、ついていないので、加速度センサーのみでのグリグリになります。
したがって、Pitch方向とRoll方向には、動かせるけど、Yaw方向は動かせません。
また、ぐるっと180度反転したときに、Z軸の富豪の反転で、180度反転したことはわかるのですが、Pitch方向で反転したのか、Roll方向で反転したのか判別できないように思います。
このあたり、何かやり方があるのかな…？
なんか、手探り状態ですが、一応、グリグリは、できたので、記録代わりに置いておきます。

WioTerminal側のスケッチは、以下
もし、追試される方があるようでしたら、ご自分の環境に合わせて、WiFiのSSIDとPassword を設定してください。　また、サーバーとなるPCのIPアドレスも設定が必要です。Portは、とりあえず、20000にしていますが、これも、必要に応じて好きなポートに設定してくださいね。
お約束ですが、何かあっても責任はとれませんので、あくまでも自己責任で…。
と、いうのも、先日、M5Stackででっかい画像データを転送して、表示させようとしたら、いきなり、TFTが壊れてしまったようで、何も表示されなくなってしまいました。
なにも、ハード的に何か触ったわけでもないので、まさか、そんなことで壊れるわけはないと思っていたのですが、まぁ、そんなことも起こったりするので、念のため、リスクは、ご自分で管理ください。

#include <AtWiFi.h>
#include"LIS3DHTR.h"
#include "TFT_eSPI.h"
#include <math.h>

LIS3DHTR<TwoWire> lis;
TFT_eSPI tft;

const char* ssid = "your SSID";
const char* password =  "your password";
 
void setup() {
    Serial.begin(115200);
    //while(!Serial); // Wait for Serial to be ready
    delay(1000);
    //Display setup
    tft.begin();
    tft.setRotation(3);
    tft.setTextColor(TFT_YELLOW);
    tft.setTextSize(2);
    tft.fillScreen(TFT_BLACK);
    
    //Accerometer setup
    lis.begin(Wire1);
 
    if (!lis) {
      Serial.println("ERROR");
      while(1);
    }
    lis.setOutputDataRate(LIS3DHTR_DATARATE_25HZ); //Data output rate
    lis.setFullScaleRange(LIS3DHTR_RANGE_2G); //Scale range set to 2g

    // Set WiFi to station mode and disconnect from an AP if it was previously connected
    WiFi.mode(WIFI_STA);
    WiFi.disconnect();
    delay(2000);
 
    WiFi.begin(ssid, password);
 
    tft.println("Connecting to WiFi");
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.println("Connecting to WiFi..");
        tft.print(".");
    }
    Serial.println("Connected to the WiFi network");
    Serial.print("IP Address: ");
    Serial.println (WiFi.localIP()); // prints out the device's IP address
    tft.print("\n");
    tft.println(WiFi.localIP());
}
 
 
void loop() {
    const uint16_t port = 20000;  // Default port
    const char* host = "XXX.XXX.XX.XX";  // Target Server IP Address

    String ss;
    
    Serial.print("Connecting to ");
    Serial.println(host);
 
    // Use WiFiClient class to create TCP connections
    WiFiClient client;
    Serial.println("client created");
    tft.println("client created");
    
    client.connect(host, port);
    Serial.println("client connected");
    tft.println("client connected");
    while ( client.connected() ) {

      // Get acceleration data
      float x_values, y_values, z_values, x_angle, y_angle;
      
      x_values = lis.getAccelerationX();
      y_values = lis.getAccelerationY();
      z_values = lis.getAccelerationZ();

      x_angle = asin(x_values);
      y_angle = asin(y_values);
      
      ss=String(x_angle) + " " + String(y_angle) + " " + String(z_values) + "\n" ;
      client.print(ss);
      delay(100);
    }
 
    Serial.println("Closing connection.");
    client.stop();
 
    Serial.println("Waiting 5 seconds before restarting...");
    delay(5000);
}

Processing 側のプログラムは、以下です。
600X480X100の直方体に6面の画像を張り付けています。　画像は、プログラムと同じフォルダに別途用意する必要があります。クオリティは、悪いですが、自前で用意するのが面倒な方は、以下の画像をどうぞ。
front.jpg
back.jpg
top.jpg
bottom.jpg
right.jpg
left.jpg

import processing.net.*;

Server server;
PImage front, back, right, left, top, bottom;

float PI = 3.1415;
float rotationY = 0.0;

void setup() {
  server = new Server(this, 20000);
  
  size(800, 600, P3D);
  front = loadImage("front.jpg");
  back = loadImage("back.jpg");
  right = loadImage("right.jpg");
  left = loadImage("left.jpg");
  top = loadImage("top.jpg");
  bottom = loadImage("bottom.jpg");
  textureMode(IMAGE);
}
  //float rotationZ=0;

void draw() {
  
  // TCP comm and IMU data
  Client c = server.available();
    if (c != null) {
      String s = c.readStringUntil('\n'); 
      println("server received: " + s);
      String[] ss = splitTokens(s);
      float pitch = float(ss[0]);
      if(Float.isNaN(pitch)) pitch = 1.57;
      float roll = float(ss[1]);
      if(Float.isNaN(roll)) roll = -1.57;
      float z_value = float(ss[2]);
      println("x=" + pitch);
      println("y=" + roll);
      println("z=" + z_value);
      if(z_value>0) {
        pitch = -PI/2-(PI/2 + pitch); 
        //roll = -PI/2-(PI/2 + roll); 
      }    

  // Graphic
  background(0);
  translate(width / 2, height / 2);
  scale(0.4);

  pushMatrix();
  if(mousePressed) {rotationY = map(mouseX, 0, width, -PI, PI); }
  
  rotateX(-1*pitch);
  rotateY(rotationY);
  rotateZ(-1*roll);
  drawWio();
  popMatrix();
    }
}

void drawWio() {
  
  beginShape();
  texture(front);
  vertex(-300, -50, -240,   0,   0); //V1
  vertex( 300, -50, -240, 600,   0); //V2
  vertex( 300, -50,  240, 600, 480); //V3
  vertex(-300, -50,  240,   0, 480); //V4
  endShape();

  beginShape();
  texture(back);
  vertex( 300,  50, -240,   0,   0); //V1
  vertex(-300,  50, -240, 600,   0); //V2
  vertex(-300,  50,  240, 600, 480); //V3
  vertex( 300,  50,  240,   0, 480); //V4
  endShape();

  beginShape();
  texture(right);
  vertex( 300,  -50,  240,   0,   0); //V1
  vertex( 300,   50,  240,   0, 100); //V2
  vertex( 300,   50, -240, 480, 100); //V3
  vertex( 300,  -50, -240, 480,   0); //V4
  endShape();

  beginShape();
  texture(left);
  vertex(-300,  -50, -240,   0,   0); //V1
  vertex(-300,  -50,  240, 480,   0); //V2
  vertex(-300,   50,  240, 480, 100); //V3
  vertex(-300,   50, -240,   0, 100); //V4
  endShape();

  beginShape();
  texture(top);
  vertex( 300,   50, -240,   0,   0); //V1
  vertex(-300,   50, -240, 600,   0); //V2
  vertex(-300,  -50, -240, 600, 100); //V3
  vertex( 300,  -50, -240,   0, 100); //V4
  endShape();

  beginShape();
  texture(bottom);
  vertex(-300,  -50,  240,   0,   0); //V1
  vertex( 300,  -50,  240, 600,   0); //V2
  vertex( 300,   50,  240, 600, 100); //V3
  vertex(-300,   50,  240,   0, 100); //V4
  endShape();
  
}

実行する際は、先にProcessing の方を実行してからWioTerminalを立ち上げると行けると思います。