Add obstacle avoidance and monitoring modes

Introduced new obstacle avoidance and monitoring system modes with dedicated source files. Updated main.cpp to use ObstacleAvoidance, refactored sensor and actuator initialization, and added telemetry output. Adjusted color sensor thresholds and logic for improved detection, updated pin assignments, and revised motor speed constants for better control.

Co-Authored-By: Oneli Wijesuriya <247418039+onelicodes@users.noreply.github.com>

Author: heyisula

ESP32-S3-Main/src/config/constants.h

@@ -16,11 +16,6 @@
 #define FOLLOW_DISTANCE_MAX 200      // cm
 #define FOLLOW_DISTANCE_DEFAULT 100  // cm
 
-// Navigation Constants
-#define LINE_THRESHOLD 512           // Analog threshold for line detection
-#define MAX_WAYPOINTS 10
-#define POSITION_PRECISION 5         // cm
-
 // Battery Constants
 #define BATTERY_LOW_VOLTAGE 10.5     // V
 #define BATTERY_CRITICAL_VOLTAGE 10.0 // V
@@ -31,8 +26,8 @@
 #define HEARTBEAT_INTERVAL 1000      // ms
 
 // Motor Speed Constants
-#define MOTOR_SPEED_MIN 20           // %
-#define MOTOR_SPEED_MAX 100          // %
+#define MOTOR_SPEED_MIN 100           // %
+#define MOTOR_SPEED_MAX 255          // %
 #define MOTOR_SPEED_DEFAULT 50       // %
 
 // Health Monitoring Constants

ESP32-S3-Main/src/config/pins.h

@@ -46,7 +46,7 @@
 #define ROTARY_CLK 38
 
 // Color Sensor
-#define COLOR_OUT 0
+#define COLOR_OUT 14
 #define COLOR_S0 47
 #define COLOR_S1 48
 #define COLOR_S2 20

ESP32-S3-Main/src/config/thresholds.h

@@ -9,36 +9,22 @@
 #define LIGHT_MIN 200                // Lux
 #define LIGHT_MAX 800                // Lux
 
-// Color Detection Thresholds
-#define BLACK_SUM_MAX 10        // Below this = too dark to read
-// WHITE (Patients) - all channels relatively high AND similar
-#define WHITE_R_MIN 12          
-#define WHITE_G_MIN 12          
-#define WHITE_B_MIN 12
-#define WHITE_BALANCE_TOLERANCE 6  // R, G, B should be within 5 of each other
-// BLUE (Surgeons/Doctors) - blue higher than red
-#define BLUE_B_MIN 10           
-#define BLUE_R_MAX 8
-#define BLUE_DIFF_MIN 3         // Blue should be at least 3 higher than red
-// GREEN (Nurses) - green higher than others
-#define GREEN_G_MIN 8          
-#define GREEN_R_MAX 6
-#define GREEN_B_MAX 6
-#define GREEN_DIFF_MIN 2        // Green should be at least 2 higher
-// RED (Medical Students) - red higher than green
-#define RED_R_MIN 8
-#define RED_G_MAX 5
-#define RED_DIFF_MIN 3          // Red should be at least 3 higher than green
-// PURPLE (Minor Staff) - both red and blue high, green low
-#define PURPLE_R_MIN 8
-#define PURPLE_B_MIN 8
-#define PURPLE_G_MAX 5
-#define PURPLE_SUM_MIN 20       // Total must be reasonably high
+// Color Sensor Thresholds
+#define BLACK_SUM_MAX 30
+#define WHITE_R_MIN 25
+#define WHITE_G_MIN 25
+#define WHITE_B_MIN 30
+#define WHITE_BALANCE_TOLERANCE 15
+#define BLUE_B_MIN 18
+#define BLUE_DIFF_MIN 5
+#define GREEN_G_MIN 12
+#define GREEN_DIFF_MIN 3
+#define RED_R_MIN 20
+#define RED_DIFF_MIN 10
 // Temporal averaging
-#define COLOR_AVG_SAMPLES        5
-// Auto-calibration
-#define AMBIENT_ADAPT_RATE       0.05f   // Slow adaptation
-#define AMBIENT_UPDATE_THRESHOLD 15      // Ignore strong colors
+#define COLOR_AVG_SAMPLES 5
+#define AMBIENT_ADAPT_RATE 0.05f
+#define AMBIENT_UPDATE_THRESHOLD 15
 
 
 // Collision Detection

ESP32-S3-Main/src/main.cpp

@@ -1,51 +1,100 @@
 #include <Arduino.h>
+
+#include "modes/obstacle_avoidance.h"
+#include "sensors/hcsr04.h"
 #include "sensors/mpu6050.h"
+#include "communication/uart.h"
+#include "actuators/ermc1604syg.h"
+#include "actuators/sfm27.h"
 
+// --------------------------------------------------
+// Global subsystem instances
+// --------------------------------------------------
+UltrasonicManager ultrasonicManager;
 MotionTracker motionTracker;
-unsigned long lastPrintTime = 0;
+UARTProtocol uartProtocol;
+Display display;
+Buzzer buzzer;
+
+// --------------------------------------------------
+// Obstacle Avoidance controller
+// --------------------------------------------------
+ObstacleAvoidance obstacleAvoidance(
+    &ultrasonicManager,
+    &motionTracker,
+    &uartProtocol,
+    &display,
+    &buzzer
+);
 
+// --------------------------------------------------
+// Setup
+// --------------------------------------------------
 void setup() {
     Serial.begin(115200);
-    while (!Serial && millis() < 3000);
-    
-    Serial.println("\n=== MPU6050 Motion Tracker ===");
-    Serial.println("Initializing...\n");
-    
-    if (!motionTracker.begin()) {
-        Serial.println("ERROR: MPU6050 initialization failed!");
-        while (1) delay(1000);
-    }
-    
-    Serial.println("MPU6050 initialized successfully!");
-    
-    motionTracker.autoCalibrate();
-    
-    Serial.println("--- Starting measurements ---\n");
-    Serial.println("Time(s) | Roll(°) | Pitch(°) | Fwd(g) | Side(g) | Mag(g) | RawX | RawY | RawZ");
-    Serial.println("--------|---------|----------|--------|---------|--------|------|------|------");
-    
-    lastPrintTime = millis();
+    delay(1000);
+
+    Serial.println();
+    Serial.println("======================================");
+    Serial.println(" Obstacle Avoidance - SYSTEM TEST ");
+    Serial.println("======================================");
+
+    obstacleAvoidance.begin();
+
+    Serial.println("Initialization complete.");
 }
 
+// --------------------------------------------------
+// Loop
+// --------------------------------------------------
 void loop() {
-    motionTracker.update();
-    
-    if (millis() - lastPrintTime >= 100) {
-        lastPrintTime = millis();
-        
-        float roll = motionTracker.getRoll();
-        float pitch = motionTracker.getPitch();
-        float fwd = motionTracker.getForwardAcceleration();
-        float side = motionTracker.getSideAcceleration();
-        float mag = motionTracker.getAccelMagnitude();
-        float rawX = motionTracker.getAccelX();
-        float rawY = motionTracker.getAccelY();
-        float rawZ = motionTracker.getAccelZ();
-        
-        char buf[150];
-        snprintf(buf, sizeof(buf),
-                 "%7.2f | %7.2f | %8.2f | %6.3f | %7.3f | %6.3f | %4.2f | %4.2f | %4.2f",
-                 millis()/1000.0, roll, pitch, fwd, side, mag, rawX, rawY, rawZ);
-        Serial.println(buf);
-    }
+    obstacleAvoidance.update();
+
+    // ----------------------------------------------
+    // Read back telemetry from ObstacleAvoidance
+    // ----------------------------------------------
+    ObstacleAvoidance::SensorData data = obstacleAvoidance.getSensorData();
+
+    Serial.print("[STATUS=");
+    Serial.print(data.status);
+    Serial.print("] ");
+
+    Serial.print("SPD=");
+    Serial.print(data.speed);
+    Serial.print("% | ");
+
+    Serial.print("F:");
+    Serial.print(data.frontDist, 1);
+    Serial.print("cm ");
+
+    Serial.print("B:");
+    Serial.print(data.rearDist, 1);
+    Serial.print("cm ");
+
+    Serial.print("L:");
+    Serial.print(data.leftDist, 1);
+    Serial.print("cm ");
+
+    Serial.print("R:");
+    Serial.print(data.rightDist, 1);
+    Serial.print("cm | ");
+
+    Serial.print("Pitch:");
+    Serial.print(data.pitch, 1);
+    Serial.print(" ");
+
+    Serial.print("Roll:");
+    Serial.print(data.roll, 1);
+    Serial.print(" | ");
+
+    Serial.print("AccX:");
+    Serial.print(data.forwardAccel, 2);
+    Serial.print(" ");
+
+    Serial.print("AccY:");
+    Serial.print(data.sideAccel, 2);
+
+    Serial.println();
+
+    delay(250);
 }