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