Cod Functional
Cod Functional
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
int errorCount1 = 0;
int errorCount2 = 0;
void setup() {
if (tfmP1.begin(&Serial1)) {
} else {
if (tfmP2.begin(&Serial2)) {
} else {
void loop() {
if (sensor1Success) {
Serial.print(avgDist1);
Serial.print(roundedDist1);
updateUniqueDistanceHistory(lastUniqueDist1, roundedDist1);
} else {
errorCount1++;
resetSensor(1);
errorCount1 = 0;
if (sensor2Success) {
Serial.print(avgDist2);
Serial.print(roundedDist2);
Serial.print(" cm");
updateUniqueDistanceHistory(lastUniqueDist2, roundedDist2);
} else {
errorCount2++;
Serial.print("Sensor 2 - Error");
resetSensor(2);
errorCount2 = 0;
if (checkForConsecutiveDecreases(lastUniqueDist1) &&
checkForConsecutiveDecreases(lastUniqueDist2) &&
compareSequences(lastUniqueDist1, lastUniqueDist2)) {
triggerMotor();
Serial.println("Motor stopped.");
Serial.println(); // Move to the next line for the next loop iteration
buffer[index] = newValue;
int32_t sum = 0;
sum += buffer[i];
void triggerMotor() {
Serial.println("Motor triggered!");
// Shift values in the history if the new distance is different from the last
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;
if (history1[i] != history2[i]) {
Serial.println(sensorNumber);
if (sensorNumber == 1) {
} else if (sensorNumber == 2) {
tfmP2.sendCommand(SOFT_RESET, 0); // Send reset command to
Sensor 2