/**
* 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 thetaOld;
float fi = 0;
int ctrX;
int ctrY;
float newTheta;
//float d20x, d20y;
//float d21x, d21y;
//float d22x, d22y;
//float d23x, d23y;
//float d24x, d24y;
//float d25x, d25y;
//float d26x, d26y;
//float d27x, d27y;
float x20a, y20a;
float x21a, y21a;
float x22a, y22a;
float x23a, y23a;
float x24a, y24a;
float x25a, y25a;
float x26a, y26a;
float x27a, y27a;
float targetX, targetY, targetZ;
float target20X, target20Y;
float target21X, target21Y;
float target22X, target22Y;
float target23X, target23Y;
float target24X, target24Y;
float target25X, target25Y;
float target26X, target26Y;
float target27X, target27Y;
float easing = 0.05;
float r;
boolean startByte, ignore;
PFont fontA;
float x20,x21,x22,x23,x24,x25,x26,x27;
float y20,y21,y22,y23,y24,y25,y26,y27;
float dsdt;
//---------------------------------------------------------------------------------
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();
// 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);
//Graphical Text
fontA = loadFont("CourierNew36.vlw");
textAlign(CENTER);
// Set the font and its size (in units of pixels)
textFont(fontA, 32);
}
void draw()
{
background(85); //White Backgroun
translate (width/2, height/2,0);
//Draw the ground-level grid
//Change the value of rotate to get a 3D view of the grid
rotateX(0.8);
pushMatrix ();
translate (0, 0,0);
stroke(255, 102, 0);
noFill();
rect(0, 0, 150, 150);
rect(0, 0, -150, 150);
rect(0, 0, 150, -150);
rect(0, 0, -150, -150);
popMatrix();
// check to see which direction has max value
findMaxValue ();
findValues ();
// Yellow spotlight from the front
ambientLight(128, 128, 128);
spotLight(255, 255, 109, // Color
100, 40, 300, // Position
width/4, height/4, 0, // Direction
PI , 2); // Angle, concentration
// Draw a floating ellipse at the cartesian coordinate
ellipseMode(CENTER);
noStroke();
fill(200);
// Average out the position of the ellipise by checking old value to new values
//Draw a floating ellipse at the cartesian coordinate
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;
}
// Average out the position of the ellipise for each sensor direction in a spider web by checking old value to new values
//Draw an ellipse at the cartesian coordinate for each direction N,NE,E,SE,S,SW,W,NW
//----------------------------------------
target20X = x20;
float d20x = target20X - x20a;
if(abs(d20x) > 10) {
x21a += d20x * easing;
}
target20Y = y20;
float d20y = target20Y - y20a;
if(abs(d20y) > 10) {
y20a += d20y * easing;
}
//----------------------------------------
target21X = x21;
float d21x = target21X - x21a;
if(abs(d21x) > 10) {
x21a += d21x * easing;
}
target21Y = y21;
float d21y = target21Y - y21a;
if(abs(d21y) > 10) {
y21a += d21y * easing;
}
//----------------------------------------
target22X = x22;
float d22x = target22X - x22a;
if(abs(d22x) > 10) {
x22a += d22x * easing;
}
target22Y = y22;
float d22y = target22Y - y22a;
if(abs(d22y) > 10) {
y22a += d22y * easing;
}
//----------------------------------------
target23X = x23;
float d23x = target23X - x23a;
if(abs(d23x) > 10) {
x23a += d23x * easing;
}
target23Y = y23;
float d23y = target23Y - y23a;
if(abs(d23y) > 10) {
y23a += d23y * easing;
}
//----------------------------------------
target24X = x24;
float d24x = target24X - x24a;
if(abs(d24x) > 10) {
x24a += d24x * easing;
}
target24Y = y24;
float d24y = target24Y - y24a;
if(abs(d24y) > 10) {
y24a += d24y * easing;
}
//----------------------------------------
target25X = x25;
float d25x = target25X - x25a;
if(abs(d25x) > 10) {
x25a += d25x * easing;
}
target25Y = y25;
float d25y = target25Y - y25a;
if(abs(d25y) > 10) {
y25a += d25y * easing;
}
//----------------------------------------
target26X = x26;
float d26x = target26X - x26a;
if(abs(d26x) > 10) {
x26a += d26x * easing;
}
target26Y = y26;
float d26y = target26Y - y26a;
if(abs(d26y) > 10) {
y26a += d26y * easing;
}
//----------------------------------------
target27X = x27;
float d27x = target27X - x27a;
if(abs(d27x) > 10) {
x27a += d27x * easing;
}
target27Y = y27;
float d27y = target27Y - y27a;
if(abs(d27y) > 10) {
y27a += d27y * easing;
}
//--SHADOWS-------------------------
//Copy the postion to give a shadow in 3D mode
pushMatrix();
// Create a Shadow of the images//
noStroke();
fill(254,255,254);
//Create a shadow
ellipse(x1*2, y1*2, z1+10, z1+10);
fill(254,254,254);
//lines from centre of frame
noStroke();
noFill();
line (0,0,x20a*4,y20a*4);
line (0,0,x21a*4,y21a*4);
line (0,0,x22a*4,y22a*4);
line (0,0,x23a*4,y23a*4);
line (0,0,x24a*4,y24a*4);
line (0,0,x25a*4,y25a*4);
line (0,0,x26a*4,y26a*4);
line (0,0,x27a*4,y27a*4);
//ellipses on end of lines
noStroke();
fill (254,254,254);
ellipse(x20a*4,y20a*4, 10, 10);
ellipse(x21a*4,y21a*4, 10, 10);
ellipse(x22a*4,y22a*4, 10, 10);
ellipse(x23a*4,y23a*4, 10, 10);
ellipse(x24a*4,y24a*4, 10, 10);
ellipse(x25a*4,y25a*4, 10, 10);
ellipse(x26a*4,y26a*4, 10, 10);
ellipse(x27a*4,y27a*4, 10, 10);
//Join up all the lines
stroke(100,100, 100);
line (x20a*4,y20a*4,x21a*4,y21a*4);
line (x21a*4,y21a*4,x22a*4,y22a*4);
line (x22a*4,y22a*4,x23a*4,y23a*4);
line (x23a*4,y23a*4,x24a*4,y24a*4);
line (x24a*4,y24a*4,x25a*4,y25a*4);
line (x25a*4,y25a*4,x26a*4,y26a*4);
line (x26a*4,y26a*4,x27a*4,y27a*4);
line (x27a*4,y27a*4,x20a*4,y20a*4);
//--RADAR ARCS-------------------------
stroke(0, 255,0,90);
fill (0,100,0,60);
arc (0,0,maxValue*4,maxValue*4, ((-atan2(x1,y1)-0.5)+PI/2),(-atan2(x1,y1)+0.5)+PI/2);
noFill();
noStroke();
translate (0,0,z1*5);
fill(130,254,254);
ellipse(x1*2, y1*2, z1+10, z1+10);
fill(50,0,0);
//lines from centre of frame
stroke(200, 200, 200);
line (0,0,x20a*4,y20a*4);
line (0,0,x21a*4,y21a*4);
line (0,0,x22a*4,y22a*4);
line (0,0,x23a*4,y23a*4);
line (0,0,x24a*4,y24a*4);
line (0,0,x25a*4,y25a*4);
line (0,0,x26a*4,y26a*4);
line (0,0,x27a*4,y27a*4);
//ellipses on end of lines
noStroke();
fill (255,255,0);
ellipse(x20a*4,y20a*4, 10, 10);
ellipse(x21a*4,y21a*4, 10, 10);
ellipse(x22a*4,y22a*4, 10, 10);
ellipse(x23a*4,y23a*4, 10, 10);
ellipse(x24a*4,y24a*4, 10, 10);
ellipse(x25a*4,y25a*4, 10, 10);
ellipse(x26a*4,y26a*4, 10, 10);
ellipse(x27a*4,y27a*4, 10, 10);
//Join up all the lines
stroke(100, 100, 0);
line (x20a*4,y20a*4,x21a*4,y21a*4);
line (x21a*4,y21a*4,x22a*4,y22a*4);
line (x22a*4,y22a*4,x23a*4,y23a*4);
line (x23a*4,y23a*4,x24a*4,y24a*4);
line (x24a*4,y24a*4,x25a*4,y25a*4);
line (x25a*4,y25a*4,x26a*4,y26a*4);
line (x26a*4,y26a*4,x27a*4,y27a*4);
line (x27a*4,y27a*4,x20a*4,y20a*4);
popMatrix ();
//--CUBE----------------------------
translate(-width/2,-height/2.0, 100);
pushMatrix();
// Orange point light on the right
pointLight(150, 100, 0, // Color
200, -150, 0); // Position
// Blue directional light from the left
directionalLight(0, 102, 255, // Color
1, 0, 0); // The x-, y-, z-axis direction
// Yellow spotlight from the front
spotLight(255, 255, 109, // Color
0, 40, 200, // Position
0, -0.5, -0.5, // Direction
PI / 2, 2); // Angle, concentration
rotateY(map(x1, 0, width, 0, PI*2));
rotateX(map(-y1, 0, height, 0, PI*2));
box(100);
popMatrix();
textAlign(LEFT);
text ("Distance= "+ maxValue*2+"%", 0,200, 350,30);
}
//_____________________________________________________________
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
serialCount = 0;
}
}
else {
//if first contact has been made then add subsequent data to the array
// Add the latest byte from the serial port to array:
//println ("looking "+inByte);
if ((inByte == 255) && ( firstContact = true))
{
println ("Startbyte found " + serialCount + " " + inByte);
boolean startByte=true; //flag start byte found
serialCount = 0;
boolean ignore=true; //ignore this value in the array
} else {
serialInArray[serialCount] = inByte ;
serialCount++; //if its not the startbit then store this value
}
//if (inByte==255) serialCount=serialCount-1; //reset counter if a startbit
//}
// 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
startByte=false; //reset startbyte flag
ignore=false;
//}
}
}
}
// println("N="+ N + " NE="+ NE + " E="+ E + " SE="+ SE + " S=" +S + " SW=" +SW + " W=" +W + " NW=" +NW) ;
//__________________________________________________________________
void findValues ()
{
r=150;
// Line for NE direction
// Convert polar to cartesian
x20 = serialInArray[0] * cos(radians (270));
y20 = serialInArray[0] * sin(radians (270));
x21 = serialInArray[1] * cos(radians (315));
y21 = serialInArray[1] * sin(radians (315));
x22 = serialInArray[2] * cos(radians (0));
y22 = serialInArray[2] * sin(radians (0));
x23 = serialInArray[3] * cos(radians (45));
y23 = serialInArray[3] * sin(radians (45));
x24 = serialInArray[4] * cos(radians (90));
y24 = serialInArray[4] * sin(radians (90));
x25 = serialInArray[5] * cos(radians (135));
y25 = serialInArray[5] * sin(radians (135));
x26 = serialInArray[6] * cos(radians (180));
y26 = serialInArray[6] * sin(radians (180));
x27 = serialInArray[7] * cos(radians (225));
y27 = serialInArray[7] * sin(radians (225));
}
//__________________________________________________________________
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 r= sqrt (serialInArray[1]*serialInArray[1] + serialInArray[7] * serialInArray[7]);
theta = atan2(serialInArray[0],serialInArray[7]-serialInArray[1]);
theta2 = radians (180);
theta = theta + theta2;
// 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+180);
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+180);
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+180);
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+180);
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+180);
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+180);
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+180);
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)
myPort.clear();
serialCount=0;
// }
}