Processing Sketch 1

3D - 360 Degree Sensing Demo

Processing Sketch 1 - Just download the application from www.processing.org

/**

* Interface to robotmaker's 3D-360degree sensor (www.robotmaker.eu)

* by Colin Bacon. 7th June, 2011

*

* This program interfaces to the IRCF360 via an Arduino. It could also have been done directly with

* a Serial port level such as MAX232

* This routine just sends a command to the IRCF360 to start 360 degee proximity sensing routine

* By clicking on the screen it will stop and start the command.

* The Sketch is a 3D environment and a floating disk represents direction of highest Proxmity reading.

* This is only approximate and no clever maths has been done. Each direction is checked and then averaged

* The closer you get to the IRCF360 the Z-Axis is effected and also the size

* The disk will fly to the next reading received.

* This exmaple has is a combination of many of the example from the processing software

* Credits go to all those who created the first examples

*

* Note: This sketch assumes an Arduino and a ROBOTmaker 3D-360 sensor is connetect to the usb port

*/

import processing.serial.*;

float x_arm = 100;

float y_arm = 200;

int bgcolor; // Background color

int fgcolor; // Fill color

int N;

int NE;

int E;

int SE;

int S;

int SW;

int W;

int NW;

float x ;

float y ;

float angle;

int fuzzyFactor = 5;

int maxIndex ;

float oldTheta;

float theta ;

float theta1 ;

float theta2 ;

float rotaryAngle = 0.000;

float diameter = 84.0;

float segLength = 100;

Serial myPort; // The serial port

int[] serialInArray = new int[24]; // Where we'll put what we receive

int serialCount = 0; // A count of how many bytes we receive

int xpos, ypos; // Starting position of the ball

boolean firstContact = false; // Whether we've heard from the microcontroller

PGraphics pg;

float radius = 150;

int maxValue;

float x1;

float y1, z1;

float targetX, targetY, targetZ;

float easing = 0.05;

float r;

void setup() {

// Print a list of the serial ports, for debugging purposes:

println(Serial.list());

size(800, 800, P3D);

pg = createGraphics(80, 80, P3D);

frameRate(30);

smooth();

stroke(204, 102, 0);

fill(100);

rect(width/4, height/4, 55, 55);

//ellipse(0,0, 100, 100);

rotateX(0.8);

// I know that the first port in the serial list on my mac

// is always my FTDI adaptor, so I open Serial.list()[0].

// On Windows machines, this generally opens COM1.

// Open whatever port is the one you're using.

String portName = Serial.list()[0];

myPort = new Serial(this, portName, 9600);

}

void draw() {

background(51); //White Backgroun

// check to see which direction has max value

findMaxValue ();

// defineLights();

translate(width/2, height/2,0 );

// Orange point light on the right

pointLight(150, 100, 0, // Color

200, -150, 0); // Position

// Blue directional light from the left

directionalLight(255, 255, 109, // Color

1, 0, 0); // The x-, y-, z-axis direction

ambientLight(128, 128, 128);

// Yellow spotlight from the front

spotLight(0, 255, 109, // Color

100, 40, 200, // Position

0, -0.5, -0.5, // Direction

PI , 2); // Angle, concentration

rotateX(0.8);

// Draw the ellipse at the cartesian coordinate

ellipseMode(CENTER);

noStroke();

fill(200);

targetX = x;

float dx = targetX - x1;

if(abs(dx) > 10) {

x1 += dx * easing;

}

targetY =y;

float dy = targetY - y1;

if(abs(dy) > 10) {

y1 += dy * easing;

}

targetZ = maxValue;

float dz = targetZ - z1;

if(abs(dz) > 10) {

z1 += dz * easing;

}

translate (x1,y1,z1);

pushMatrix();

ellipse(x1, y1, z1, z1);

sphere (z1);

//rotateX(theta);

//rotateY(theta);

//box (z1);

popMatrix();

//println(N + "\t" + NE + "\t" + E + "\t" + SE + "\t" + S + "\t" + SW + "\t" + W + "\t" + NW +"Theta="+theta);

line (CENTER,CENTER,x,y);

}

void serialEvent(Serial myPort) {

// read a byte from the serial port:

int inByte = myPort.read();

// if this is the first byte received, and it's an A,

// clear the serial buffer and note that you've

// had first contact from the microcontroller.

// Otherwise, add the incoming byte to the array:

if (firstContact == false) {

if (inByte == 'A') { //echo from Arduino / IRCF

myPort.clear(); // clear the serial port buffer

firstContact = true; // you've had first contact from the microcontroller

}

}

else {

//if first contact has been made then add subsequent data to the array

if (firstContact = true) {

// Add the latest byte from the serial port to array:

//Take and average of 50 readings

//first read 50 cylces

serialInArray[serialCount] = inByte ;

serialCount++;

// If we have 8 bytes:

if (serialCount > 7) {

N = serialInArray[0];

NE = serialInArray[1];

E = serialInArray[2];

SE = serialInArray[3];

S = serialInArray[4];

SW = serialInArray[5];

W = serialInArray[6];

NW = serialInArray[7];

// print the values (for debugging purposes only):

//println(N + "\t" + NE + "\t" + E + "\t" + SE + "\t" + S + "\t" + SW + "\t" + W + "\t" + NW);

// Reset serialCount:

serialCount = 0;

myPort.clear(); // clear the serial port buffer

}

}

}

// println("N="+ N + " NE="+ NE + " E="+ E + " SE="+ SE + " S=" +S + " SW=" +SW + " W=" +W + " NW=" +NW) ;

}

void findMaxValue () {

maxValue=0;

maxIndex = 0;

maxValue = serialInArray[0];

for (int i = 1; i < serialInArray.length; i++) {

if (serialInArray[i] > maxValue) {

maxValue = serialInArray[i];

maxIndex = i;

}

}

//Check that divide by zero does not occur and that index does not rollback to -1

// calculation of theta is atan( maxvalue / serialInArray[maxIndex - 1])

// this is to work of the angle of a triangle in a circle using trig T=O/A

// Looking at the top of the sensor each IR LED is position at 45 degrees.

// Compare reading between each IRLED by plottiong LED1 on Y axis and LED2 on X axis. reading are then used

// to find the angle between the two readings. But as the LEDs are position 45 degrees rather than 90 degrees apart the answer needs to be divided by 2

//______________________________________________________

// N LED has max value

if (maxValue > 0) {

if (maxIndex == 0) {

//float theta1 ;

//float theta2 ;

//theta1 = acos(serialInArray[0] / 50);

//theta2 = asin(serialInArray[7] / 50;

r=150 ;

//float r= sqrt (serialInArray[1]*serialInArray[1] + serialInArray[7] * serialInArray[7]);

theta = atan2(serialInArray[0],serialInArray[7]-serialInArray[1]);

// Convert polar to cartesian

x = r * cos(theta);

y = r * sin(theta);

}

if (maxIndex == 1) {

theta = atan2(serialInArray[1],serialInArray[0]-serialInArray[2]);

theta2 = radians (45);

theta = theta + theta2;

// Convert polar to cartesian

x = r * cos(theta);

y = r * sin(theta);

}

if (maxIndex == 2) {

theta = atan2(serialInArray[2],serialInArray[1]-serialInArray[3]);

theta2 = radians (2*45);

theta = theta + theta2;

// Convert polar to cartesian

x = r * cos(theta);

y = r * sin(theta);

}

if (maxIndex == 3) {

theta = atan2(serialInArray[3],serialInArray[2]-serialInArray[4]);

theta2 = radians (3*45);

theta = theta + theta2;

// Convert polar to cartesian

x = r * cos(theta);

y = r * sin(theta);

}

if (maxIndex == 4) {

theta = atan2(serialInArray[4],serialInArray[3]-serialInArray[5]);

theta2 = radians (4*45);

theta = theta + theta2;

// Convert polar to cartesian

x = r * cos(theta);

y = r * sin(theta);

}

if (maxIndex == 5) {

theta = atan2(serialInArray[5],serialInArray[4]-serialInArray[6]);

theta2 = radians (5*45);

theta = theta + theta2;

// Convert polar to cartesian

x = r * cos(theta);

y = r * sin(theta);

}

if (maxIndex == 6) {

theta = atan2(serialInArray[6],serialInArray[5]-serialInArray[7]);

theta2 = radians (6*45);

theta = theta + theta2;

// Convert polar to cartesian

x = r * cos(theta);

y = r * sin(theta);

}

if (maxIndex == 7) {

theta = atan2(serialInArray[7],serialInArray[6]-serialInArray[0]);

theta2 = radians (7*45);

theta = theta + theta2;

// Convert polar to cartesian

x = r * cos(theta);

y = r * sin(theta);

}

}

if (maxValue==0) {

x=0;

y=0;

}

}

void mousePressed()

{

//if(circleOver) {

//fill(circleColor);

firstContact=false; //reset header byte

myPort.write('A'); //start the transmission)

// }

}