From 76f1105a8ffc7a29328b1fb2d259b8aeb49b0fac Mon Sep 17 00:00:00 2001 From: Andrea Date: Fri, 7 Mar 2025 10:26:28 +0100 Subject: [PATCH] Refactor main.cpp to adjust ball detection threshold, clean up commented code, and add IR seeker test functionality --- src/main.cpp | 98 +++++++++++++++++++++++++++++++++------------------- 1 file changed, 63 insertions(+), 35 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 778d0e8..74f7f90 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,7 +1,7 @@ #include #include #include "Adafruit_BNO08x_RVC.h" -// #include // Commented out +#include // Commented out #include "Ultrasonic.h" // Commented out #include "OmniWheelMotors.hpp" #include "DisplayLib.hpp" @@ -9,7 +9,7 @@ // Constants #define MOTOR_SPEED 120 #define ROTATION_ADJUSTMENT_SPEED 55 -#define SIGNAL_CLOSE_THRESHOLD 50 // Threshold for considering ball close (0 is closest, 255 is farthest) +#define SIGNAL_CLOSE_THRESHOLD 30 // Threshold for considering ball close (0 is closest, 255 is farthest) #define SIGNAL_TIMEOUT 5000 // Time in ms to consider ball lost if no detection // Pin definitions for the motors @@ -21,11 +21,11 @@ #define MOTOR3_L_PWM 11 // Sensor objects -// PixyI2C pixy; // Commented out +PixyI2C pixy; Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC(); Ultrasonic uSensor1 = Ultrasonic(7); -// Ultrasonic uSensor2 = Ultrasonic(10); // Commented out -// Ultrasonic uSensor3 = Ultrasonic(9); // Commented out +Ultrasonic uSensor2 = Ultrasonic(10); // Commented out +Ultrasonic uSensor3 = Ultrasonic(9); // Commented out // Motor control object OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM, @@ -50,7 +50,7 @@ void setup() delay(10); Serial.println("ElBestia Robot Starting..."); - + // Initialize display display.initDisplay(); display.welcomeScreen(); @@ -71,15 +71,9 @@ void setup() // Initialize motors motors.begin(); motors.setSpeedLimits(150, 255); // Set min and max speeds - motors.setRotationTime(2000); // Set time for full rotation (2 seconds = 360°) + motors.setRotationTime(2000); // Set time for full rotation (2 seconds = 360°) Serial.println("Motors initialized"); - // Brief motor test - motors.moveForward(MOTOR_SPEED); - delay(500); - motors.stop(); - delay(200); - // Manual motor test analogWrite(MOTOR1_R_PWM, MOTOR_SPEED); delay(500); @@ -105,6 +99,27 @@ void setup() delay(500); analogWrite(MOTOR3_L_PWM, 0); + // Test the IR Seeker + Wire.requestFrom(0x10 / 2, 2); + if (Wire.available()) + { + int sensorNum = Wire.read(); + int strength = Wire.read(); + Serial.print("IR Seeker - Sensor: "); + Serial.print(sensorNum); + Serial.print(", Strength: "); + Serial.println(strength); + } + else + { + Serial.println("IR Seeker not found"); + Serial.println("Stopping robot..."); + display.clearDisplay(); + display.displayMessage("IR Seeker not found"); + display.displayError("Stopping robot..."); + stop(); + } + Serial.println("Initialization complete. Waiting for commands..."); // wait for 'start' command from serial @@ -280,7 +295,7 @@ 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); @@ -291,19 +306,21 @@ void searchForBall() { 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 - delay(300); // Move briefly in that direction + delay(300); // Move briefly in that direction motors.stop(); - + // Check for ball after moving Wire.requestFrom(0x10 / 2, 2); - if (Wire.available() >= 2) { + if (Wire.available() >= 2) + { int sensorNum = Wire.read(); int strength = Wire.read(); - - if (sensorNum >= 1 && sensorNum <= 12) { + + if (sensorNum >= 1 && sensorNum <= 12) + { // Ball found! Update tracking info lastBallSensor = sensorNum; lastBallStrength = strength; @@ -313,41 +330,44 @@ void searchForBall() } } } - + // Systematic search pattern // Change direction periodically to avoid getting stuck - if (millis() - lastSearchChange > 5000) { - searchDirection *= -1; // Reverse direction + 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) { + if (Wire.available() >= 2) + { int sensorNum = Wire.read(); int strength = Wire.read(); - - if (sensorNum >= 1 && sensorNum <= 12) { + + 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); } @@ -367,7 +387,9 @@ void loop() robotActive = false; motors.stop(); Serial.println("Robot stopped! Send 'start' to resume"); - } else if (command.equalsIgnoreCase("test")) { + } + else if (command.equalsIgnoreCase("test")) + { motors.moveForward(100); delay(1000); motors.moveBackward(100); @@ -380,7 +402,7 @@ void loop() delay(1000); motors.rotateToAngle(-90); delay(1000); - motors.stop(); + motors.stop(); } else if (command.equalsIgnoreCase("start") && !robotActive) { @@ -410,7 +432,7 @@ void loop() } long RangeInCentimeters; RangeInCentimeters = uSensor1.MeasureInCentimeters(); // two measurements should keep an interval - Serial.print(RangeInCentimeters);//0~400cm + Serial.print(RangeInCentimeters); // 0~400cm Serial.println(" cm"); // Ball tracking or searching @@ -431,4 +453,10 @@ void loop() // Small delay to stabilize readings delay(50); -} \ No newline at end of file +} + +// Very bad way to stop but the only way to stop the robot +void stop() +{ + while (1); +}