A processing and arduino sketch for 3D mapping with an ultrasonic sensor

ServoScan.ino

/*
Arduino program to control a ultrasonic 3-D mapping sensor.
The programs controls an HC-SR04 ultrasonic sensor connected to the Arduino board.
A collaborating Processing sketch interacts with this program with serial communication to control the angle 

Authors:Prithvijit Chakrabarty (prithvichakra@gmail.com)
        Kartik S. Lovekar (kslovekar@gmail.com)
*/
#include <Servo.h>
#include <NewPing.h>

//Set pin numbers
#define PING_PIN  8
#define ECHO_PIN  9
//Maximum distance(cm) the HC-SR04 can detect accurately
#define MAX_DISTANCE 450
//Time delay to wait for the receiver to register the echo
#define DELAY_LOOP  3

Servo servo1;
Servo servo2;
NewPing sonar(PING_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pin and maximum distance.
int val;

void setup() {
  pinMode(1,OUTPUT);
  //Attach servos
  servo1.attach(14); //analog pin 0
  servo2.attach(15);  //analog pin 1
  Serial.begin(9600);
  //Serial.println("Ready");
  val = 0;
  //Set servos to 0 degreees initially
  servo1.write(val);
  servo2.write(val);
}

void loop() {
  //Protocol to communicate with the Processing sketch
  if(Serial.available()){
    int c = Serial.read();
    //delay(50);
    switch(c){
      case '0'...'9': val = val*10 + c - '0';
                      break;
      case 'x': servo1.write(val);
                scan(1);
                c = '\0';
                break;
      case 'y': servo2.write(val);
                scan(2);
                c = '\0';
                break;
    }
    //servo1.write(c);
    //delay(50);
  }
}

//Run one scan with a sonar ping
void scan(int servo_num){
  int avg = 0, i, NUM = 3;
  //for(i = 0; i < NUM; i++){
      unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
      /*int dist = sonar.ping();
      avg += dist;
      //delay(30);
  }
  unsigned int uS = avg/NUM;*/
  
  //Format to communicate with the Processing sketch <servo_num>-<angle>:<distance>c
  //servo_num represents the horizontal or vertical servo
  Serial.print(servo_num);
  Serial.print("-");
  Serial.print(val);
  Serial.print(":");
  //Convert to cm
  Serial.print(uS / US_ROUNDTRIP_CM);
  Serial.print("c");
  val = 0;
}

UltraScan.pde

/*Processing sketch to control two servos
  and an HC-SR04 ultrasonic sensor for 3D surface scanning.

Authors:Prithvijit Chakrabarty (prithvichakra@gmail.com)
        Kartik S. Lovekar (kslovekar@gmail.com)
*/

import processing.serial.*;       
import toxi.geom.*;
import toxi.processing.*;
import peasy.*;

Serial port;
ToxiclibsSupport gfx;
PeasyCam cam;
int servo_num = 1, val = 0, alpha = 0, beta = 0, dist = 0;
int INC = 3;
float SERVO_RADIUS = 3.5;
float distances[][];
ArrayList<Vec3D> cloud;
boolean scanOver;

void setup(){
  size(700, 500, P3D);
  frameRate(100);
  println(Serial.list());
  port = new Serial(this, Serial.list()[0], 9600);
  
  //Setup 3D plotting tool
  gfx = new ToxiclibsSupport(this); 
  cam = new PeasyCam(this, 0, 0, 0, 100);
  cloud = new ArrayList<Vec3D>();
  scanOver = false;
  
  distances = new float[181][71];  //Holds the minimum distance to a point on a surface
  for(int i = 0; i < 181; i++)
    for(int j = 0; j < 71; j++)
      distances[i][j] = -1;
}

void draw(){
  background(255);
  if(!scanOver){
    //Control loops to control the servos on the Arduino
    for(int i = 1; i < 70; i+=INC){  //Loop for beta
      for(int j = 0; j < 180; j+=INC){  //Loop for alpha
        port.write(j+"x");  //Rotate servo 1
        delay(50);
      }
      for(int j = 180; j >= 0; j-=INC){  //Return loop for alpha
        port.write(j+"x");  //Rotate back servo 1
        delay(50);
      }
      port.write(i+"y");  //Rotate servo 2
      delay(50);
    }
  }
  scanOver = true;
  plot();
  drawAxes();
}

//Parse echo scan values returned from the Arduino
void serialEvent(Serial port){
  int next = port.read();
  //Format for return: <servo_num>-<angle>:<distance>c
  if(next == (int)('-')){
      servo_num = val;
      val = 0;
  }
  else if(next == (int)(':')){
      if(servo_num == 1)
        alpha = val;
      else if(servo_num == 2)
        beta = val;
      val = 0;
  }
  else if(next == (int)('c')){
      //Print current scan status
      println("Servo:"+servo_num+" alpha="+alpha+" beta="+beta+" Dist = "+val);
      dist = val;
      val = 0;
      
      //Find coordinates here - apply transforms to calculate (x,y,z)
      float x = SERVO_RADIUS*sin(radians(beta)) + dist*cos(radians(alpha))*cos(radians(beta));
      float y = dist*sin(radians(alpha));
      float z = SERVO_RADIUS*cos(radians(beta)) + dist*cos(radians(alpha))*sin(radians(beta));
      //Now, rotate out the faulty initial setting
      int INIT_XY = 20, INIT_XZ = 60;
      x = x*cos(radians(20))-y*sin(radians(20));
      y = x*sin(radians(20))+y*cos(radians(20));
      //rotation(INIT_XZ, x, z);
      x = x*cos(radians(60))-z*sin(-radians(60));
      z = x*sin(-radians(60))+z*cos(radians(60));
      if(distances[alpha][beta] != -1){
        distances[alpha][beta] = distances[alpha][beta]<dist?distances[alpha][beta]:dist;
        cloud.add(new Vec3D(x,y,z));
      }
      else
        distances[alpha][beta] = dist;
  }
  else if(next >= '0' && next <= '9'){
      val = val * 10 + next - (int)('0');
  }
}

//Add points from the cloud to the 3D space
void plot(){
     for(int i = 0; i < cloud.size(); i++)
        gfx.point(cloud.get(i)); 
}

void drawAxes() {
  //X axis - Red
  stroke(255, 0, 0);
  line(-500, 0, 0, 500, 0, 0);
  
  //Y axis - Green
  stroke(0, 255, 0);
  line(0, -500, 0, 0, 500, 0);
  
  //Z axis - Blue
  stroke(0, 0, 255);
  line(0, 0, -500, 0, 0, 500);
}

Further details on the project- https://www.dropbox.com/s/xyy5gs41gbnroqh/3D_SCANNER.pdf?dl=0