IR Range Finder + Processing = WINNING!

(Sorry for the Sheen-ism)

So I bought one of those SHARP IR Optical Range Finders and mounted it on a servo:



With a small Arduino sketch to control the servo and sample the output, I set up the sketch to respond to commands sent via the Serial port (“SC”) which triggered the scan code:

double convertToDistance(int analogValue) {
  // Convert to rough distance
  double voltage = max(analogValue,1) * 5.0 / 1024;
  return 57.673 / voltage;
}

void scan()
{
  // Sweep servo from 0 to 180
  irServo.write(0);
  delay(250);

  for( int angle = 0; angle < 190; angle += 10 )
  {
    irServo.write(angle);
    delay(50);

    // Read IR analog
    int range = convertToDistance(analogRead(IR_IN_PIN));

    // Output distance in hex
    Serial.print((int)range, HEX);
    Serial.print(' ');
  }
  irServo.write(90);
  Serial.println();
}

It would write to the Serial output a line like:

3E EC 78 52 171 9F 90 89 B5 77 7B 48 3E 20 2F 20 32 2A 45

Then using the Processing framework (don’t laugh! I used to laugh at Processing too until this project :neutral_face:), I wrote a simple program to send the “SC” command to the Arduino and read the hex string returned.

I converted the text string into wedges, color coded based on “danger”, and got this:


.

You click on the screen and it rescans and redraws. I’ve got it hooked up to my Wixel-controlled Arduino Tank, and I think I’ll send it scouting around the apartment. :slight_smile:

What do you guys think?

Hello, tnaran.

Thanks for sharing your project! That is nice visualization of the distance sensor’s output. What is the fastest rate that you can practically perform scans?

- Kevin

Awesome project, saw a similar radar yesterday. Can you share your processing code too please?

Sorry for taking so long to reply! The forum didn’t e-mail me that anyone posted. :slight_smile:

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!

About 25 ms per range scan seems to be about the sweet spot for me. Any faster and no two scans return the same result. Any slower, and I can’t see a difference in quality.

Very cool!

I love Processing! I was able to put together a quick visualization of my robot’s navigation/orientation in something like an hour… would’ve taken me days and days if I’d started from scratch.