Refactor and enhance system initialization and UI

Major refactor of main.cpp to improve initialization order, add detailed serial feedback, and integrate menu and rotary encoder UI. Updated display logic and status reporting in assistant and monitoring modes. Improved UART protocol with acknowledgment tracking and connection status. Adjusted constants and thresholds for sensors and safety. Cleaned up comments and removed redundant I2C initializations from sensor drivers. Added new menu system implementation and improved actuator and sensor update intervals for more responsive operation.

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

Author: heyisula

ESP32-S3-Main/platformio.ini

@@ -1,3 +1,13 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
 [env:esp32s3_n16r8_devkit]
 platform = espressif32
 board = esp32-s3-devkitc-1
@@ -9,7 +19,6 @@ lib_deps =
 	sparkfun/SparkFun MAX3010x Pulse and Proximity Sensor Library@^1.1.2
 	adafruit/Adafruit Unified Sensor@^1.1.15
 	hasenradball/AM2302-Sensor@^1.4.0
-	ericksimoes/Ultrasonic@^3.0.0
 	panjkrc/tcs3200@^1.3.1
 	codingabi/KY040@^1.0.3
 	powerbroker2/SerialTransfer@^3.1.5

ESP32-S3-Main/src/actuators/ermc1604syg.cpp

@@ -7,12 +7,9 @@ Display::Display() : lcd(LCD_ADDR, LCD_COLS, LCD_ROWS) {
 }
 
 bool Display::begin() {
-    // Initialize Wire with the project-defined pins
-    Wire.begin(I2C_SDA, I2C_SCL);
-    Wire.setClock(I2C_FREQUENCY);
     // Initialize the LCD and turn backlight on
     lcd.init();        // initialize the PCF8574 I/O expander and LCD
-    lcd.backlight();   // ensure backlight is ON (you requested always ON)
+    lcd.backlight();   // ensure backlight is ON
 
     lcd.clear();
 
@@ -26,8 +23,6 @@ bool Display::begin() {
     lcd.setCursor(0, 3);
     lcd.print(" Backlight: ON  ");
 
-    lcd.print(" Backlight: ON  ");
-
     delay(500);
     return true;
 }
@@ -58,22 +53,18 @@ void Display::print(float value, int decimals) {
     lcd.print(value, decimals);
 }
 
-void Display::displayStatus(String mode, int battery, float distance, int hr, int spo2) {
+void Display::displayStatus(int battery, float temp, float humidity, int modeNum) {
     clear();
     setCursor(0, 0);
-    // truncate if needed
-    String line1 = mode + " Bat:" + String(battery) + "%";
-    if (line1.length() > LCD_COLS) line1 = line1.substring(0, LCD_COLS);
+    String line1 = "Battery: " + String(battery) + "%";
     print(line1);
 
     setCursor(0, 1);
-    String line2 = "Following: " + String(distance, 1) + "m";
-    if (line2.length() > LCD_COLS) line2 = line2.substring(0, LCD_COLS);
+    String line2 = "T:" + String(temp, 1) + "C H:" + String(humidity, 0) + "%";
     print(line2);
 
     setCursor(0, 2);
-    String line3 = "HR:" + String(hr) + " SpO2:" + String(spo2) + "%";
-    if (line3.length() > LCD_COLS) line3 = line3.substring(0, LCD_COLS);
+    String line3 = "Active Mode: " + String(modeNum);
     print(line3);
 
     setCursor(0, 3);

ESP32-S3-Main/src/actuators/ermc1604syg.h

@@ -25,7 +25,7 @@ class Display {
     void print(int value);
     void print(float value, int decimals = 1);
 
-    void displayStatus(String mode, int battery, float distance, int hr, int spo2);
+    void displayStatus(int battery, float temp, float humidity, int modeNum);
     void displayError(String error);
 };
 

ESP32-S3-Main/src/actuators/le0066.cpp

@@ -35,7 +35,7 @@ void LEDArray::setStatus(bool leftOn, bool rightOn) {
 }
 
 void LEDArray::update() {
-    // Reserved for any periodic updates if needed
+    
 }
 
 void LEDArray::controlFromFirebase(int lightadj_left, int lightadj_right) {

ESP32-S3-Main/src/actuators/sfm27.cpp

@@ -24,7 +24,6 @@ void Buzzer::setVolume(int vol) {
     volume = constrain(vol, 0, 100);
 }
 
-// For active buzzers, we ignore frequency and use steady push-pull
 void Buzzer::drive(uint16_t duration_ms, uint16_t freq) {
     if (volume == 0) {
         stop();

ESP32-S3-Main/src/communication/firebase_manager.cpp

@@ -24,7 +24,7 @@ bool FirebaseManager::ready() {
 }
 
 void FirebaseManager::update() {
-    // Reserved for future background tasks if needed
+    
 }
 
 bool FirebaseManager::sendData(const FirebaseTxData& d) {
@@ -59,7 +59,7 @@ bool FirebaseManager::sendData(const FirebaseTxData& d) {
     // Compartment - SEND ONLY
     json.set("compartment", d.compartment);
 
-    if (!Firebase.updateNode(fbdo, "/telemetry", json)) {
+    if (!Firebase.updateNode(fbdo, "/", json)) {
         Serial.print("Firebase TX failed: ");
         Serial.println(fbdo.errorReason());
         return false;
@@ -71,12 +71,11 @@ bool FirebaseManager::sendData(const FirebaseTxData& d) {
 bool FirebaseManager::receiveData(FirebaseRxData& d) {
     if (!ready()) return false;
 
-    // Non-blocking throttling: Don't fetch from server too fast
     static unsigned long lastRX = 0;
     if (millis() - lastRX < 1000) return true; // Keep old data if less than 1s passed
     lastRX = millis();
 
-    if (!Firebase.getJSON(fbdo, "/controls")) {
+    if (!Firebase.getJSON(fbdo, "/")) {
         Serial.print("Firebase RX failed: ");
         Serial.println(fbdo.errorReason());
         return false;
@@ -95,13 +94,15 @@ bool FirebaseManager::receiveData(FirebaseRxData& d) {
     json.get(jd, "buzzersound");
     if (jd.success) d.buzzersound = jd.intValue;
 
+
     // LED brightness control - RECEIVE ONLY
     json.get(jd, "lightadj_left");
     if (jd.success) d.lightadj_left = jd.intValue;
 
     json.get(jd, "lightadj_right");
     if (jd.success) d.lightadj_right = jd.intValue;
 
+
     // Sensor start/stop flags - RECEIVE ONLY
     json.get(jd, "colour_start");
     if (jd.success) d.colour_start = jd.boolValue;

ESP32-S3-Main/src/communication/firebase_manager.h

@@ -10,26 +10,16 @@ struct FirebaseTxData {
     int angular;               // Send angular velocity
     int battery;               // Send battery level
     int voltage;               // Send voltage
-    
-    // Environment - SEND ONLY
     int temp;                  // Send temperature
     int humidity;              // Send humidity
     int lightlevel;            // Send light amount from path LDRs
-    
-    // Health - SEND ONLY
     int hr;                    // Send heart rate
     int sp02;                  // Send SpO2 level
-    
-    // Ultrasonic - SEND ONLY
     int ultrasonic_center;     // Send center distance
     int ultrasonic_left;       // Send left distance
     int ultrasonic_rear;       // Send rear distance
     int ultrasonic_right;      // Send right distance
-    
-    // Color - SEND ONLY
     String colour;             // Send detected color (RED/BLUE/GREEN/WHITE/UNKNOWN)
-    
-    // Compartment - SEND ONLY
     int compartment;           // Send compartment state (0=open, 255=closed)
 };
 

ESP32-S3-Main/src/communication/uart.cpp

@@ -5,6 +5,9 @@
 UARTProtocol::UARTProtocol() {
     serial = &Serial1;
     lastSendTime = 0;
+    lastSentCommand = CMD_STOP;
+    isWaitingForAck = false;
+    lastAckTime = 0;
 }
 
 void UARTProtocol::begin() {
@@ -18,11 +21,15 @@ void UARTProtocol::sendMotorCommand(MotorCommand cmd, uint8_t speed) {
     uint8_t speedValue = constrain(speed, 0, 100);
 
     // Put command and speed into distinct slots
-    transfer.txObj(cmdValue, 0);       // Slot 0: Command
-    transfer.txObj(speedValue, 1);    // Slot 1: Speed
+    transfer.txObj(cmdValue, 0);      
+    transfer.txObj(speedValue, 1);    
 
-    // Send exactly 2 bytes of data objects
     transfer.sendData(2); 
+
+    // Update tracking state
+    lastSentCommand = cmd;
+    lastSendTime = millis();
+    isWaitingForAck = true;
 }
 
 
@@ -36,7 +43,21 @@ bool UARTProtocol::receiveAcknowledgment(MotorCommand &cmd, uint8_t &speed) {
         // Read command and speed from receive buffer
         transfer.rxObj(cmd, 0);
         transfer.rxObj(speed, 1);
-        return true;  // acknowledgment received
+
+        // Verify if this matches what we sent
+        if (isWaitingForAck && cmd == lastSentCommand) {
+            isWaitingForAck = false;
+            lastAckTime = millis();
+            return true;  // Correct acknowledgment received
+        }
+        
+        return true; // Received some packet, even if not the specific ACK we waited for
     }
+
+    // Check for timeout
+    if (isWaitingForAck && (millis() - lastSendTime > 500)) {
+        isWaitingForAck = false; 
+    }
+
     return false;  // no data yet
 }

ESP32-S3-Main/src/communication/uart.h

@@ -23,13 +23,19 @@ class UARTProtocol {
 
     unsigned long lastSendTime;
 
+    MotorCommand lastSentCommand;
+    bool isWaitingForAck;
+    unsigned long lastAckTime;
 
 public:
     UARTProtocol();
     void begin();
     void sendMotorCommand(MotorCommand cmd, uint8_t speed);
     void sendEmergencyStop();
     bool receiveAcknowledgment(MotorCommand &cmd, uint8_t &speed);
+    
+    bool isLastCommandAcked() const { return !isWaitingForAck; }
+    bool isConnected() const { return (millis() - lastAckTime < 2000); }
 };
 
 #endif

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

@@ -7,7 +7,7 @@
 #define I2C_FREQUENCY 100000
 
 // Ultrasonic Constants
-#define MAX_ULTRASONIC_DISTANCE 200  // cm
+#define MAX_ULTRASONIC_DISTANCE 100  // cm
 #define MIN_SAFE_DISTANCE 20         // cm
 #define SOUND_SPEED 0.034            // cm/microsecond
 
@@ -31,6 +31,6 @@
 #define SPO2_MIN 95                  // %
 #define SPO2_MAX 100                 // %
 
-#define MPU_ALPHA 0.96f  // Start here, tune between 0.96-0.98
+#define MPU_ALPHA 0.96f  // Complementary filter constant
 
 #endif