diff --git a/lib/display/DisplayLib.cpp b/lib/display/DisplayLib.cpp new file mode 100644 index 0000000..68836af --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/lib/display/DisplayLib.hpp b/lib/display/DisplayLib.hpp new file mode 100644 index 0000000..a725616 --- /dev/null +++ b/lib/display/DisplayLib.hpp @@ -0,0 +1,21 @@ +#ifndef DISPLAYLIB_HPP +#define DISPLAYLIB_HPP + +#include + +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 \ No newline at end of file diff --git a/lib/motor/OmniWheelMotors.cpp b/lib/motor/OmniWheelMotors.cpp index faf4dc4..80d391d 100644 --- a/lib/motor/OmniWheelMotors.cpp +++ b/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; diff --git a/lib/motor/OmniWheelMotors.hpp b/lib/motor/OmniWheelMotors.hpp index 1b6b139..3518926 100644 --- a/lib/motor/OmniWheelMotors.hpp +++ b/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); diff --git a/platformio.ini b/platformio.ini index 7ddb981..ab0222a 100644 --- a/platformio.ini +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index 153468a..778d0e8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,8 +2,9 @@ #include #include "Adafruit_BNO08x_RVC.h" // #include // 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)