Sorry for taking so long to reply! The forum didn’t e-mail me that anyone posted. ![:slight_smile: :slight_smile:](https://forum.pololu.com/images/emoji/twitter/slight_smile.png?v=9)
There are two files in my Processing sketch.
range_finder:
/**
* range_finder -- Interface to SHARP ARRS range-finder
*
* Controls & visualizes data from the range finder mounted on a servo.
*
* Jan 3, 2012 Travers Naran Created
*/
import processing.serial.*;
Serial tankbotPort;
final int SCAN_ANGLE = 190;
final int WEDGE_SIZE = 10;
final int MAX_OBSERVATIONS = SCAN_ANGLE / WEDGE_SIZE;
final int DEAD_AHEAD = 90; // degrees
final float MOVE_LIMIT = 4;
int ranges[] = new int[MAX_OBSERVATIONS];
int heading = 0;
int toMove = 0;
PFont font;
//
// FLAGS
//
boolean blocked = false;
boolean busy = false;
void setup()
{
font = loadFont("Helsinki-Narrow-48.vlw");
size(500, 500);
smooth();
println(Serial.list());
tankbotPort = new Serial(this, "COM4", 9600);
tankbotPort.bufferUntil(10);
scan();
}
void drawRange(float radius)
{
stroke(64, 64, 64);
ellipseMode(CENTER);
float angleOffset = 90 - DEAD_AHEAD;
for (int i = 0; i < MAX_OBSERVATIONS; i++) {
float angle1 = PI-radians(i * WEDGE_SIZE) - radians(90 - DEAD_AHEAD);
float angle2 = PI-radians((i + 1) * WEDGE_SIZE) - radians(90 - DEAD_AHEAD);
float wedgeRadius = ranges[i] / 128.0 * radius;
line(0, 0, wedgeRadius * cos(angle1), wedgeRadius * sin(angle1));
if ( ranges[i] < 60 )
fill(250, 16, 16);
else if (ranges[i] < 100)
fill(255, 255, 0);
else
fill(32, 128, 32);
arc(0, 0, wedgeRadius * 2, wedgeRadius * 2, angle2, angle1);
line(0, 0, wedgeRadius * cos(angle2), wedgeRadius * sin(angle2));
}
}
void drawArrow(float radius, float heading) {
float northX = radius * cos(radians(heading + 90));
float northY = radius * sin(radians(heading + 90));
stroke(255, 0, 0);
pushMatrix();
rotate(radians(heading));
float arrowRadius = radius;
line(0, 0, 0, arrowRadius);
line(0, arrowRadius, - arrowRadius / 40, arrowRadius - arrowRadius / 20);
line(0, arrowRadius, arrowRadius / 40, arrowRadius - arrowRadius / 20);
popMatrix();
}
void drawNorth(float radius) {
// Draw heading
float northX = radius * cos(radians(heading + 90));
float northY = radius * sin(radians(heading + 90));
stroke(250, 250, 255);
fill(250, 250, 255);
pushMatrix();
rotate(radians(heading));
float arrowRadius = radius - 20;
line(0, 0, 0, arrowRadius);
line(0, arrowRadius, - arrowRadius / 40, arrowRadius - arrowRadius / 20);
line(0, arrowRadius, arrowRadius / 40, arrowRadius - arrowRadius / 20);
scale(1, -1);
textFont(font, 24);
textAlign(CENTER, BOTTOM);
text("N", 0, -arrowRadius);
popMatrix();
}
void draw()
{
if (busy)
background(128, 16, 0);
else
background(0, 16, 128);
float radius = min(width / 2, height / 2) - 10;
pushMatrix();
scale(1.0, -1.0);
translate(width / 2, -height / 2);
drawRange(radius);
drawNorth(radius);
if ( mousePressed ) {
// Calculate vector from mouse xy
float x = mouseX - width / 2;
float y = -(mouseY - height / 2);
stroke(255, 255, 0);
line(0, 0, x, y);
}
// Draw move limit
stroke( 128, 128 );
noFill();
ellipse( 0, 0, radius / (MOVE_LIMIT / 2), radius / (MOVE_LIMIT / 2) );
popMatrix();
if (blocked)
{
textFont(font, 24);
textAlign(LEFT, TOP);
text("BLOCKED!", 0, 0);
}
}
void mouseReleased() {
float radius = min(width / 2, height / 2) - 10;
blocked = false;
// Calculate vector from mouse xy
float x = mouseX - width / 2;
float y = -(mouseY - height / 2);
// angle = -atan2(y, x) + HALF_PI
int angle = floor(degrees(atan2(x, y)));
if (angle < 0)
{
command("lt", -angle);
}
else
{
command("rt", angle);
}
// d = sqrt(x^2 + y^2) * 5000 / radius
float d = sqrt(x*x + y*y);
// If d > MOVE_LIMIT then FD floor(d)
if (d >= (radius / MOVE_LIMIT) ) {
d = d * 5000 / radius;
toMove = floor(d);
}
else
toMove = 0;
}
void keyPressed()
{
scan();
}
tankbot:
void scan()
{
busy = true;
command("sc");
tankbotPort.write("hd\r\n");
redraw();
}
void command(String s)
{
println("proc> " + s);
tankbotPort.write(s + "\r\n");
}
void command(String s, int v)
{
println("proc> " + s + " " + v);
tankbotPort.write(s + " " + v + "\r\n");
}
void serialEvent(Serial p) {
String inString = (tankbotPort.readString());
println("tank> " + trim(inString));
if (inString.startsWith("DONE"))
{
if( toMove > 0 ) {
command("fd",toMove);
toMove = 0;
} else
scan();
}
else if (inString.startsWith("SCAN"))
{
inString = inString.substring(5);
String[] readings = split(inString, ' ');
for (int i = 0; i < readings.length; i++ ) {
if (i < MAX_OBSERVATIONS) {
ranges[i] = unhex(readings[i]);
}
}
busy = false;
redraw();
}
else if (inString.startsWith("HEADING"))
{
inString = inString.substring(8);
heading = int(trim(inString));
busy = false;
redraw();
}
else if (inString.startsWith("BLOCKED"))
{
blocked = true;
scan();
}
}
Have fun!