# 3D room map by TFmini Lidar

**URL:** <https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709>\
**Category:** Electronics (Arduino, etc.)\
**Created:** [July 18, 2020, 5:31pm UTC](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709 "2020-07-18T17:31:15Z")\
**Posts on this page:** 7\
**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:31pm UTC](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709/1 "2020-07-18T17:31:15Z")

</div>

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:

```auto
#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 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 Processing code:

```auto
// *******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<PVector> pointList;
final int serialSpeed = 115200;
void setup(){
  size(800,640,P3D);
  colorMode(RGB,255,255,255);
  pointList = new ArrayList<PVector>();
  //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();
      }
  }
}

```

---

<div class="post-metadata">

**Author:** ![RichardDL](https://avatars.discourse-cdn.com/v4/letter/r/f475e1/32.png) [@RichardDL](https://discourse.processing.org/u/RichardDL)\
**Post date:** [July 18, 2020, 7:31pm UTC](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709/2 "2020-07-18T19:31:48Z")

</div>

Is this your third topic on the same subject?

---

<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, 7:46pm UTC](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709/3 "2020-07-18T19:46:37Z")

</div>

Yes. Sorry for the mess , I’ve received a guide how to poss on forum. I tried to delete other topics but I can not delete them so I put flags in order to recognize the alerts. Can you help me with simulation, I don’t know what is missing there

---

<div class="post-metadata">

**Author:** ![RichardDL](https://avatars.discourse-cdn.com/v4/letter/r/f475e1/32.png) [@RichardDL](https://discourse.processing.org/u/RichardDL)\
**Post date:** [July 18, 2020, 10:05pm UTC](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709/4 "2020-07-18T22:05:27Z")

</div>

You haven’t replied to me on your first topic, or @glv on the 2nd.Suggest you continue the conversation with glv and ingore the other two.

---

<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, 1:31pm UTC](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709/5 "2020-07-19T13:31:51Z")

</div>

> [@eladsoll](#):
>
> The simulation is not good at all. I need help.

Please elaborate.

You are not sending any data on Serial; it is commented:

```auto
// Serial.println(String(x, 5) + " " + String(y, 5) + " " + String(z, 5));        
// Serial.println(liDARval);//debug

```

I do not have this hardware so wrote some code to send known data for x, y, z data.  
I knew what the data was that I was sending and expected to plot this on Processing side.

On the processing side I was able to plot this my cod (sending known data) and receiving with your code:

 ![image](https://canada1.discourse-cdn.com/flex036/uploads/processingfoundation1/original/2X/3/3f0ff06822eb5bc0eb05860474d0b7a9e067198f.png)

I just plotted points in the Processing side and removed some of your default camera rotations and increased strokeWeight().

`:)`

---

<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 19, 2020, 2:00pm UTC](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709/6 "2020-07-19T14:00:23Z")

</div>

Thank’s for the quick response!  
The problem is when I remove the print serial comment as you mentioned , the lidar does not give me proper area recognition , and moreover the point recevied as circle shape , not linear. Is there any thing I can do inorder to improve object recognition by Processing code?

---

<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, 2:26pm UTC](https://discourse.processing.org/t/3d-room-map-by-tfmini-lidar/22709/7 "2020-07-19T14:26:11Z")

</div>

Hello,

You must send data to Processing to be able to plot it.  
I suggest you _simplify your code_ and build on that.

Add some test prints here and there to see what your code is doing.

This is the test data I am sending:

> **Test Data**
>
> ```auto
> float x, y, z;
> int counter;
> unsigned long tfminiCurrentTime, tfminiPreviousTime;
> 
> //Simulated data
> float theta;
> void setup() 
> {
> Serial.println(Serial);
>   
> //TFmini initializing:
> Serial.begin(115200);
>   
> // wait for serial port to connect. Needed for native USB port only
> while(!Serial);
> }
> 
> void loop() 
> {  
> if((millis()-tfminiPreviousTime) >= 100)
> {
> theta = counter*(TWO_PI/100);
> counter++;
>     
> x = sin(theta);
> y = cos(theta);
> z = counter;
> 
> // Serial.print(x); Serial.print(',');
> // Serial.print(y); Serial.print(',');
> // Serial.println(z);
>     
> Serial.println(String(x, 5) + " " + String(y, 5) + " " + String(z, 5));        
> //Serial.println(liDARval);//debug
> tfminiPreviousTime = millis();
> //Serial.println(counter);
> }  
> }  
> 
> ```

> [@eladsoll](#):
>
> moreover the point recevied as circle shape , not linear

> [@glv](#):
>
> I do not have this hardware so wrote some code to send known data for x, y, z data.

`:)`