Untitled

#include <SoftwareSerial.h>

//#define HORN_PIN 2 // Pin for activating the horn
//#define RED_LIGHT_PIN 3 // Pin for the red light
//#define GREEN_LIGHT_PIN 4 // Pin for the green light
#define TF02_RX_PIN 10 // Pin for TF02 sensor RX
#define TF02_TX_PIN 11 // Pin for TF02 sensor TX

SoftwareSerial tf02Serial(TF02_RX_PIN, TF02_TX_PIN); // SoftwareSerial instance for TF02 sensor

const int NUM_READINGS = 5; // Number of readings to average
uint16_t distanceReadings[NUM_READINGS]; // Array to store distance readings
int currentIndex = 0; // Index for storing the current reading

void setup() {
  Serial.begin(9600);
  //pinMode(HORN_PIN, OUTPUT);
  //pinMode(RED_LIGHT_PIN, OUTPUT);
  //pinMode(GREEN_LIGHT_PIN, OUTPUT);

  //digitalWrite(RED_LIGHT_PIN, LOW); // Turn off red light initially
  //digitalWrite(GREEN_LIGHT_PIN, HIGH); // Turn on green light initially

  tf02Serial.begin(115200); // Initialize serial communication for TF02 at baud rate 115200

  // Initialize distance readings array
  for (int i = 0; i < NUM_READINGS; i++) {
    distanceReadings[i] = 0;
  }
}

void loop() {
  //uint16_t distance = readDistanceFromTF02(); // Read distance measurement from TF02
  //Serial.print("Distance: ");
  //Serial.print(distance);
  //println(" cm");
  Serial.print("\n\n");
  setBaudRate(9600);
  setSave();
  delay(20000);
  // Update distance readings array
  //distanceReadings[currentIndex] = distance;
  //currentIndex = (currentIndex + 1) % NUM_READINGS;

}

uint16_t readDistanceFromTF02() {
  // Send command to request distance data
  tf02Serial.write((uint8_t)0x5A); // Command to request distance data
  tf02Serial.write((uint8_t)0x05); // Data length
  tf02Serial.write((uint8_t)0x00); // Data byte 1
  tf02Serial.write((uint8_t)0x00); // Data byte 2
  tf02Serial.write((uint8_t)0x00); // Data byte 3
  tf02Serial.write((uint8_t)0x01); // Data byte 4

  // Read response
  uint8_t response[9];
  tf02Serial.readBytes(response, 9);

  // Check if response is valid
  if (response[0] == 0x59 && response[1] == 0x59) {
    // Extract distance data
    uint16_t distance = (response[3] << 8) | response[2]; // Combine high and low bytes
    return distance;
  } else {
    return 0; // Invalid response, return 0 distance
  }
}

void setBaudRate(int baud) {
  byte command[8] = {0x5A, 0x08, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00};

  switch (baud) {
    case 9600:
      command[5] = 0x25;
      command[6] = 0x80;
      break;
    case 19200:
      command[5] = 0x4B;
      command[6] = 0x00;
      break;
    case 38400:
      command[5] = 0x96;
      command[6] = 0x00;
      break;
    case 115200:
      command[5] = 0xC2;
      command[6] = 0x01;
      break;
    default:
      Serial.println("Nieobsługiwany baud rate!");
      return;
  }

  // Oblicz sumę kontrolną
  command[7] = (0xF1);

  Serial.print("Komenda wysłana: ");
        for (int i = 0; i < 9; i++) {
          if (command[i] < 0x10) Serial.print("0"); // Add leading zero for clarity
          Serial.print(command[i], HEX);
          Serial.print(" ");
        }

  // Wyślij komendę do lidar
  tf02Serial.write(command, 8);

  // Read response
  uint8_t response[9];
  tf02Serial.readBytes(response, 9);
  
  Serial.print("Raw Data: ");
        for (int i = 0; i < 9; i++) {
          if (response[i] < 0x10) Serial.print("0"); // Add leading zero for clarity
          Serial.print(response[i], HEX);
          Serial.print(" ");
        }

}

void setSave() {
  byte command[4] = {0x5A, 0x04, 0x11, 0x6F};


  Serial.print("Komenda wysłana: ");
        for (int i = 0; i < 9; i++) {
          if (command[i] < 0x10) Serial.print("0"); // Add leading zero for clarity
          Serial.print(command[i], HEX);
          Serial.print(" ");
        }
  Serial.print("\n\n");

  // Wyślij komendę do lidar
  tf02Serial.write(command, 4);

  // Read response
  uint8_t response[5];
  tf02Serial.readBytes(response, 5);
  
  Serial.print("Raw Data: ");
        for (int i = 0; i < 5; i++) {
          if (response[i] < 0x10) Serial.print("0"); // Add leading zero for clarity
          Serial.print(response[i], HEX);
          Serial.print(" ");
        }

}

//void activateHorn() {
 // digitalWrite(HORN_PIN, HIGH); // Activate horn
 // delay(1000); // Horn duration: 1 second
 // digitalWrite(HORN_PIN, LOW); // Deactivate horn
//}`