Untitled
unknown
plain_text
5 months ago
4.5 kB
2
Indexable
#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 //}`
Editor is loading...
Leave a Comment