【Arduino 动手做】项目灯塔 - 360° 迷你 Arduino 激光雷达 简单

//Update 1.1 of program, released on Instructables on 13/02/21.
//This update requires no modifications to the hardware, but makes improvements to the code.
//Please see the corresponding video!

#include <Wire.h> //These are the necessary libraries used in the program
#include "Adafruit_VL53L0X.h"

//The following adafruit tutorial is recommended to learn more about the VL53L0X sensors:
//https://learn.adafruit.com/adafruit-vl53l0x-micro-lidar-distance-sensor-breakout/arduino-code

#define LOX1_ADDRESS 0x30 //The first intended 12C address
#define LOX2_ADDRESS 0x31 //The second intended 12C address

#define SHT_LOX1 11 //Arduino pin for the X wire of the first VL53L0X
#define SHT_LOX2 12 //Arduino pin for the X wire of the second VL53L0X

Adafruit_VL53L0X lox1 = Adafruit_VL53L0X(); //Setting up the sensors using the adafruit library
Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();

VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;

const int FwdPin = 10;  //Forward Motor Pin
const int BwdPin = 9; //Backward Motor Pin
int MaxSpd = 60;  //Top speed (0-255) - I found that 60 is a good level to test
boolean DirFlag = true; //Flag for direction

int sensor1, sensor2; //Creates int type variable for sensors
int sum = 0;
int counter;
float xangle = 0;
int xinterval = 0;
int heading = 0;
int marker = 0;

void setup()
{
  delay(1000);
  Serial.begin(115200); //Sets baud rate for Serial. Make sure identical rate is selected in Serial Monitor/Processing
  while (! Serial) {  //Wait until serial port opens for native USB devices
    delay(1);
  }
  pinMode(SHT_LOX1, OUTPUT);
  pinMode(SHT_LOX2, OUTPUT);
  //Serial.println("Shutdown pins initiated...");
  digitalWrite(SHT_LOX1, LOW);
  digitalWrite(SHT_LOX2, LOW);
  //Serial.println("Both in reset mode...(pins are low)");
  //Serial.println("Starting...");
  setID(); //Runs code to assign unique 12C addresses to each of the two sensors.
  pinMode(FwdPin, OUTPUT);
  pinMode(BwdPin, OUTPUT);

  if (DirFlag)
  {
    analogWrite(FwdPin, MaxSpd); //Send instructions to Forward motor pin. Block this line out to stop motor from running
  }

  lox1.setMeasurementTimingBudgetMicroSeconds(20000);
  lox2.setMeasurementTimingBudgetMicroSeconds(20000);
  calibration(); //Runs calibration code (see function below)

}

void loop()
{
  while (millis() < 30000) { //This while loop cuts the sensor after 30 seconds of function, for a short test
    readsensors(); //Runs the function that records the distance measured from each sensor
    Serial.print(heading); //Prints the data to Serial in the manner expected by the Processing code, split by commas
    Serial.print(",");
    Serial.print(sensor1);
    Serial.print(",");
    Serial.println(sensor2);
  }
  analogWrite(FwdPin, 0); //Sets motor to OFF
  while (1) { //This while loop effectively pauses the program forever, so the sensor will turn OFF
  }
}

void setID() {
  // all reset
  digitalWrite(SHT_LOX1, LOW);
  digitalWrite(SHT_LOX2, LOW);
  delay(10);
  // all unreset
  digitalWrite(SHT_LOX1, HIGH);
  digitalWrite(SHT_LOX2, HIGH);
  delay(10);
  // activating LOX1 and resetting LOX2
  digitalWrite(SHT_LOX1, HIGH);
  digitalWrite(SHT_LOX2, LOW);
  // initiating LOX1
  if (!lox1.begin(LOX1_ADDRESS)) {
    //Serial.println(F("Failed to boot first VL53L0X"));
    while (1);
  }
  delay(10);
  // activating LOX2
  digitalWrite(SHT_LOX2, HIGH);
  delay(10);
  //initiating LOX2
  if (!lox2.begin(LOX2_ADDRESS)) {
    //Serial.println(F("Failed to boot second VL53L0X"));
    while (1);
  }
}

void calibration() {
  //CALIBRATION SEQUENCE
  //This conducts a series of test rotations, and from that determines the rotational speed of the sensor.
  //This was found to be consistent, so you can then measure how many readings you can complete per turn.
  //This value is then used to calculate this interval that is the angular increment of each reading.
  for (int i = 0; i <= 4; i++) {
    counter = 0;
    sensor1 = 0;
    while (sensor1 < 45 || sensor1 > 55) {
      lox1.rangingTest(&measure1, false);
      lox2.rangingTest(&measure2, false);
      sensor1 = measure1.RangeMilliMeter;
      delay(2);
    }
    counter ++;
    while (sensor1 < 45 || sensor1 > 55 || counter <= 6) {
      lox1.rangingTest(&measure1, false);
      lox2.rangingTest(&measure2, false);
      sensor1 = measure1.RangeMilliMeter;
      delay(2);
      counter ++;
    }
    sum = sum + counter;
  }
  xangle = sum / 5;
  xangle = 360 / xangle;
  xangle = round(xangle);
  xinterval = int(xangle);
  return (xinterval);
}

void readsensors() {
  lox1.rangingTest(&measure1, false);
  lox2.rangingTest(&measure2, false);
  sensor1 = measure1.RangeMilliMeter;
  sensor2 = measure2.RangeMilliMeter;
  //The line below identified when the sensor is seeing the pillar of the LiDAR tower
  //When obscured the sensor was found to read between these values.
  if (sensor1 > 45 && sensor1 < 55) {
    if (marker == 0) {
      heading = 0; //Resets the heading (of that particular distance measurement) to 0
      marker ++;
    }
    else {
      heading = heading + xinterval;
      marker ++;
    }
  }
  else {
    heading = heading + xinterval; //Adds the angular interval to each successive reading
    marker = 0;
  }
  //The line below identified when the received range is out of range
  //If the sensor did not receive a good reading (due to out of range) it would produce random value below 45
  if (sensor1 <= 45 || sensor1 >= 500) {
    sensor1 = 500;
  }
  if (heading <= 360) { //Filters out when heading (due to an error) has accidentally exceeded a full rotation
    return (sensor1);
    return (sensor2);
    return (heading);
  }
}