#include <TFMPlus.
h> // Include the TFMPlus library
TFMPlus tfmP1; // Create an instance of the TFMPlus class for the first
sensor
TFMPlus tfmP2; // Create an instance of the TFMPlus class for the second
sensor
const int bufferSize = 5; // Size of the moving average buffer
int16_t distBuffer1[bufferSize] = {0}; // Buffer for first sensor
int16_t distBuffer2[bufferSize] = {0}; // Buffer for second sensor
int bufferIndex1 = 0; // Current index in the buffer for first sensor
int bufferIndex2 = 0; // Current index in the buffer for second sensor
int errorCount1 = 0;
int errorCount2 = 0;
const int motorPin = 9; // Motor control pin
bool motorRunning = false; // Flag to check if the motor is running
unsigned long motorStartTime = 0; // Variable to store the motor start
time
// Arrays to store the last unique rounded distances
int16_t lastUniqueDist1[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // Array to store 8
distances for sensor 1
int16_t lastUniqueDist2[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // Array to store 8
distances for sensor 2
void setup() {
Serial.begin(9600); // Initialize the Serial Monitor
Serial1.begin(115200); // Initialize communication with the first TF-Luna
on Serial1
Serial2.begin(115200); // Initialize communication with the second TF-
Luna on Serial2
pinMode(motorPin, OUTPUT); // Set motor pin as output
digitalWrite(motorPin, LOW); // Ensure motor is off initially
// Initialize the first sensor
if (tfmP1.begin(&Serial1)) {
Serial.println("TF-Luna 1 initialized successfully.");
} else {
Serial.println("TF-Luna 1 initialization failed. Check wiring.");
// Initialize the second sensor
if (tfmP2.begin(&Serial2)) {
Serial.println("TF-Luna 2 initialized successfully.");
} else {
Serial.println("TF-Luna 2 initialization failed. Check wiring.");
void loop() {
int16_t dist1 = 0, flux1 = 0, temp1 = 0; // Variables for first sensor
int16_t dist2 = 0, flux2 = 0, temp2 = 0; // Variables for second sensor
// Read data from the first sensor
bool sensor1Success = tfmP1.getData(dist1, flux1, temp1);
if (sensor1Success) {
errorCount1 = 0; // Reset error count on successful read
// Add the new distance to the buffer and calculate the moving average
updateBuffer(distBuffer1, bufferIndex1, dist1);
int16_t avgDist1 = calculateAverage(distBuffer1);
int16_t roundedDist1 = (avgDist1 + 5) / 10 * 10; // Round the averaged
distance to nearest 10
Serial.print("Sensor 1 - Averaged: ");
Serial.print(avgDist1);
Serial.print(" cm, Rounded: ");
Serial.print(roundedDist1);
Serial.print(" cm, ");
// Update the unique distance history
updateUniqueDistanceHistory(lastUniqueDist1, roundedDist1);
} else {
errorCount1++;
Serial.print("Sensor 1 - Error, ");
if (errorCount1 > 3) { // Reset after 3 consecutive errors
resetSensor(1);
errorCount1 = 0;
// Read data from the second sensor
bool sensor2Success = tfmP2.getData(dist2, flux2, temp2);
if (sensor2Success) {
errorCount2 = 0; // Reset error count on successful read
// Add the new distance to the buffer and calculate the moving average
updateBuffer(distBuffer2, bufferIndex2, dist2);
int16_t avgDist2 = calculateAverage(distBuffer2);
int16_t roundedDist2 = (avgDist2 + 5) / 10 * 10; // Round the averaged
distance to nearest 10
Serial.print("Sensor 2 - Averaged: ");
Serial.print(avgDist2);
Serial.print(" cm, Rounded: ");
Serial.print(roundedDist2);
Serial.print(" cm");
// Update the unique distance history
updateUniqueDistanceHistory(lastUniqueDist2, roundedDist2);
} else {
errorCount2++;
Serial.print("Sensor 2 - Error");
if (errorCount2 > 3) { // Reset after 3 consecutive errors
resetSensor(2);
errorCount2 = 0;
// Check if both sensors have matching sequences of 8 consecutive
decreases of 10 cm
if (checkForConsecutiveDecreases(lastUniqueDist1) &&
checkForConsecutiveDecreases(lastUniqueDist2) &&
compareSequences(lastUniqueDist1, lastUniqueDist2)) {
triggerMotor();
// Check if the motor should be stopped after running for 1 second
if (motorRunning && (millis() - motorStartTime >= 1000)) {
digitalWrite(motorPin, LOW); // Turn off the motor
motorRunning = false; // Reset the motor running flag
Serial.println("Motor stopped.");
Serial.println(); // Move to the next line for the next loop iteration
delay(5); // Reduced delay for faster loop speed
// Function to update the moving average buffer
void updateBuffer(int16_t buffer[], int &index, int16_t newValue) {
buffer[index] = newValue;
index = (index + 1) % bufferSize; // Move to the next buffer index in a
circular manner
// Function to calculate the average of the buffer
int16_t calculateAverage(int16_t buffer[]) {
int32_t sum = 0;
for (int i = 0; i < bufferSize; i++) {
sum += buffer[i];
return sum / bufferSize;
}
// Function to trigger the motor
void triggerMotor() {
if (!motorRunning) { // Check if the motor is not already running
Serial.println("Motor triggered!");
digitalWrite(motorPin, HIGH); // Turn on the motor
motorStartTime = millis(); // Record the start time
motorRunning = true; // Set the motor running flag
// Function to update the unique distance history array
void updateUniqueDistanceHistory(int16_t history[8], int16_t newDist) {
// Shift values in the history if the new distance is different from the last
if (newDist != history[7]) { // Adjusted to check against the last (8th)
distance
history[0] = history[1];
history[1] = history[2];
history[2] = history[3];
history[3] = history[4];
history[4] = history[5];
history[5] = history[6];
history[6] = history[7];
history[7] = newDist;
// Function to check for 8 consecutive decreases of 10 cm
bool checkForConsecutiveDecreases(int16_t history[8]) {
return (history[0] - history[1] == 10) &&
(history[1] - history[2] == 10) &&
(history[2] - history[3] == 10) &&
(history[3] - history[4] == 10) &&
(history[4] - history[5] == 10) &&
(history[5] - history[6] == 10) &&
(history[6] - history[7] == 10);
// Function to compare if both sequences are identical
bool compareSequences(int16_t history1[8], int16_t history2[8]) {
for (int i = 0; i < 8; i++) {
if (history1[i] != history2[i]) {
return false; // Return false if any element does not match
return true; // Return true if all elements match
// Function to reset the sensor in case of repeated errors
void resetSensor(int sensorNumber) {
Serial.print("Resetting Sensor ");
Serial.println(sensorNumber);
if (sensorNumber == 1) {
tfmP1.sendCommand(SOFT_RESET, 0); // Send reset command to
Sensor 1
} else if (sensorNumber == 2) {
tfmP2.sendCommand(SOFT_RESET, 0); // Send reset command to
Sensor 2
delay(500); // Shorter wait for the sensor to reset
