Arduino UNO LIDAR Scanner. How To?

[dondon pramis]

The arduino code is pretty straight-forward: moving the servos and reading from the sensor. During development two main problems were found. Reading from the sensor inside nested loops was not working properly, making the servos behave unexpectedly.

A solution was using conditionals inside the arduino loop() to make the servos move. Another problem was communicating with the PC: took us some time to figure out that the string being sent through the serial port should have a fixed length, therefore adding leading zeros to the angles and the sensor values solved the problem.

Lidar Scanner Schematic.png (36.47 KiB) Viewed 806 times

Parts
- Arduino Uno R3 USB Microcontroller
- LIDAR-Lite 3 Laser Rangefinder
- Lynxmotion Pan and Tilt Kit
- LCD Keypad Shield Arduino Compatible
- Cytron Arduino Proto Shield
- Wall Adapter Power Supply 7.5VDC 1A
- Electrolytic Capacitor Pack 1000uF/25V
- LM78M05CT 5V 0.5A Voltage Regulator
- 2.1mm Barrel Jack Adapter

LidarScanner.pde Code

Code: Select all

// LidarScanner.pde Processing sketch
// Load sketch into Processing, available from:
// https://processing.org/
// This sketch accepts XYZ coordinates from Arduino LIDAR scanner
// and displays them graphically as a 3D point cloud that you can
// pan, zoom, and rotate using keyboard.

import processing.serial.*;

Serial serial;
int serialPortNumber = 0;
float angle = 6.5f;
float angleIncrement = 0;
float xOffset = 3.0;
float xOffsetIncrement = 0;
float yOffset = 152.0f;
float yOffsetIncrement = 0;
float scale = 2.6f;
float scaleIncrement = 0;
ArrayList<PVector> vectors;
int lastPointIndex = 0;
int lastPointCount = 0;

void setup() {
  size(800, 600, P3D);
  colorMode(RGB, 255, 255, 255);
  noSmooth();
  vectors = new ArrayList<PVector>();
  String[] serialPorts = Serial.list();
  String serialPort = serialPorts[serialPortNumber];
  println("Using serial port \"" + serialPort + "\"");
  println("To use a different serial port, change serialPortNumber:");
  printArray(serialPorts);
  serial = new Serial(this, serialPort, 9600);
}

void draw() {
  String input = serial.readStringUntil(10);
  if (input != null) {
    String[] components = split(input, ' ');
    if (components.length == 3) {
      vectors.add(new PVector(float(components[0]), float(components[1]), float(components[2])));
    }
  }
  background(0);
  translate(width/2, height/2, -50);
  rotateY(angle);
  int size = vectors.size();
  for (int index = 0; index < size; index++) {
    PVector v = vectors.get(index);
    if (index == size - 1) {
      // draw red line to show recently added LIDAR scan point
      if (index == lastPointIndex) {
        lastPointCount++;
      } else {
        lastPointIndex = index;
        lastPointCount = 0;
      }
      if (lastPointCount < 10) {
        stroke(255, 0, 0);
        line(xOffset, yOffset, 0, v.x * scale + xOffset, -v.z * scale + yOffset, -v.y * scale);
      }
    }
    stroke(255, 255, 255);
    point(v.x * scale + xOffset, -v.z * scale + yOffset, -v.y * scale);
  }
  angle += angleIncrement;
  xOffset += xOffsetIncrement;
  yOffset += yOffsetIncrement;
  scale += scaleIncrement;
}

void keyPressed() {
  if (key == 'q') {
    // zoom in
    scaleIncrement = 0.02f;
  } else if (key == 'z') {
    // zoom out
    scaleIncrement = -0.02f;
  } else if (key == 'a') {
    // move left
    xOffsetIncrement = -1f;
  } else if (key == 'd') {
    // move right
    xOffsetIncrement = 1f;
  } else if (key == 'w') {
    // move up
    yOffsetIncrement = -1f;
  } else if (key == 's') {
    // move down
    yOffsetIncrement = 1f;
  } else if (key =='x') {
    // erase all points
    vectors.clear();
  } else if (key == CODED) {
    if (keyCode == LEFT) {
      // rotate left
      angleIncrement = -0.015f;
    } else if (keyCode == RIGHT) {
      // rotate right
      angleIncrement = 0.015f;
    }
  }
}

void keyReleased() {
  if (key == 'q') {
    scaleIncrement = 0f;
  } else if (key == 'z') {
    scaleIncrement = 0f;
  } else if (key == 'a') {
    xOffsetIncrement = 0f;
  } else if (key == 'd') {
    xOffsetIncrement = 0f;
  } else if (key == 'w') {
    yOffsetIncrement = 0f;
  } else if (key == 's') {
    yOffsetIncrement = 0f;
  } else if (key == CODED) {
    if (keyCode == LEFT) {
      angleIncrement = 0f;
    } else if (keyCode == RIGHT) {
      angleIncrement = 0f;
    }
  }
}

LidarScanner.ino Code

Code: Select all

// LidarScanner.ino Arduino sketch
// Load sketch into Arduino software, available from:
// https://www.arduino.cc/
// This sketch controls X and Y servos to pan/tilt a LIDAR detector,
// either manually (by pressing LCD buttons to control XY location),
// or automatically (scanning horizontally and vertically).
// XYZ coordinates are output to the serial port to be received and
// displayed on computer by LidarScanner.pde Processing sketch.
// This sketch requires library "LIDAR-Lite v3" by Garmin.
// Select menu "Sketch", "Include Library", "Manage Libraries...",
// and in textbox "Filter your search...", enter "lidar".
// Freetronics LCD shield uses D3 to control backlight brightness,
// but digital output needed for servo control, so disable the
// backlight control by cutting strap marked "D3" on LCD shield.

#include <Servo.h>
#include <LIDARLite.h>
#include <LiquidCrystal.h>

Servo servoX;
Servo servoY;
LIDARLite lidar;
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);

// Minimum and maximum servo angle in degrees
// Modify to avoid hitting limits imposed by pan/tilt bracket geometry
int minPosX = 0;
int maxPosX = 180;
int minPosY = 26;
int maxPosY = 125;

int buttonPin = A0;
int buttonValue = 0;
int buttonThreshold = 50;
int lastPosX = 0;
int lastPosY = 0;
int loopCount = 0;
int radius = 0;
int lastRadius = 0;
boolean selectButtonPressed = false;
boolean scanning = false;
boolean scanDirection = false;
int scanIncrement = 1;
int posX = (maxPosX + minPosX) / 2;
int posY = (maxPosY + minPosY) / 4;
float pi = 3.14159265;
float deg2rad = pi / 180.0;

void setup() {
  lcd.begin(16, 2);
  updateModeDisplay();
  lidar.begin(0, true);
  lidar.configure(0);
  servoX.attach(2);
  servoY.attach(3);
  servoX.write(posX);
  servoY.write(posY);
  Serial.begin(9600);
}

void loop() {

  buttonValue = analogRead(buttonPin);

  if (scanning) {
    if (abs(buttonValue - 741) < buttonThreshold) {
      if (!selectButtonPressed) {
        // switch to manual scan mode
        selectButtonPressed = true;
        scanning = false;
        updateModeDisplay();
      }
    } else {
      selectButtonPressed = false;
    }
    if (scanDirection) {
      posX += scanIncrement;
    } else {
      posX -= scanIncrement;
    }
    if (posX > maxPosX || posX < minPosX) {
      // hit limit X limit, reverse auto scan direction
      scanDirection = !scanDirection;
      posY += scanIncrement;
      if (posY > maxPosY) {
        // completed auto scan, return to manual scan mode
        scanning = false;
        updateModeDisplay();
      }
    }
  } else {
    if (abs(buttonValue - 741) < buttonThreshold) {
      if (!selectButtonPressed) {
        // switch to auto scan mode
        selectButtonPressed = true;
        scanning = true;
        posX = minPosX;
        posY = minPosY;
        scanDirection = true;
        updateModeDisplay();
      }
    } else if (abs(buttonValue - 505) < buttonThreshold) {
      // manual scan left
      posX += 1;
    } else if (abs(buttonValue - 329) < buttonThreshold) {
      // manual scan down
      posY -= 1;
    } else if (abs(buttonValue - 145) < buttonThreshold) {
      // manual scan up
      posY += 1;
    } else if (abs(buttonValue - 0) < buttonThreshold) {
      // manual scan right
      posX -= 1;
    } else {
      selectButtonPressed = false;
    }
  }

  posX = min(max(posX, minPosX), maxPosX);
  posY = min(max(posY, minPosY), maxPosY);
  bool moved = moveServos();
  displayPosition();

  loopCount += 1;
  if (loopCount % 100 == 0) {
    // recalibrate scanner every 100 scans
    radius = lidar.distance();
  } else {
    radius = lidar.distance(false);
  }
  if (abs(radius - lastRadius) > 2)
  {
    lastRadius = radius;
    lcd.setCursor(8, 0);
    lcd.print("D:" + String(radius / 100.0, 2) + "  ");
  }
  if (scanning || moved) {
    float azimuth = posX * deg2rad;
    float elevation = (180 - maxPosY + posY) * deg2rad;
    double x = radius * sin(elevation) * cos(azimuth);
    double y = radius * sin(elevation) * sin(azimuth);
    double z = radius * cos(elevation);
    Serial.println(String(-x, 5) + " " + String(y, 5) + " " + String(-z, 5));
  }
}

bool moveServos()
{
  bool moved = false;
  static int lastPosX;
  static int lastPosY;
  int delta = 0;  
  if (posX != lastPosX) {
    delta += abs(posX - lastPosX);
    servoX.write(posX);
    lastPosX = posX;
    moved = true;
  }
  if (posY != lastPosY) {
    delta += abs(posY - lastPosY);
    servoY.write(posY);
    lastPosY = posY;
    moved = true;
  }
  delay(30);
  return moved;
}

void displayPosition()
{
  static int lastPosX;
  static int lastPosY;
  if (posX != lastPosX) {
    lcd.setCursor(0, 0);
    lcd.print("X:" + String(posX) + "  ");
    lastPosX = posX;
  }
  if (posY != lastPosY) {
    lcd.setCursor(0, 1);
    lcd.print("Y:" + String(posY) + "  ");
    lastPosY = posY;
  }  
}

void updateModeDisplay()
{
  lcd.setCursor(8, 1);
  if (scanning) {
    lcd.print("Scanning");
  } else {
    lcd.print("Manual  ");
  }
}

This 3D scanner uses a Garmin LIDAR-Lite rangefinder to produce a point cloud representation of the surrounding area.

I hope you like this guys. It takes one week to make one. So far all good, you will surely enjoy it.