0% found this document useful (0 votes)
11 views8 pages

Cod Functional

COD ESP MEGA

Uploaded by

tmendkpaver
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
0% found this document useful (0 votes)
11 views8 pages

Cod Functional

COD ESP MEGA

Uploaded by

tmendkpaver
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 8

#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

You might also like