Two TF02 pro LiDar sensor with arduino mega. i want to make blind corner saftey system using horn and redlight activating by relays this code doesnt

Projects General Guidance
1 
#include <SoftwareSerial.h>

#define RELAY1_PIN 4
#define RELAY2_PIN 3
#define TF02_1_RX_PIN 19   // Pin for TF02 sensor 1 RX (Green) - Serial1 RX1
#define TF02_1_TX_PIN 18   // Pin for TF02 sensor 1 TX (White) - Serial1 TX1
#define TF02_2_RX_PIN 17   // Pin for TF02 sensor 2 RX (Green) - Serial2 RX2
#define TF02_2_TX_PIN 16   // Pin for TF02 sensor 2 TX (White) - Serial2 TX2

const int NUM_SAMPLES = 5;   // Number of samples for averaging
const int MIN_DISTANCE_CHANGE = 5;   // Minimum change in distance (in cm) to count as significant

uint16_t dist_cm1, dist_cm2;          // LiDAR actually measured distance values in centimeters
uint16_t distBuffer1[NUM_SAMPLES];    // Buffer to store the last readings for sensor 1
uint16_t distBuffer2[NUM_SAMPLES];    // Buffer to store the last readings for sensor 2
uint8_t bufferIndex1 = 0, bufferIndex2 = 0; // Index to keep track of buffer position

const uint8_t HEADER = 0x59;          // Data package frame header

void setup() {
  Serial.begin(115200);              // Set the Baud rate of Arduino and computer serial port
  Serial1.begin(115200);             // Set the Baud rate of LiDAR 1 and Arduino Serial1 port
  Serial2.begin(115200);             // Set the Baud rate of LiDAR 2 and Arduino Serial2 port
  pinMode(RELAY1_PIN, OUTPUT);
  pinMode(RELAY2_PIN, OUTPUT);       // Initialize relay pins as output
  digitalWrite(RELAY1_PIN, HIGH);    // Ensure relays are off at start
  digitalWrite(RELAY2_PIN, HIGH);    // Ensure relays are off at start

  // Initialize distance buffers with large initial values
  for (int i = 0; i < NUM_SAMPLES; i++) {
    distBuffer1[i] = 10000;
    distBuffer2[i] = 10000;
  }

  Serial.println("Setup complete");
}

void loop() {
  bool vehicleApproaching1 = checkSensor(Serial1, distBuffer1, bufferIndex1, "Sensor 1");
  bool vehicleApproaching2 = checkSensor(Serial2, distBuffer2, bufferIndex2, "Sensor 2");

  if (vehicleApproaching1 || vehicleApproaching2) {
    // Activate both relays (LOW)
    digitalWrite(RELAY1_PIN, LOW);
    digitalWrite(RELAY2_PIN, LOW);
    Serial.println("Relays activated");
  } else {
    // Deactivate both relays (HIGH)
    digitalWrite(RELAY1_PIN, HIGH);
    digitalWrite(RELAY2_PIN, HIGH);
    Serial.println("Relays deactivated");
  }

  delay(100);
}

bool checkSensor(HardwareSerial &serial, uint16_t distBuffer[], uint8_t &bufferIndex, const char* sensorName) {
  if (serial.available() >= 9) { // Check if the serial port has available data
    if (serial.read() == HEADER && serial.read() == HEADER) { // Determine data package frame header 0x59
      byte highByte = serial.read();
      byte lowByte = serial.read();
      uint16_t currentDist = ((uint16_t)highByte << 8) | lowByte; // Combine high and low bytes to get the distance value in centimeters

      // Store the current distance in the buffer
      distBuffer[bufferIndex] = currentDist;
      bufferIndex = (bufferIndex + 1) % NUM_SAMPLES;

      // Check if all distances in the buffer are decreasing
      bool isDecreasing = true;
      for (int i = 1; i < NUM_SAMPLES; i++) {
        int prevIndex = (bufferIndex + i) % NUM_SAMPLES;
        int currIndex = (bufferIndex + i - 1) % NUM_SAMPLES;
        if (distBuffer[currIndex] >= distBuffer[prevIndex]) {
          isDecreasing = false;
          break;
        }
      }

      // Check for significant change
      bool significantChange = (distBuffer[(bufferIndex + 1) % NUM_SAMPLES] - currentDist) > MIN_DISTANCE_CHANGE;

      // Debugging: Print current and average distance
      Serial.print(sensorName);
      Serial.print(" Current Distance: ");
      Serial.print(currentDist);
      Serial.print(" cm");

      // Debugging: Print isDecreasing status
      if (isDecreasing && significantChange) {
        Serial.print(" - Decreasing");
      }
      Serial.println();

      // If the distance has been decreasing for the last NUM_SAMPLES readings with a significant change
      if (isDecreasing && significantChange) {
        Serial.print(sensorName);
        Serial.println(" Approaching detected");
        return true; // Vehicle is approaching
      }
    }
  }
  return false; // Vehicle is not approaching
}
2

Do you have a question ?

3

yes. My codes are not working correctly. Actually i am making blind corner safety sys using two LiDar sensor facing to each side of road. i want them to detect approaching vehicle only and activate the horn and red light by activating two relays.

4

We need more details of the problem

What happens when you run the sketch ?

5

code runs and uploads. but it cant recognizes approaching vehicle and working receding vehicle also.

6

What do you see if you print the data received from the sensors ? Is it what you expect ?

7

To determine why not, put in some more Serial.print() statements that test your assumptions.

For example, is this line working as expected? (Hint: probably not).

bool significantChange = (distBuffer[(bufferIndex + 1) % NUM_SAMPLES] - currentDist) > MIN_DISTANCE_CHANGE;

1 Like
8

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.