レーダー探知機を自作（完成編）

#include <Servo.h>
Servo servo;
int angle = 0;
bool ch = true;

int data;
int L = 0;

float T = 0.05;
float K = 1.0;

unsigned long t0, t1;
float y0, y1;
int u0, u1;

void setup() {
  Serial.begin(9600);
  servo.attach(3);
  servo.write(angle);
  y0 = 0.0;
  y1 = 0.0;
  u0 = 0.0;
  u1 = 0.0;
  t0 = millis();
}

void loop() {
  y0 = y1;
  u0 = u1;
  if(ch)
  {
    angle = angle + 1;
    if(angle>=180)
    {
      ch = false;
    }
  }
  else
  {
    angle = angle - 1;
    if(angle<=0)
    {
      ch = true;
    }
  }
  servo.write(angle);
  data = analogRead(0);
  u1 = data;
  t1 = millis();
  float Ts = (float)(t1-t0)/1000;
  y1 = (2*T-Ts)/(2*T+Ts)*y0+K*Ts/(2*T+Ts)*(float)(u1+u0);
  L = (int)(pow(y1, -1.25)*21000);
  Serial.write((byte)(1));
  Serial.write((byte)(200));
  Serial.write((byte)(angle));
  Serial.write((byte)(L));
  delay(20);  
  t0 = t1;
}

import processing.serial.*;
Serial port;
int[] data = new int[2];

float W, H;

int angle = 0;
int ch = 1;
float L = 0;
float[] Point = new float[181];

void setup() {
  size(displayWidth,displayHeight);  // windowのサイズを画面いっぱいにする
  port = new Serial(this, Serial.list()[0], 9600);
  background(25);  // 背景の色指定 黒に指定
  W = displayWidth/33.87;    // 位置を調整しやすいようにPowerPointの単位に合わせる
  H = displayHeight/19.08;
}

void draw() {
  pushMatrix();  // 現在の座標系を保存
  translate(displayWidth/2, H*15);  // 円の中心に原点を移動
  noFill();  // 塗りつぶしなし
  strokeWeight(1);  // 線の幅は1
  stroke(157, 195, 230);  // 線の色設定
  ellipse(0, 0, H*13, H*13);  // 25cmを示す円
  ellipse(0, 0, H*26, H*26);  // 50cmを示す円
  fill(49, 235, 255);  // ロボットの位置を表すマークの設定
  noStroke();
  triangle(0, 0, 0, -H*1, -W*0.5, H*0.25);  // ロボットの位置を表示
  triangle(0, 0, 0, -H*1, W*0.5, H*0.25);
  popMatrix();  // 原点を元に戻す
  noStroke();
  fill(157, 195, 230);  // 文字の色設定
  textAlign(CENTER, TOP);  // 文字を中心のトップにそろえる
  textSize(18);
  text("15", displayWidth/2, H*2);
  text("7.5", displayWidth/2, H*8.5);
  
  pushMatrix();
  angle = data[0];
  L = data[1];
  Point[angle] = L;  // 距離と角度を配列として格納
  fill(25, 10);  // 残像の設定
  rectMode(CORNER);
  rect(0, 0, displayWidth, displayHeight);
  pushMatrix();  // 現在の座標系を保存
  translate(displayWidth/2, H*15);  // レーダーの中心に原点を移動
  rotate(-angle*PI/180);  // レーダーの向いている角度に合わせる
  fill(49, 235, 255);
  stroke(49, 235, 255);
  triangle(0, 0, H*13/15*L, 5, H*13/15*L, -5);  // レーダーの描画
  popMatrix();
  translate(displayWidth/2, H*15);  // 原点を下に戻す
  strokeWeight(10);  // 物体を認識した場所に点を打つ
  for(int i=0; i<181; i++)
  {  
    point(H*13/15*Point[i], 0);  // 格納したポイントに点を打つ
    rotate(-1*PI/180);
  }
  popMatrix();  // 原点を元に戻す
}

void serialEvent(Serial port){
  // シリアルポートからデータを受け取ったら
  if (port.available() >=4) {
    //// シリアルデータ受信
      if(port.read()==1)
      {
        if(port.read()==200)
        {
          data[0] = port.read();
          data[1] = port.read();
        }
      }
  }
}