3D room map by TFmini Lidar

Processing Electronics (Arduino, etc.)
1

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 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:

//*******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();
      }
  }
}
2

Is this your third topic on the same subject?

3

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

4

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.

5

Please elaborate.

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

//      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
image802×667 12.1 KB

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

:)

6

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?

7

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 
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);
    }  
  }

:)