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

lib/display/DisplayLib.cpp

@@ -0,0 +1,67 @@
+#include "DisplayLib.hpp"
+
+U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
+
+void DisplayLib::updateDisplay(const char* title, const char* detail) {
+    u8g2.clearBuffer();
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(0, 10, title);
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(0, 30, detail);
+    u8g2.sendBuffer();
+}
+
+void DisplayLib::welcomeScreen() {
+    u8g2.clearBuffer();
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(0, 10, "Welcome to");
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(0, 30, "Elbestia");
+    u8g2.sendBuffer();
+}
+
+void DisplayLib::clearDisplay() {
+    u8g2.clearBuffer();
+    u8g2.sendBuffer();
+}
+
+void DisplayLib::initDisplay() {
+    u8g2.begin();
+    u8g2.enableUTF8Print();
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.clearBuffer();
+    u8g2.sendBuffer();
+}
+
+void DisplayLib::displayError(const char* error) {
+    u8g2.clearBuffer();
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(0, 10, "Error:");
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(0, 30, error);
+    u8g2.sendBuffer();
+}
+
+void DisplayLib::displayMessage(const char* message) {
+    u8g2.clearBuffer();
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(0, 10, message);
+    u8g2.sendBuffer();
+}
+
+void DisplayLib::printMessage(const char* message, int x, int y) {
+    u8g2.clearBuffer();
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(x, y, message);
+    u8g2.sendBuffer();
+}
+
+void DisplayLib::printMessage(const String message, int x, int y) {
+    u8g2.clearBuffer();
+    u8g2.setFont(u8g2_font_ncenB08_tr);
+    u8g2.drawStr(x, y, message.c_str());
+}
+
+void DisplayLib::sendBuffer() {
+    u8g2.sendBuffer();
+}

lib/display/DisplayLib.hpp

@@ -0,0 +1,21 @@
+#ifndef DISPLAYLIB_HPP
+#define DISPLAYLIB_HPP
+
+#include <U8g2lib.h>
+
+class DisplayLib {
+    public: 
+        void updateDisplay(const char* title, const char* detail);
+        void welcomeScreen();
+        void clearDisplay();
+        void initDisplay();
+        void displayError(const char* error);
+        void displayMessage(const char* message);
+        void printMessage(const char* message, int x, int y);
+        void printMessage(const char* message, int x, int y, int size);
+        void printMessage(const String message, int x, int y);
+        void printMessage(const String message, int x, int y, int size);
+        void sendBuffer();
+};
+
+#endif

lib/motor/OmniWheelMotors.cpp

@@ -8,7 +8,7 @@ OmniWheelMotors::OmniWheelMotors(int m1_r_pwm, int m1_l_pwm,
     motor_3 = {m3_r_pwm, m3_l_pwm};
     
     MAX_SPEED = 70;      // Max PWM value
-    MIN_SPEED = 50;      // Default minimum speed
+    MIN_SPEED = 255;      // Default minimum speed
     FULL_ROTATION_TIME = 2000;  // Default time for 360 degrees in milliseconds
 }
 
@@ -85,8 +85,8 @@ void OmniWheelMotors::moveRight(int speed) {
 void OmniWheelMotors::rotate360() {
     // Set motor directions for full rotation
     setMotorSpeed(motor_1, 0, MIN_SPEED);  // Motor 1 forward
-    setMotorSpeed(motor_2, 0, MIN_SPEED);  // Motor 2 forward
-    setMotorSpeed(motor_3, 0, 0);          // Motor 3 neutral
+    setMotorSpeed(motor_2, MIN_SPEED, 0);  // Motor 2 forward
+    setMotorSpeed(motor_3, 0, MIN_SPEED);          // Motor 3 neutral
     
     // Wait for approximately one full rotation
     delay(FULL_ROTATION_TIME);
@@ -95,6 +95,26 @@ void OmniWheelMotors::rotate360() {
     stop();
 }
 
+void OmniWheelMotors::rotateClockwise(int speed) {
+    // Ensure speed is at least minimum speed for stable movement
+    speed = max(speed, MIN_SPEED);
+    
+    // Set motor directions for clockwise rotation
+    setMotorSpeed(motor_1, 0, speed);  // Motor 1 forward
+    setMotorSpeed(motor_2, speed, 0);  // Motor 2 backward
+    setMotorSpeed(motor_3, speed, 0);          // Motor 3 neutral
+}
+
+void OmniWheelMotors::rotateCounterClockwise(int speed) {
+    // Ensure speed is at least minimum speed for stable movement
+    speed = max(speed, MIN_SPEED);
+    
+    // Set motor directions for counter-clockwise rotation
+    setMotorSpeed(motor_1, speed, 0);  // Motor 1 backward
+    setMotorSpeed(motor_2, 0, speed);  // Motor 2 forward
+    setMotorSpeed(motor_3, 0, speed);          // Motor 3 neutral
+}
+
 void OmniWheelMotors::rotateToAngle(int targetAngle) {
     // Ensure angle is between 0 and 360
     targetAngle = targetAngle % 360;

lib/motor/OmniWheelMotors.hpp

@@ -23,7 +23,8 @@ public:
     void moveBackward(int speed);
     void moveLeft(int speed);
     void moveRight(int speed);
-    
+    void rotateClockwise(int speed);
+    void rotateCounterClockwise(int speed);
     // Rotation functions
     void rotate360();
     void rotateToAngle(int targetAngle);

platformio.ini

@@ -15,3 +15,4 @@ framework = arduino
 lib_deps = 
 	adafruit/Adafruit BNO08x RVC@^1.0.2
 	seeed-studio/Grove Ultrasonic Ranger@^1.0.1
+	olikraus/U8g2@^2.36.5

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()
@@ -47,6 +51,10 @@ void setup()
 
   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)