# 3D simulation by TFmini lidar

**URL:** <https://discourse.processing.org/t/3d-simulation-by-tfmini-lidar/22708>\
**Category:** Electronics (Arduino, etc.)\
**Created:** [July 18, 2020, 5:04pm UTC](https://discourse.processing.org/t/3d-simulation-by-tfmini-lidar/22708 "2020-07-18T17:04:44Z")\
**Posts on this page:** 3\
**Page:** 1

<div class="post-metadata">

**Author:** ![eladsoll](https://avatars.discourse-cdn.com/v4/letter/e/f05b48/32.png) [@eladsoll](https://discourse.processing.org/u/eladsoll)\
**Post date:** [July 18, 2020, 5:04pm UTC](https://discourse.processing.org/t/3d-simulation-by-tfmini-lidar/22708/1 "2020-07-18T17:04:44Z")

</div>

Hi, I’m a beginner , I’m trying to simulate 3D room map by TFmini LIDAR.  
I connected the sensor to arduino mega 2560 to communication pins. Moreover, I control 2 servo’s inorder to scan the room.  
This is my arduino code:  
\<  
#include \<Servo.h\>  
#include \<arduino.h\>  
#include “TeensyThreads.h”  
Servo yaw, pitch;  
volatile int liDARval;  
char s[15];  
double x, y, z;  
float theta, phi;  
#define MATH\_PI 3.1415f  
#define msec 10  
unsigned int pitchPos, yawPos;  
unsigned int flag;  
unsigned long yawCurrentTime, yawPreviousTime, pitchCurrentTime, pitchPreviousTime;  
unsigned long tfminiCurrentTime, tfminiPreviousTime;  
void setup() {  
//Initialize variables:  
flag = 1;  
liDARval = 0;  
yawPos = 0;  
pitchPos = 0;  
updateTimers();  
initLeds();  
TurnOffLeds();  
initServos();  
//TFmini initializing:  
Serial.begin(115200);  
// wait for serial port to connect. Needed for native USB port only  
while(!Serial);  
Serial1.begin(115200); // HW Serial for TFmini  
// Set to Standard Output mode  
initSerial1();  
}  
void loop() {

updateTimers();  
//TFmini LIDAR Measurement:  
if(pitchPos == 70){  
yaw.detach();  
pitch.detach();  
flag = 0;  
redLed();  
}  
else{  
if(flag==1){  
if(readLIDAR()==true){  
greenLed();//Serial communication is available.  
}  
else if(readLIDAR()==false){  
yellowLed();//If Serial communication isn’t avavilable, turn-on yellow led.  
}  
}  
}  
if(yawPos\<160){  
yawCCW();  
}  
if(yawPos == 160){  
pitchCCW();  
yawPos = 0;  
}  
}  
void initSerial1(){  
Serial1.write(0x42);  
Serial1.write(0x57);  
Serial1.write(0x02);  
Serial1.write(0x00);  
Serial1.write(0x00);  
Serial1.write(0x00);  
Serial1.write(0x01);  
Serial1.write(0x06);  
}  
//-------LIDAR function---------------  
boolean readLIDAR(){  
// Data Format for Benewake TFmini  
// ===============================  
// 9 bytes total per message:  
// 1) 0x59  
// 2) 0x59  
// 3) Dist\_L (low 8bit)  
// 4) Dist\_H (high 8bit)  
// 5) Strength\_L (low 8bit)  
// 6) Strength\_H (high 8bit)  
// 7) Reserved bytes  
// 8) Original signal quality degree  
// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +…+Byte8. This is only a low 8bit though  
while(Serial1.available()\>=9) // When at least 9 bytes of data available (expected number of bytes for 1 signal), then read  
{  
if((0x59 == Serial1.read()) && (0x59 == Serial1.read())) // byte 1 and byte 2  
{  
unsigned int t1 = Serial1.read(); // byte 3 = Dist\_L  
unsigned int t2 = Serial1.read(); // byte 4 = Dist\_H  
t2 \<\<= 8;  
t2 += t1;  
liDARval = t2;  
t1 = Serial1.read(); // byte 5 = Strength\_L  
t2 = Serial1.read(); // byte 6 = Strength\_H  
t2 \<\<= 8;  
t2 += t1;  
for(int i=0; i\<3; i++)Serial1.read(); // byte 7, 8, 9 are ignored  
}  
}  
theta = (float)yawPos \* PI / 180.0f;  
phi = (float)pitchPos \* PI / 180.0f;  
x = liDARval \* sin(phi) \* sin(theta);  
z = liDARval \* sin(theta) \* cos(phi);  
y = liDARval \* cos(theta);  
if((tfminiCurrentTime-tfminiPreviousTime) \>= 10){ // Don’t want to read too often as TFmini samples at 100Hz  
// Serial.println(String(x, 5) + " " + String(y, 5) + " " + String(z, 5));  
// Serial.println(liDARval);//debug  
tfminiPreviousTime = tfminiCurrentTime;  
}  
return true;  
}  
//-------Servo function--------  
void initServos(){  
//Servos pin assigment:  
pitch.attach(9);  
yaw.attach(8);  
//Servos initializing:  
yaw.write(0);  
pitch.write(0);  
}  
void pitchCW() {  
if ((pitchCurrentTime - pitchPreviousTime) \>= msec) {  
pitch.write(360 - (pitchPos += 1));  
pitchPreviousTime = pitchCurrentTime;  
}  
}  
void pitchCCW() {  
if ((pitchCurrentTime - pitchPreviousTime) \>= msec) {  
pitch.write(pitchPos += 1);  
pitchPreviousTime = pitchCurrentTime;  
}  
}  
void yawCW() {  
if ((yawCurrentTime - yawPreviousTime) \>= msec) {  
yaw.write(360 - (yawPos += 1));  
yawPreviousTime = yawCurrentTime;  
}  
}  
void yawCCW() {  
if ((yawCurrentTime - yawPreviousTime) \>= msec) {  
yaw.write((yawPos += 1));  
yawPreviousTime = yawCurrentTime;  
}  
}  
//-------Led functions--------------  
void initLeds(){  
pinMode(24, OUTPUT); //red  
pinMode(26, OUTPUT); //yellow  
pinMode(28, OUTPUT); //green  
}  
void TurnOffLeds() {  
digitalWrite(28, LOW);  
digitalWrite(26, LOW);  
digitalWrite(24, LOW);  
}  
void greenLed() {  
digitalWrite(28, HIGH);  
digitalWrite(26, LOW);  
digitalWrite(24, LOW);  
}  
void redLed() {  
digitalWrite(24, HIGH);  
digitalWrite(26, LOW);  
digitalWrite(28, LOW);  
}  
void yellowLed() {  
digitalWrite(26, HIGH);  
digitalWrite(28, LOW);  
digitalWrite(24, LOW);  
}  
//-----Timers function-----------  
void updateTimers(){  
if(flag==0){  
yawCurrentTime = 0;  
yawPreviousTime = 0;  
pitchCurrentTime = 0;  
pitchPreviousTime = 0;  
tfminiCurrentTime = 0;  
tfminiPreviousTime = 0;  
flag = 1;  
}  
if(flag==1){  
yawCurrentTime = millis();  
pitchCurrentTime = millis();  
tfminiCurrentTime = millis();  
}  
}

> 

My simulation isn’t good enough!  
This is my Processing code:  
\<  
//_ **Libraries** _\*\*\*\*\*\*\*\*\*\*\*  
import processing.serial._;  
import peasy._;  
import java.io.FileWriter;  
import java.util.Calendar;  
import java.text.SimpleDateFormat;  
import static javax.swing.JOptionPane.\*;  
//Objects:  
Serial serial;  
PeasyCam cam;  
final float angleIncrement = 0.1f;  
ArrayList pointList;  
final int serialSpeed = 115200;  
void setup(){  
size(800,640,P3D);  
colorMode(RGB,255,255,255);  
pointList = new ArrayList();  
//PeasyCam  
cam = new PeasyCam(this,800);  
cam.rotateZ(-3.1415/4);  
cam.rotateX(-3.1415/4);  
//Serial Port  
try{  
if(Serial.list().length == 0){  
println(“No serial device connected”);  
}  
else  
serial = new Serial(this,Serial.list()[0],serialSpeed);  
}  
catch(Exception error){  
println(“Not able to connect to serialPort (error:”+error.getClass().getName()+" " + error.getMessage() + “)”);  
exit();  
}  
}  
void draw(){  
perspective();  
background(33);  
stroke(255,255,255);  
//sphere(0.5f);  
fill(50);  
//ellipse(0,0,10,10);  
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 point cloud  
for(int idx = 0; idx \< pointList.size(); idx++){  
PVector v = pointList.get(idx);  
if(idx == pointList.size() -1){  
stroke(255,15,15);  
line(0, 0, 0, v.x, v.y, v.z);  
}  
stroke(255-v.z, 255-v.y, 255-v.x);  
point(v.x, v.y, v.z);  
}  
}  
void keyRealeased(){  
if(key == ‘c’){  
pointList.clear();  
}  
else if(key == ‘s’){  
saveToFile();  
}  
else if(key == CODED){  
if(keyCode == UP){  
cam.rotateX(angleIncrement);  
}  
else if(key == DOWN){  
cam.rotateX(-angleIncrement);  
}  
else if(key == LEFT){  
cam.rotateY(angleIncrement);  
}  
else if(key == RIGHT){  
cam.rotateY(-angleIncrement);  
}  
}  
}  
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 idx = 0; idx \< pointList.size(); idx++)  
pw.println((int)pointList.get(idx).x +" “+  
(int)pointList.get(idx).y +” "+  
(int)pointList.get(idx).z);  
}  
catch(Exception error){  
}  
finally{  
if(pw != null){  
pw.close();  
}  
}  
}

> 

Please Help  
Elad

---

<div class="post-metadata">

**Author:** ![glv](https://yyz2.discourse-cdn.com/flex036/user_avatar/discourse.processing.org/glv/32/18785_2.png) [@glv](https://discourse.processing.org/u/glv)\
**Post date:** [July 18, 2020, 5:08pm UTC](https://discourse.processing.org/t/3d-simulation-by-tfmini-lidar/22708/2 "2020-07-18T17:08:48Z")

</div>

Hello,

Please format your code:  
[https://discourse.processing.org/faq#format-your-code](https://discourse.processing.org/faq#format-your-code)

Please be very clear about what you are asking for help with.

`:)`

---

<div class="post-metadata">

**Author:** ![glv](https://yyz2.discourse-cdn.com/flex036/user_avatar/discourse.processing.org/glv/32/18785_2.png) [@glv](https://discourse.processing.org/u/glv)\
**Post date:** [July 19, 2020, 11:38am UTC](https://discourse.processing.org/t/3d-simulation-by-tfmini-lidar/22708/3 "2020-07-19T11:38:52Z")

</div>

The original poster formatted code and continued here:

> [@3D room map by TFmini Lidar](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709):
>
> Hi, I’m a beginner so I hope this time I worte as should. I’m trying to simulate room with TFmini lidar by using arduino Mega with serial port communication. The simulation is not good at all. I need help. My Arduino code: #include \<Servo.h\> #include \<arduino.h\>\<br\>#include "TeensyThreads.h" Servo yaw, pitch; volatile int liDARval; char s[15]; double x, y, z; float theta, phi; #define MATH\_PI 3.1415f #define msec 10 unsigned int pitchPos, yawPos; unsigned int flag; unsigned long yawCurrentT…

Please continue there…