dami_map_mod1_v1
/**
*
* 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 robotLenght = 192;
int robotWidth = 120;
// robot draw
float lastRx = 0;
float lastRy = 0;
float lastRa = 0;
void setup() {
size(1900, 1000);
background(255);
smooth();
// 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);
}
void draw() {
if ( myPort.available() > 0) { // If data is available,
String inBuffer = myPort.readStringUntil('\n');
if (inBuffer != null) {
//println(inBuffer);
String[] messages = inBuffer.split("\\s+");
/*
//if (messages.length != 5) {
if (messages.length != 2) {
println("Invalid message size: ", messages.length);
return;
}
*/
//int ctl = Integer.parseInt(messages[0]);
Float rx = Float.parseFloat(messages[1]);
Float ry = Float.parseFloat(messages[2]);
Float ra = Float.parseFloat(messages[3]);
Float px0 = Float.parseFloat(messages[4]);
Float py0 = Float.parseFloat(messages[5]);
Float px1 = Float.parseFloat(messages[6]);
Float py1 = Float.parseFloat(messages[7]);
Float px2 = Float.parseFloat(messages[8]);
Float py2 = Float.parseFloat(messages[9]);
//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
stroke(255, 0, 0);
point(rx + px0, py0);
// sonar left
stroke(0, 255, 0);
// note: nao entendo porque é que tenho que adicionar px1/10
//point(getCoordinate(px1) + px1/10, getCoordinate(py1));
point(rx + px1, py1);
// sonar right
stroke(0, 0, 255);
point(rx + px2, py2);
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);
}