最近、自作VIVE trackerづくりを進めています。

前回までは、ブレッドボード上に構築した回路でBase Stationからの赤外線信号を受光したり、その受光した信号のパルスからデータを読めるようにしたりしました。

今回は、同等の回路を持つ「自作基板の動作チェック」と、「信号処理を深める」こと、「Rayを可視化」することを行なったことについて記事にしていきます。

今回の完成形

基板を移動させると、画面上の緑の線（Ray）が移動していることがわかるかと思います。

自作基板の動作チェック

f:id:ume-boshi:20210522230753j:plain — このカッコいい基板

ume-boshi.hatenablog.jp

Seeeduinoを用いた基板で、最大6つのPhoto Diodeが接続できます。今回は1つのPhoto Diodeしか使っていませんが。2つのPhoto Diodeが使えなくなる代わりに、2つのスイッチとか1つの可変抵抗を使用することも可能です。

前回はESP32で実装していましたが、Seeeduinoにマイコンを変えたところでソースコードがほぼ変化しませんでした。

信号処理を深める

前回までは、「フェーズ開始 → Sweep信号受光」に掛かった時間までしか計算していませんでしたが、Rayの方向を出すところまで実装しました。

f:id:ume-boshi:20210519224126p:plain — これです

//Base Stationのsweep信号を受信する箇所
//LighthouseA
if(LH[0])photo1.axisTime[0][photo1.nextAxis] = now - photo1.signalStart;
//LighthouseB
if(LH[1])photo1.axisTime[1][photo1.nextAxis] = now - photo1.signalStart;

if(digitalRead(10) == HIGH){ // ボタンスイッチを押している間のみ送信
  // Sweepの検出角度(rad)を求める
  float theta = 120 * (int(photo1.axisTime[0][0])-4000) * 3.141592 * 0.000006;
  float phi = 120 * (int(photo1.axisTime[0][1])-4000) * 3.141592 * 0.000006;
  char buf[20];
  char buf1[20];
  char buf2[20];
  dtostrf(theta,8,3,buf1); // 文字を0埋めしたかったが効いていない
  dtostrf(phi,8,3,buf2);
  sprintf(buf, "%s,%s\n",buf1,buf2); // 送信文字列を作成
  Serial.print(buf); // 送信
}

Rayの可視化

処理の流れとしては、まずSeeeduinoから115200bpsの速度でSerial通信し、その値をProcessingで1文字ずつ受け取ります。そのメッセージから数値を解析し、x軸に関するSweepのtheta角と、y軸に関するSweepのphi角を取得します。そして、この2角度をもとにRayの方向ベクトルを算出し、画面上にlineとして出力している形です。

f:id:ume-boshi:20210519224746p:plain — Rayの方向ベクトルを求める方法

import processing.serial.*;
Serial port;

float theta = 0;
float phi = 0;
int in_data;
int[] datas = new int[100]; //Seeeduinoからのメッセージ受け取り用

void setup() {
  size(1000, 1000, P3D); //P3Dライブラリを使う
  port = new Serial(this, "COM4", 115200); // Seeeduinoの接続先
  background(0);
  noFill();
  stroke(255);
}

void draw() {
  textSize(80);
  int boxSize = int(width * 0.5);
  // 描画エリア設定
  if (frameCount%1 == 0) {
    // 方向ベクトルを求める
    float xdeg =  -1 * cos(theta)*sin(phi);
    float ydeg =  sin(theta)*cos(phi);
    float zdeg =  -1 * cos(theta)*cos(phi);

    background(0);
    noFill();
    stroke(255);
    text(theta, 20, 80);
    text(phi, 20, 160);

    // Rayが見やすい角度に画面を回転しboxを描画
    translate(width/2, height/2);
    rotateX(-PI / 8.0);
    rotateY(PI / 2.0);
    box(boxSize);

    // Rayの始点を分かりやすくするための対角線
    line(boxSize/2, -boxSize/2, -boxSize/2, -boxSize/2, boxSize/2, -boxSize/2);
    line(boxSize/2, boxSize/2, -boxSize/2, -boxSize/2, -boxSize/2, -boxSize/2);
    
    // Rayの描画
    translate(0, 0, -boxSize/2);
    stroke(0, 255, 0);
    line(0, 0, 0, xdeg*(boxSize/zdeg), ydeg*(boxSize/zdeg), boxSize);

    // Rayの先端に丸を描く
    translate(0, 0, boxSize);
    stroke(0, 255, 255);
    ellipse(xdeg*(boxSize/zdeg), ydeg*(boxSize/zdeg), 10, 10);
  }

  if (port.available() > 0 ) {
    int count = 0;
    int minus = 1;
    int xy = 0;
    theta = 0;
    phi = 0;

    // 馬鹿みたいに1文字ずつ受信して、解析したぞ！
    while (port.available() > 0) {
      in_data = port.read();

      if (in_data == 13) { //\r
        continue;
      } else if (in_data == 10) { //\n
        break;
      } else {
        switch(in_data) {
        case 32:count++; break;//" "
        case 43:count++; break;//+
        case 44:xy=1; theta=theta*minus; minus=1; break;//,
        case 45:count++; minus=-1; break;//-
        case 46:count++; break;//,
        default://print((in_data-48)); datas[count]=in_data; 
          if (xy == 0) theta += (in_data-48)*(pow(10, (3-count)));
          else phi += (in_data-48)*(pow(10, (11-count)));
          count++; 
          break;
    }}}
    phi=phi*minus;
}}