Arduino + Processing: 3D Sensor Data Visualisation

/*
luckylarry.co.uk
3D Scan Visualisation for Sharp GP2Y0A02 IR range finder
Sends sensor readings for every degree moved by the servo
values sent to serial port to be picked up by Processing
*/
#include <Servo.h>            // include the standard servo library
Servo leftRightServo;         // set a variable to map the servo
int leftRightPos = 0;         // set a variable to store the servo position
const int numReadings = 10;   // set a variable for the number of readings to take
int index = 0;                // the index of the current reading
float total = 0;              // the total of all readings must be a float to allow totaling of float values
int average = 0;              // the average
int IRpin = 1;                // analog pin for reading the IR sensor
 
/* setup the pins, servo and serial port */
void setup() { 
  leftRightServo.attach(9);
  // initialize the serial port:
  Serial.begin(9600);
} 
 
/* begin rotating the servo and getting sensor values */
void loop() { 
  for(leftRightPos = 0; leftRightPos < 180; leftRightPos++) {  // going left to right.                                
    leftRightServo.write(leftRightPos);             
      for (index = 0; index<=numReadings;index++) {            // take x number of readings from the sensor and average them
        float volts = analogRead(IRpin)*0.0048828125;          // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
        float distance = 65*pow(volts, -1.10);                 // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
        total = total + distance;                              // update total
        delay(20);
      }
    average = (int) total/numReadings;                         // create average reading CAST TO INT!! remove the decimal places
 
    if (index >= numReadings)  {                               // reset the counts when at the last item of the array    
      index = 0;           
      total = 0;     
    }
    Serial.print("X");                                         // print leading X to mark the following value as degrees
    Serial.print(leftRightPos);                                // current servo position
    Serial.print("V");                                         // preceeding character to separate values
    Serial.println(average);                                   // average of sensor readings
  }
  /* 
  start going right to left after we got to 180 degrees 
  same code as above
  */
  for(leftRightPos = 180; leftRightPos > 0; leftRightPos--) {  // going right to left                                
    leftRightServo.write(leftRightPos);             
    for (index = 0; index<=numReadings;index++) {
      float volts = analogRead(IRpin)*0.0048828125;            // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
      float distance = 65*pow(volts, -1.10);                   // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
      total = total + distance;
      delay(20);
    }
    average = (int) total/numReadings;  
    if (index >= numReadings)  {           
      index = 0;           
      total = 0;     
    }
    Serial.print("X");
    Serial.print(leftRightPos);
    Serial.print("V");
    Serial.println(average);
   }  
}

/*
luckylarry.co.uk
3D Scan Visualisation for Sharp GP2Y0A02
Maps out an area of what the GP2Y0A02 sees into a 3D view
*/
import processing.serial.*;     // import serial library
Serial myPort;                  // declare a serial port
int x, y, z;                     // variable to store x and y co-ordinates for vertices   
int value = 0;                  // value from sensor
int[][][] data = new int[181][61][1];  // create an array to store each new sensor value for each servo position
int count;
int average = 1;
PFont myFont;                   // setup fonts in Processing
int numberOfSegments = 179;
createArc Arc;                  // create an instance of the Arc class
float radius = 150;
float angle = 0;
 
void setup(){
  size(640, 360, P3D);
  Arc = new createArc();        // assign the Arc classes to an object
  myFont = createFont("verdana", 12);
  textFont(myFont);
  // setup the serial port and buffer
  myPort = new Serial(this, Serial.list()[1], 9600);
  myPort.bufferUntil('\n');
 
}
 
void draw(){
  background(255);
  float tempX = 0, tempY = 1, tempZ = 0;
  fill(200);
  lights();
  translate(width/2, 200, 110);
  angle = 180.0 / numberOfSegments;
  rotateY(count * PI/10800);
  for (int j = 0; j < numberOfSegments; j++){
      tempZ = cos(radians(angle))*radius;
      tempX = sin(radians(angle))*radius;
      pushMatrix();
      translate(tempX, tempY, tempZ); // move each instance we create of the arc object to a slightly new position and rotation
      rotateY(radians(angle));    
      Arc.create(j);
      popMatrix();
      angle += 180.0/numberOfSegments;
  }
 
}
// creates an arc object using QUAD_STRIP
class createArc {
// pass values to create method to know which array to load 1 to 180...
  void create(int degree){
    pushMatrix();
    rotateY(radians(135));
    beginShape(QUAD_STRIP);
    for(int a=60; a < 120;a++) {  
      float x1 = cos(radians(((90+a))))*(data[degree][a-60][0]/average);
      float y1 = sin(radians(((90+a))))*(data[degree][a-60][0]/average);
      float x2 = cos(radians(((90+a))))*(data[degree+1][a-60][0]/average);
      float y2 = sin(radians(((90+a))))*(data[degree+1][a-60][0]/average);
      vertex(x1,y1,100);
      vertex(x2,y2,105);
    }  
    endShape();
    popMatrix();
  }
} 
 
/* get values from serial port */
 
void serialEvent (Serial myPort) {
  String xString = myPort.readStringUntil('\n');  // read the serial port until a new line
    if (xString != null) {  // if theres data in between the new lines
    	xString = trim(xString); // get rid of any whitespace just in case
    	String getY = xString.substring(1, xString.indexOf("X")); // get the value of the servo position
        String getX = xString.substring(xString.indexOf("X")+1, xString.indexOf("Z")); // get the value of the sensor reading
        String getZ = xString.substring(xString.indexOf("Z")+1, xString.length()); // get the value of the sensor reading
    	x = Integer.parseInt(getX); // set the values to variables
    	y = Integer.parseInt(getY);
        z = Integer.parseInt(getZ);
        data[x][y][0] += z;
        //println(z); // for debugging
        count++;
        if (count > 10800) {
          count = 0;
          average++;       
        }
  }
}