Add DisplayLib for OLED display management and enhance motor control with rotation methods

src/main.cpp

@@ -2,8 +2,9 @@
 #include <Wire.h>
 #include "Adafruit_BNO08x_RVC.h"
 // #include <PixyI2C.h>  // Commented out
-// #include "Ultrasonic.h" // Commented out
+#include "Ultrasonic.h" // Commented out
 #include "OmniWheelMotors.hpp"
+#include "DisplayLib.hpp"
 
 // Constants
 #define MOTOR_SPEED 120
@@ -22,7 +23,7 @@
 // Sensor objects
 // PixyI2C pixy;  // Commented out
 Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();
-// Ultrasonic uSensor1 = Ultrasonic(7);  // Commented out
+Ultrasonic uSensor1 = Ultrasonic(7);
 // Ultrasonic uSensor2 = Ultrasonic(10); // Commented out
 // Ultrasonic uSensor3 = Ultrasonic(9);  // Commented out
 
@@ -31,11 +32,14 @@ OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM,
                        MOTOR2_R_PWM, MOTOR2_L_PWM,
                        MOTOR3_R_PWM, MOTOR3_L_PWM);
 
+// Display object
+DisplayLib display;
+
 // Global variables
 int lastBallSensor = -1;
 int lastBallStrength = 255; // Start at max distance
 unsigned long lastBallDetectionTime = 0;
-bool ballLost = false;
+bool ballLost = true;
 bool robotActive = false; // Flag to indicate if robot should be operating
 
 void setup()
@@ -46,6 +50,10 @@ void setup()
     delay(10);
 
   Serial.println("ElBestia Robot Starting...");
+  
+  // Initialize display
+  display.initDisplay();
+  display.welcomeScreen();
 
   // Initialize I2C
   Wire.begin();
@@ -62,7 +70,7 @@ void setup()
 
   // Initialize motors
   motors.begin();
-  motors.setSpeedLimits(45, 70); // Set min and max speeds
+  motors.setSpeedLimits(150, 255); // Set min and max speeds
   motors.setRotationTime(2000);  // Set time for full rotation (2 seconds = 360°)
   Serial.println("Motors initialized");
 
@@ -102,6 +110,8 @@ void setup()
   // wait for 'start' command from serial
   // wait for 'start' command from serial
   Serial.println("Send 'start' to begin operation or 'stop' to halt");
+  display.clearDisplay();
+  display.displayMessage("Send 'start' to begin operation or 'stop' to halt");
   while (!robotActive)
   {
     if (Serial.available() > 0)
@@ -136,6 +146,11 @@ void moveBasedOnSensor(int sensorNum, int strength)
 
   // Maps the 12 sensors to movement directions
   // North is 12, and we go clockwise (1, 2, 3...)
+
+  display.clearDisplay();
+  display.printMessage("Sensor: " + String(sensorNum), 0, 10);
+  display.printMessage("Strength: " + String(strength), 0, 30);
+  display.sendBuffer();
   switch (sensorNum)
   {
   case 12: // North - move forward
@@ -203,9 +218,12 @@ void moveBasedOnSensor(int sensorNum, int strength)
 
   // Adjust movement duration based on signal strength
   // Closer (lower value) = shorter movements
+
+  /* Smooth like butter
   int moveDuration = map(strength, 0, 255, 200, 800);
   delay(moveDuration);
   motors.stop();
+  */
 }
 
 void trackBall()
@@ -259,25 +277,79 @@ void trackBall()
 
 void searchForBall()
 {
+  static int searchDirection = 1; // 1 for clockwise, -1 for counter-clockwise
+  static unsigned long lastSearchChange = 0;
+  static int searchPhase = 0; // 0-3: increases search area over time
+  
   Serial.println("Searching for ball...");
+  display.clearDisplay();
+  display.printMessage("Searching for ball", 0, 10);
+  display.sendBuffer();
 
-  // If we previously detected the ball, turn toward that direction first
-  if (lastBallSensor != -1)
+  // If we previously detected the ball, first try turning toward that direction
+  if (lastBallSensor != -1 && millis() - lastBallDetectionTime < 5000) // Only use last position if recent
   {
+    Serial.print("Searching in last known direction: Sensor ");
+    Serial.println(lastBallSensor);
+    
     // Use the last known direction to guide search
-    moveBasedOnSensor(lastBallSensor, 200); // Use medium strength value for searching
-  }
-  else
-  {
-    // Execute a search pattern - rotate slowly clockwise
-    // Use public methods instead of accessing private motor members
-    // This will rotate the robot clockwise to scan for the ball
-    motors.rotateToAngle(10); // Rotate a small amount (10 degrees)
-
-    delay(300); // Short rotation
+    moveBasedOnSensor(lastBallSensor, 200); // Use medium strength value
+    delay(300); // Move briefly in that direction
     motors.stop();
-    delay(300); // Pause to get sensor readings
+    
+    // Check for ball after moving
+    Wire.requestFrom(0x10 / 2, 2);
+    if (Wire.available() >= 2) {
+      int sensorNum = Wire.read();
+      int strength = Wire.read();
+      
+      if (sensorNum >= 1 && sensorNum <= 12) {
+        // Ball found! Update tracking info
+        lastBallSensor = sensorNum;
+        lastBallStrength = strength;
+        lastBallDetectionTime = millis();
+        ballLost = false;
+        return; // Exit search function as we found the ball
+      }
+    }
   }
+  
+  // Systematic search pattern
+  // Change direction periodically to avoid getting stuck
+  if (millis() - lastSearchChange > 5000) {
+    searchDirection *= -1; // Reverse direction
+    searchPhase = (searchPhase + 1) % 4; // Cycle through search phases
+    lastSearchChange = millis();
+  }
+  
+  motors.rotate360();
+  
+  // Short rotation then check
+  delay(100);
+  motors.stop();
+  
+  // Poll for ball after rotation
+  Wire.requestFrom(0x10 / 2, 2);
+  if (Wire.available() >= 2) {
+    int sensorNum = Wire.read();
+    int strength = Wire.read();
+    
+    if (sensorNum >= 1 && sensorNum <= 12) {
+      // Ball found! Update tracking info
+      lastBallSensor = sensorNum;
+      lastBallStrength = strength;
+      lastBallDetectionTime = millis();
+      ballLost = false;
+      
+      Serial.print("Ball found at sensor ");
+      Serial.print(sensorNum);
+      Serial.print(" with strength ");
+      Serial.println(strength);
+    }
+  }
+  
+  // Small delay before next search increment
+  delay(100);
 }
 
 void loop()
@@ -336,6 +408,10 @@ void loop()
 
       lastDebugPrint = millis();
     }
+    long RangeInCentimeters;
+    RangeInCentimeters = uSensor1.MeasureInCentimeters(); // two measurements should keep an interval
+    Serial.print(RangeInCentimeters);//0~400cm
+    Serial.println(" cm");
 
     // Ball tracking or searching
     if (!ballLost)