/**
*
* DAMI mapper v1.0
*
* Read data from the serial port and draw a room map with robot on it
*
*/
import processing.serial.*;
Serial myPort; // Create object from Serial class
int scaleFactor = 10;
int robotLenght = 192 / 4;
int robotWidth = 120 / 4;
// robot draw
float lastRx = 0;
float lastRy = 0;
float lastRa = 0;
// fill and clear
int[][] dPoints;
float r;
float theta;
float x;
float y;
void setup() {
String inBuffer;
size(1000, 1000);
background(0);
smooth();
dPoints = new int[16][2];
// Change the index on Serial.list()[0]
// On Windows [0] generally is COM1.
// Open whatever port is the one you're using.
String portName = Serial.list()[1];
println(portName);
myPort = new Serial(this, portName, 115200);
if ( myPort.available() > 0) { // If data is available,
inBuffer = myPort.readStringUntil('\n');
// send 0 to base station to stop sending data
myPort.write("0\n");
delay(50);
}
// send 19 to base station to stop sending data
myPort.write("19\n");
delay(250);
// send 1 to base station to stop sending data
myPort.write("1\n");
if ( myPort.available() > 0) { // If data is available,
inBuffer = myPort.readStringUntil('\n');
}
if ( myPort.available() > 0) { // If data is available,
inBuffer = myPort.readStringUntil('\n');
}
}
void draw() {
if ( myPort.available() > 0) { // If data is available,
String inBuffer = myPort.readStringUntil('\n');
if (inBuffer != null) {
//println(inBuffer);
String[] msg = inBuffer.split("\\s+");
//int ctl = Integer.parseInt(messages[0]);
Float rx = Float.parseFloat(msg[1]) / scaleFactor;
Float ry = Float.parseFloat(msg[2]) / scaleFactor;
Float ra = Float.parseFloat(msg[3]);
int msgIdx = 4;
int dPIdx = 0;
while(dPIdx < 16) {
dPoints[dPIdx][0] = Integer.parseInt(msg[msgIdx]) / scaleFactor;
msgIdx++;
dPoints[dPIdx][1] = Integer.parseInt(msg[msgIdx]);
msgIdx++;
dPIdx++;
}
int decision = Integer.parseInt(msg[msgIdx]);
msgIdx++;
println(decision);
//println("in ", ctl, rx, ry, ra, px0, py0, px1, py1, px2, py2);
drawRobot(lastRx, lastRy, lastRa, 0);
drawRobot(rx, ry, ra, 1);
lastRx = rx;
lastRy = ry;
lastRa = ra;
pushMatrix();
// move the origin to the pivot point
//translate(500 + rx - (robotLenght / 4.0), 500 + ry - (robotWidth / 4.0));
translate(500 + rx , 500 + ry );
// scale
//scale(.5);
// then pivot the grid
rotate(radians(ra));
// laser front
r = dPoints[0][0];
theta = radians(dPoints[0][1]);
x = r * cos(theta);
y = r * sin(theta);
stroke(255);
//line(0, 0, x, y);
stroke(255, 0, 0);
point(x, y);
// sonar left
r = dPoints[1][0];
theta = radians(dPoints[1][1]);
x = r * cos(theta);
y = r * sin(theta);
stroke(255);
//line(0, 0, x, y);
stroke(0, 255, 0);
point(x, y);
// sonar right
r = dPoints[2][0];
theta = radians(dPoints[2][1]);
x = r * cos(theta);
y = r * sin(theta);
stroke(255);
//line(0, 0, x, y);
stroke(0, 0, 255);
point(x, y);
// other points
dPIdx = 3;
while(dPIdx < 16) {
r = dPoints[dPIdx][0];
theta = radians(-(dPoints[dPIdx][1] - 90));
x = r * cos(theta);
y = r * sin(theta);
stroke(255);
//line(0, 0, x, y);
stroke(255, 255, 0);
point(x, y);
dPIdx++;
}
popMatrix();
}
}
}
void drawRobot(float x, float y, float a, int fill ) {
pushMatrix();
// move the origin to the pivot point
//translate(500 + (robotLenght / 2.0), 500 - (robotWidth / 4.0));
translate(500 + x , 500 + y );
// scale
scale(.5);
// then pivot the grid
rotate(radians(a));
// set fill and stroke
switch( fill ) {
case 0:
stroke(255);
fill(255);
break;
case 1:
noStroke();
fill(255, 0, 0);
break;
}
// and draw the square at the origin
rect( -robotLenght / 4.0, -robotWidth / 4.0, (robotLenght / 2.0), robotWidth / 2.0);
// control point 1 (not required)
stroke(0);
noFill();
ellipse(0, 0, 10, 10);
popMatrix();
// control point 2 (not required)
noStroke();
fill(0, 0, 255);
ellipse(x + 500, y + 500, 5, 5);
}