¶ Lidar Garmin
 |
 |
¶ Arduino
#include <Servo.h>
#include <Wire.h>
#include <LIDARLite.h>
//Delay between each sample to avoid mechanical wobble
#define DELAY_BETWEEN_SAMPLES 100
//Size of the steps of YAW/PITCH in degrees (1 = full res)
#define YAW_STEP 2
#define PITCH_STEP 2
#define MATH_PI 3.1415f
#define zOffset 10
//Variables
LIDARLite myLidarLite;
Servo servoYaw,servoPitch;
char s[15];
int yawAngle,pitchAngle;
int x,y,z,r;
float theta,phi;
void setup() {
// Initialize serial connection to display distance readings
Serial.begin(115200);
//Servo init
servoYaw.attach(2);
servoPitch.attach(3);
//Lidar Lite v3 init
myLidarLite.begin(0, true);
myLidarLite.configure(0);
}
void loop() {
delay(5000);
//Sweep Yaw servomotor
for (yawAngle = 0; yawAngle <= 110; yawAngle += YAW_STEP) {
servoYaw.write(yawAngle);
//Sweep Pitch servomotor. The direction depends on the current directory
if(pitchAngle < 90){
for (pitchAngle = 0; pitchAngle <= 180;pitchAngle+= PITCH_STEP){
servoPitch.write(pitchAngle);
sendMeasurement();
}
} else {
for (pitchAngle = 180; pitchAngle >= 0;pitchAngle-= PITCH_STEP){
servoPitch.write(pitchAngle);
sendMeasurement();
}
}
}
}
// Function to acquire, convert and send the measurement.
void sendMeasurement(){
delay(DELAY_BETWEEN_SAMPLES);
// Get spherical coordinates
r = myLidarLite.distance(false)+10;
theta = (90-(float)yawAngle) * (PI / 180.0f);
phi = (90-(float)pitchAngle) * (PI / 180.0f);
// Convert and send them
sprintf(s,"%d %d %d\n\0",(int)(r*cos(phi)*cos(theta)),(int)(r*cos(phi)*sin(theta)),(int)(r*sin(phi)));
Serial.print(s);
}
¶ Processing
// LidarViewer.pde Processing sketch
// Press 's' to save the cloud as a text file in Processing directory.
import processing.serial.*;
import peasy.*;
import java.io.FileWriter;
import java.util.Calendar;
import java.text.SimpleDateFormat;
import static javax.swing.JOptionPane.*;
Serial serial;
PeasyCam cam;
final float angleIncrement=0.1f;
ArrayList<PVector> pointList;
final int SERIAL_SPEED = 115200;
void setup() {
size(1024, 768, P3D);
colorMode(RGB, 255, 255, 255);
pointList = new ArrayList<PVector>();
// PeasyCam
cam = new PeasyCam(this, 1024);
cam.rotateZ(-3.1415/4);
cam.rotateX(-3.1415/4);
// Serial Port (added dialog)
try {
if (Serial.list().length == 0) {
println("No serial device connected");
exit();
}
else if (Serial.list().length == 1) {
// only one device, select it
serial = new Serial(this, Serial.list()[0], SERIAL_SPEED);
}
else {
// more than 1, show dialog
StringBuffer ttyList = new StringBuffer();
for (int j = 0; j < Serial.list().length; j++)
ttyList.append("\n" + Serial.list()[j]);
String selection = showInputDialog("Enter serial port to use:"+ ttyList.toString());
if (selection == null || selection.isEmpty())
exit();
serial = new Serial(this, selection, SERIAL_SPEED); // change baud rate to your liking
}
}
catch (Exception e) {
println("Not able to connect to serialPort (error:"+e.getClass().getName()+" " + e.getMessage() + ")");
exit();
}
}
void draw() {
// Prepare window & drawing
perspective();
background(33);
stroke(255,255,255);
sphere(0.5f);
fill(50);
ellipse(0, 0, 10, 10);
//Read Serial Port (if we can)
String serialString = serial.readStringUntil('\n');
if (serialString != null) {
String[] coordinates = split(serialString, ' ');
if (coordinates.length == 3) {
pointList.add(new PVector(float(coordinates[0]), float(coordinates[1]), float(coordinates[2])));
}
}
// Draw the actual point cloud
for (int index = 0; index < pointList.size(); index++) {
PVector v = pointList.get(index);
if (index == pointList.size() - 1) {
// Draw a line between Lidar and last point
stroke(255, 15, 15);
line(0, 0, 0, v.x,v.y,v.z);
}
// Draw point with a variable color
stroke(255-v.z, 255-v.y, 255-v.x);
point(v.x, v.y, v.z);
}
}
// Handle keyboard events : movement, save & clear all points.
void keyReleased() {
if (key =='x') {
// erase all points
pointList.clear();
} else if(key == 's'){
saveToFile();
}
else if (key == CODED) {
if (keyCode == UP) {
cam.rotateX(angleIncrement);
} else if (keyCode == DOWN) {
cam.rotateX(-angleIncrement);
}else if (keyCode == LEFT) {
cam.rotateY(angleIncrement);
}else if (keyCode == RIGHT) {
cam.rotateY(-angleIncrement);
}
}
}
// Function to save the point cloud in the Processing install directory
void saveToFile(){
String fileName = "./points_"+
new SimpleDateFormat("yyMMdd_HHmmss").format(Calendar.getInstance().getTime())+".xyz";
PrintWriter pw = null;
try{
pw = new PrintWriter(new FileWriter(fileName,true));
for(int i=0;i<pointList.size();i++)
pw.println((int)pointList.get(i).x + " " +
(int)pointList.get(i).y + " " +
(int)pointList.get(i).z);
}
catch(Exception e){
}
finally {
if(pw != null) pw.close();
}
}
lidar_lite_v3_operation_manual_and_technical_specifications.pdf
¶ Sources
https://www.qcontinuum.org/lidar-scanner
http://charleslabs.fr/en/project-3D+Lidar+Scanner+MK2