* 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)
// }
}
