PROJECT:
I am building a short-range Lidar for use on an autonomous vehicle, it utilizes:
2 - Adafruit VL53L0X (Time Of Flight Sensors)
1 – Nema 17 Stepper Motor (200 Steps)
1 - Adafruit TB6612 Motor Driver
1 – Teensy 3.2 (Micro Controller)
1 – Raspberry Pi 3b (RPi)
SET UP:
I have mounted 2- Adafruit VL53L0X TOF sensors back to back on the Nema 17 Stepper motor shaft, The Stepper motor is wired to the TB6612 Motor Driver, which in turn has its control inputs wired to the teensy 3.2 GPIO pins (9, 10, 11, 12). The VL53L0X TOF sensors are also wired to the Teensy 3.2 i2c GPIO pins, the teensy 3.2 is connected to the RPi through its USB port, utilizing a USB cable only capable of data transfer this connection is utilized for sending TOF readings and nothing else. The RPi utilizing its GPIO pins controls the Teensy 3.2, the RPi can turn the Teensy 3.2 on and off, Start/Stop the Stepper from rotating, and Start/Stop the VL53L0X TOF from taking measurements.
LIDAR SETUP:
Currently the compete Lidar (Stepper + 2 VL53L0X TOF) rotates 100 steps clockwise (180 Degrees), which allows the front and back VL53L0X TOF Sensors to scan a combined 360 Degrees.
PROBLEM:
At the moment I am testing just the “Front” VL53L0X TOF Sensor which is responsible for 0 Degrees – 180 Degrees, I am testing the Lidar in a square box, I have run numerous tests, and trial code however I am unable to Map ½ of the square box, the map displays with rounded corners. I Believe it’s due to Code issues, I have not been able to sync 2 threads, 1 Thread is responsible for rotating the Stepper, the second takes the measurements from the VL53L0X TOF Sensor. Currently it takes 748 Millis to rotate the stepper 100 steps, and I have managed to get the measurement code to take 730 Millis. This is not close enough to create an accurate map.
I have tried to combine the stepper code in the same thread as the measurement code, however this causes the stepper to “stutter” and run in a “jittery” fashion.
What I need to happen is every step a measurement is taken, I was thinking a state-machine type setup with alternating a Boolean true/false before updating the ranges variable? Any suggestions as to how to either sync these threads exactly, or utilize a state-machine(ish) type setup?
#include <Stepper.h>
#include <VL53L0X.h>
#include <Wire.h>
#include <ros.h>
#include <sensor_msgs/LaserScan.h>
#include "TeensyThreads.h"
// change this to the number of steps on your motor
#define STEPS 100
#define HIGH_SPEED
VL53L0X frontTOF;
VL53L0X rearTOF;
ros::NodeHandle nh;
// Laser Scan
sensor_msgs::LaserScan lidar_msg;
ros::Publisher lidar_pub("/laser_scan", &lidar_msg);
float ranges[100] = {0};
int p = 0;
int id1; // thread IDS
bool begin_rotate = false; // Bool for rotate control
bool begin_measure = false; // Bool for measurement control
bool take_measure = true;
float angle_min;
float angle_max;
const float Pi = 3.14159; // PI Constant
float deg = 0; // deg LIDAR is currently at
Stepper stepper(STEPS, 9, 10, 11, 12);
void setup(){
Wire.begin();
pinMode(0, OUTPUT);
digitalWrite(0, LOW);
pinMode(23, OUTPUT);
digitalWrite(23, LOW);
#if defined LONG_RANGE
// lower the return signal rate limit (default is 0.25 MCPS)
frontTOF.setSignalRateLimit(0.1);
// increase laser pulse periods (defaults are 14 and 10 PCLKs)
frontTOF.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
frontTOF.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
#endif
#if defined HIGH_SPEED
// reduce timing budget to 20 ms (default is about 33 ms)
frontTOF.setMeasurementTimingBudget(20000);
#elif defined HIGH_ACCURACY
// increase timing budget to 200 ms
frontTOF.setMeasurementTimingBudget(200000);
#endif
pinMode(0, INPUT);
frontTOF.init(true);
frontTOF.setAddress((uint8_t)22);
frontTOF.setTimeout(500);
pinMode(23, INPUT);
rearTOF.init(true);
rearTOF.setAddress((uint8_t)25);
rearTOF.setTimeout(500);
pinMode(3,INPUT); // Initialize LIDAR rotation control pin
attachInterrupt(digitalPinToInterrupt(3), control_rotate, RISING);
pinMode(4,INPUT); // Initialize LIDAR measure control pin
attachInterrupt(digitalPinToInterrupt(4), control_measure, RISING);
// Initialize ROS node handle, advertise and subscribe the topics
nh.initNode();
nh.getHardware()->setBaud(112500);
nh.advertise(lidar_pub);
// set the speed of the motor to 30 RPMs
stepper.setSpeed(130);
// Initialize LIDAR rotation thread and suspend
std::thread th_rotate(rotate_stepper);
th_rotate.detach();
id1 = th_rotate.get_id();
threads.suspend(id1);
}
void rotate_stepper(){
int i = 0;
do {
// Rotate stepper 180 degrees clockwinse
while(i <= 100){
stepper.step(1);
// Determine rotation angle in Radians(Rads)
angle_min = deg*Pi/180;
angle_max = ((deg+1.8)*Pi/180);
deg = deg + 1.8;
++i;
}
// Rotate stepper 180 degrees counterclockwise
while(i >= 0){
stepper.step(-1);
// Determine rotation angle in Radians(Rads)
angle_max = deg*Pi/180;
angle_min = ((deg-1.8)*Pi/180);
// Ensure roatation angle never reads below 0.0
if(deg >= 0) { deg = deg - 1.8;}
else { deg = 0; }
--i;
}
} while(1);
}
void loop(){
while(begin_measure == true) {
p = 0;
lidar_msg.header.frame_id = "laser_frame";
lidar_msg.angle_min = 0.0;
lidar_msg.angle_max = 3.14159;
lidar_msg.angle_increment = 0.0314159;
lidar_msg.range_min = 0.05; // minimum range value [m]
lidar_msg.range_max = 1.2; // maximum range value [m]
unsigned long scan_start = millis();
while(p <= 99) {
if(take_measure == true) {
ranges[p] = (float)frontTOF.readRangeSingleMillimeters()/1000.0;
++p;
}
}
lidar_msg.scan_time = (millis()-scan_start)/1000.0;
lidar_msg.ranges_length = p;
lidar_msg.ranges = ranges;
lidar_msg.header.stamp = nh.now();
lidar_pub.publish(&lidar_msg);
nh.spinOnce();
}
}
void control_rotate(){
// Begin or cease LIDAR rotation depending on pin state
if(begin_rotate == false) {
threads.restart(id1);
begin_rotate = true;
}
else {
threads.suspend(id1);
begin_rotate = false;
}
}
void control_measure(){
if(begin_measure == false) {
begin_measure = true;
}
else {
begin_measure = false;
}
}
VISUAL:
This should resemble 1/2 a box, not have curved front and back.
Nema-17.pdf (760 KB)