/////////////////////////////////////////////////////////////
//
// *** Arduino Code ***
//
// Reads reflected ultrasound data as function of angular
// position of Servo Motor. Prints the measured values of 
// distance in cm and angle in degrees to the serial port.
// Dec 2015
/////////////////////////////////////////////////////////////
#include <Servo.h>
int echoPin = 7;
int trigPin = 8;
unsigned long duration;
float distance; // Duration used to calculate distance
int angle = 5, value=1;

Servo motor;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  // we need additional +5 volts from pin 6
  pinMode(6,OUTPUT);
  digitalWrite(6,HIGH);
  motor.attach(9);
  motor.write(angle);
}

void loop() {
    motor.write(angle);

    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);

    distance = duration/58.2;

    if (distance > 200 || distance < 2) {
      Serial.print(200);
      Serial.print(",");
      Serial.println(angle);
    }
    else {
      Serial.print(int(distance));
      Serial.print(",");
      Serial.println(angle);
    }

    angle = angle + value;
    if(angle==0 || angle==160) value = -value; 
    delay(100);
  
}

 
/////////////////////////////////////////////////////////////
//
// *** Processing Code ***
//
// Plots data collected from the Serial port (COM3).
// This program requires Transform.pde which can be found at
// http://www1.gantep.edu.tr/~bingul/ep486/src/Transform.pde
// Dec 2015
/////////////////////////////////////////////////////////////
import processing.serial.*;
Serial myPort;

int X1, Y1, X2,Y2;
int msd = 50; // maximum distance you want to sweep
Transform ct; // coordinate transformation

void setup(){
  size(800,500);
  ct = new Transform(-msd,msd, 0, msd);
  println(Serial.list());
  myPort = new Serial(this,  "COM3", 9600);
}

void draw(){
   while (myPort.available() > 0)
   {
     String data = myPort.readStringUntil('\n');
     println(data);
     if(data==null) continue;

     String[] listnums = split(data, ',');
     float distance = float(listnums[0]);
     float angle    = float(listnums[1]);

     if(angle==0 || angle==160) background(200);

     if(distance>0 && distance==distance && angle==angle){
       drawRadar(distance, angle);
       delay(100);
     }
   }
} 

// This functions draws the radar lines in polar coordinates.
// r     : the input distance in cm
// theta : the input angle in radians
void drawRadar(float r, float theta){
  theta = theta * 3.1415926/180.0;
  float x = r*cos(theta);
  float y = r*sin(theta);
  X1 = ct.getX(0); 
  Y1 = ct.getY(0);
  X2 = ct.getX(x); 
  Y2 = ct.getY(y);
  line(X1, Y1, X2, Y2);
}