This was my final project for my Intro to Photonics class. It’s a lidar 3D scanner, based off an Arduino, a TFT display, a servo, and laser distance sensor. The servo scans through the room recording spherical coordinates, I then convert those measurements to cylindrical, and “unfurl it”. This is by taking into account the distance along the cylinder formed at that radius. Every projection method has its flaws but this is the best I figured out. The dots are colored by distance. In the photo above you can see the silhouette of a person who was recorded by the system.
#include <Elegoo_TFTLCD.h>
#include <Elegoo_GFX.h>
#include <TouchScreen.h> //TFT
#include <Wire.h>
#include <VL53L1X.h> //LIDAR
#include <Servo.h> //Servo
#include <EasyColor.h>
//libraries ^^^
#define BLACK 0x0000
#define BLUE 0x001F
#define RED 0xF800
#define GREEN 0x07E0
#define CYAN 0x07FF
#define MAGENTA 0xF81F
#define YELLOW 0xFFE0
#define WHITE 0xFFFF
#define GRAY 0x8410
#define ORANGE 0xFD00
//base colors ^^^
#define TS_MINX 120
#define TS_MINY 80
#define TS_MAXX 920
#define TS_MAXY 905
#define XP 8
#define YP A3
#define XM A2
#define YM 9
Elegoo_TFTLCD tft(A3, A2, A1, A0, A4);
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 364);
//touchscreen stuff ^^^
float countRate = 0.0;
int rangeStatus = 0;
unsigned long StartT;
unsigned long currentT; //vars
VL53L1X sensor;
Servo servo1;
Servo servo2; //initalize
int servo_1_PIN = 44; // Horizontal Rotation
int servo_2_PIN = 45; // Vertical Rotation
int yawdiv = 2;
int pitchdiv = 2;
EasyColor::HSVRGB HSVConverter;
void setup() {
Serial.begin(9600); //starts serial
Wire.begin();
Wire.setClock(400000); //starts wire
servo1.attach(servo_1_PIN);
servo2.attach(servo_2_PIN);
sensor.setTimeout(500);
if (!sensor.init()) {
Serial.println("Failed to detect and initialize sensor!");
while (1);
}
sensor.setDistanceMode(VL53L1X::Long);
sensor.setMeasurementTimingBudget(50000);
sensor.startContinuous(50); //stars LIDAR
StartT = millis(); //count in milliseconds
tft.reset();
tft.begin(0x9341);
tft.setRotation(3);
UI();
} //end up setup
void loop() {
//wait for button to be pressed to take an image
for (int yaw = -45; yaw <= 45; yaw = yaw + yawdiv) {
for (int pitch = 0; pitch <= 60; pitch = pitch + pitchdiv) {
servo1.write(90 + yaw); //horizontal
servo2.write(180 - pitch); //vertical
delay(50); //wait for servo to respond
float LIDARdst = getLIDAR(); //gets lidar distance
if (LIDARdst == 0) { //skips 0 values
continue;
}
//float metersx = LIDARdst * sin(degtorad(90 - pitch)) * cos(degtorad(yaw)); //depth
//float metersy = LIDARdst * sin(degtorad(90 - pitch)) * sin(degtorad(yaw)); //side
//float metersz = LIDARdst * cos(degtorad(90 - pitch)); //up
//spherical to cartesian
//float metersradius = LIDARdst * sin(degtorad(90 - pitch));
//float theta = yaw;
//float height = LIDARdst * cos(degtorad(90 - pitch));
//spherical to cylinderical
float metersx = LIDARdst * sin(degtorad(90 - pitch)); //depth
float metersz = LIDARdst * cos(degtorad(90 - pitch)); //up
float metersy = LIDARdst * sin(degtorad(90 - pitch)) * degtorad(yaw); //side
//spherical to plane (map-projection)
// Serial.print(" Yaw: ");
// Serial.print(yaw);
// Serial.print(" Pitch: ");
// Serial.print(pitch);
// Serial.print(" Distance: ");
// Serial.print(LIDARdst, 3);
// Serial.print(" metersx: ");
// Serial.print(metersx, 3);
// Serial.print(" metersy: ");
// Serial.print(metersy, 3);
// Serial.print(" metersz: ");
// Serial.println(metersz, 3); //debug
displayIMG(metersx, metersy, metersz); //plots on display
}
}
exit(0);
} //end of loop
//Functions woooooo \/\/\/\/\/\/\/\/\/\/
int flip(int x) {
int flipped = 320 - x;
return flipped;
}
void UI() {
tft.fillScreen(BLACK); //initalizes tft display
tft.drawRect(40, 0, 240, 240, WHITE); //image box
tft.drawRect(295, 41, 20, 160, WHITE); //color box
for (int i = 41; i <= 200; i = i + 40) {
tft.drawLine(295, i, 288, i, WHITE);
}
tft.drawLine(295, 200, 288, 200, WHITE); //big graduations
for (int i = 41; i <= 200; i = i + 20) {
tft.drawLine(295, i, 291, i, WHITE);
}
for (int i = 41; i <= 200; i = i + 10) {
tft.drawLine(295, i, 293, i, WHITE);
}
tft.drawRect(10, 10, 3, 3, RED);
tft.drawRect(20, 10, 3, 3, BLUE);
tft.drawRect(10, 20, 3, 3, GREEN);
tft.setCursor(300, 205);
tft.setTextColor(WHITE);
tft.setTextSize(1);
tft.println("0m");
tft.setCursor(300, 30);
tft.setTextColor(WHITE);
tft.setTextSize(1);
tft.println("4m");
for (int i = 199; i >= 42; i--) {
int colordepth = map(i, 199, 42, 0, 300);
hsv in_hsv;
in_hsv.h = colordepth;
in_hsv.s = 100;
in_hsv.v = 100;
rgb out_rgb;
out_rgb.r;
out_rgb.g;
out_rgb.b;
out_rgb = HSVConverter.HSVtoRGB(in_hsv, out_rgb);
tft.drawLine(296, i, 313, i, tft.color565(out_rgb.r, out_rgb.g, out_rgb.b));
}
}
float degtorad(float degree) {
float radian = degree * (PI/180);
return radian;
}
float getLIDAR() {
currentT = millis();
sensor.read();
int distanceMM = sensor.ranging_data.range_mm;
//Serial.println(distanceMM);
countRate = sensor.ranging_data.peak_signal_count_rate_MCPS;
rangeStatus = sensor.ranging_data.range_status;
float distanceM = distanceMM * 0.001;
//Serial.println(distanceM, 6);
if (VL53L1X::rangeStatusToString(rangeStatus)=="wrap target fail"){
return 0;
}
else {
return distanceM;
}
} //end of getLIDAR
void displayIMG(float xloc, float yloc, float zloc) { //adds a pixel 320x240pixels orgin: (160, 120)
float yPix = 160 + (60 * yloc);
float zPix = 10 + (60 * zloc);
int yPix_mapped = yPix;
int zPix_mapped = zPix;
Serial.print("yPix: ");
Serial.print(yPix_mapped);
Serial.print(" zPix: ");
Serial.print(zPix_mapped); //debug
//function for color aquistion
float colordepth = (xloc - 0) * (300 - 0) / (4 - 0) + 0; //map = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
int colordepth_int = colordepth;
Serial.print(" xloc: ");
Serial.print(xloc, 6); //debug
Serial.print(" xPix: ");
Serial.println(colordepth_int);
hsv in_hsv;
in_hsv.h = colordepth_int;
in_hsv.s = 100;
in_hsv.v = 100;
rgb out_rgb;
out_rgb.r;
out_rgb.g;
out_rgb.b;
out_rgb = HSVConverter.HSVtoRGB(in_hsv, out_rgb);
if (yPix_mapped >= 41 && yPix_mapped <= 279 && zPix_mapped >= 1 && zPix_mapped <= 239) {
tft.drawPixel(yPix_mapped, 240 - zPix_mapped, tft.color565(out_rgb.r, out_rgb.g, out_rgb.b));
}
} //end of displayIMG