Refactor main.cpp to adjust ball detection threshold, clean up commented code, and add IR seeker test functionality

src/main.cpp

@@ -1,7 +1,7 @@
 #include <Arduino.h>
 #include <Wire.h>
 #include "Adafruit_BNO08x_RVC.h"
-// #include <PixyI2C.h>  // Commented out
+#include <PixyI2C.h>    // 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 uSensor2 = Ultrasonic(10); // Commented out
-// Ultrasonic uSensor3 = Ultrasonic(9);  // 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) {
+  if (millis() - lastSearchChange > 5000)
-    searchDirection *= -1; // Reverse direction
+  {
+    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);
-}
+}
+
+// Very bad way to stop but the only way to stop the robot
+void stop()
+{
+  while (1);
+}