IR Range Finder + Processing = WINNING!

tnaran · January 4, 2012, 6:47am

(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 ), 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.

What do you guys think?

kevin · January 9, 2012, 7:12pm

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

meanpc · January 26, 2012, 2:16pm

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

tnaran · April 29, 2012, 4:40am

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

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!

tnaran · April 29, 2012, 4:44am

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.

shimniok · May 7, 2012, 2:47am

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.