Refactor main.cpp to integrate OmniWheelMotors for improved motor control and remove unused sensor code

src/main.cpp

@@ -1,116 +1,246 @@
 #include <Arduino.h>
 #include <Wire.h>
 #include "Adafruit_BNO08x_RVC.h"
-#include <PixyI2C.h>
+// #include <PixyI2C.h>  // Commented out
-#include "Ultrasonic.h"
+// #include "Ultrasonic.h" // Commented out
+#include "OmniWheelMotors.hpp"
 
-PixyI2C pixy;
+// Constants
+#define MOTOR_SPEED 60
+#define ROTATION_ADJUSTMENT_SPEED 55
+#define SIGNAL_CLOSE_THRESHOLD 50  // 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
+#define MOTOR1_R_PWM 3
+#define MOTOR1_L_PWM 5
+#define MOTOR2_R_PWM 6
+#define MOTOR2_L_PWM 9
+#define MOTOR3_R_PWM 10
+#define MOTOR3_L_PWM 11
+
+// Sensor objects
+// PixyI2C pixy;  // Commented out
 Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();
-Ultrasonic uSensor1 = Ultrasonic(7);
+// Ultrasonic uSensor1 = Ultrasonic(7);  // Commented out
-Ultrasonic uSensor2 = Ultrasonic(10);
+// Ultrasonic uSensor2 = Ultrasonic(10); // Commented out
-Ultrasonic uSensor3 = Ultrasonic(9);
+// Ultrasonic uSensor3 = Ultrasonic(9);  // Commented out
 
-void setup()
+// Motor control object
-{
+OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM, 
-  Wire.begin();
+                       MOTOR2_R_PWM, MOTOR2_L_PWM, 
-  pixy.init();
+                       MOTOR3_R_PWM, MOTOR3_L_PWM);
 
-  // Wait for serial monitor to open
+// Global variables
+int lastBallSensor = -1;
+int lastBallStrength = 255;  // Start at max distance
+unsigned long lastBallDetectionTime = 0;
+bool ballLost = true;
+
+void setup() {
+  // Initialize serial communication
   Serial.begin(115200);
-  while (!Serial)
+  while (!Serial && millis() < 3000) // Wait up to 3 seconds for serial
     delay(10);
 
-  Serial1.begin(115200); // This is the baud rate specified by the datasheet
+  Serial.println("ElBestia Robot Starting...");
-  while (!Serial1)
-    delay(10);
   
-  if (!rvc.begin(&Serial1))
+  // Initialize I2C
-  { // connect to the sensor over hardware serial
+  Wire.begin();
-    while (1)
+  
-      delay(10);
+  // Initialize IMU
+  Serial1.begin(115200);
+  if (!rvc.begin(&Serial1)) {
+    Serial.println("Failed to initialize IMU!");
+    while (1) delay(10);
   }
-  // Print labels for the values
+  Serial.println("IMU initialized");
-  Serial.print(F("Yaw"));
+
-  Serial.print(F("\tPitch"));
+  // Initialize motors
-  Serial.print(F("\tRoll"));
+  motors.begin();
-  Serial.print(F("\tX"));
+  motors.setSpeedLimits(45, 70); // Set min and max speeds
-  Serial.print(F("\tY"));
+  motors.setRotationTime(2000);  // Set time for full rotation (2 seconds = 360°)
-  Serial.println(F("\tZ"));
+  Serial.println("Motors initialized");
+  
+  // Brief motor test
+  motors.moveForward(MOTOR_SPEED);
+  delay(500);
+  motors.stop();
+  delay(200);
+  
+  Serial.println("Initialization complete. Starting main loop.");
 }
 
-void loop()
+void moveBasedOnSensor(int sensorNum, int strength) {
-{
+  // Get current heading from IMU for reference
-  // Get IMU data
-  Wire.requestFrom(0x10 / 2, 2);
   BNO08x_RVC_Data heading;
-  static int i = 0;
+  if (rvc.read(&heading)) {
-  uint16_t blocks;
+    float yaw = heading.yaw;
-  char buf[32];
+    Serial.print("Current Yaw: ");
+    Serial.println(yaw);
+  }
   
-  while (rvc.read(&heading))
+  // Calculate intensity for movement - closer means slower movement
-  {
+  // Invert the strength so 0 (close) becomes high intensity and 255 (far) becomes low
-    int c = Wire.read(); // direction is the first byte
+  int moveIntensity = map(strength, 0, 255, MOTOR_SPEED, MOTOR_SPEED/2);
-    int strength = Wire.read();
+  
-    // Print IMU data
+  // Maps the 12 sensors to movement directions
-    Serial.print("IMU - Direction: ");
+  // North is 12, and we go clockwise (1, 2, 3...)
-    Serial.print(c);
+  switch (sensorNum) {
-    Serial.print(" Strength: ");
+    case 12: // North - move forward
+      Serial.println("Moving forward (North)");
+      motors.moveForward(moveIntensity);
+      break;
+    case 1: // North-northeast
+      Serial.println("Moving forward-right (NNE)");
+      motors.moveForward(moveIntensity);
+      delay(200);
+      motors.moveRight(moveIntensity);
+      break;
+    case 2: // Northeast
+      Serial.println("Moving forward-right (NE)");
+      motors.moveForward(moveIntensity/2);
+      motors.moveRight(moveIntensity);
+      break;
+    case 3: // East-northeast
+      Serial.println("Moving right (ENE)");
+      motors.moveRight(moveIntensity);
+      break;
+    case 4: // East
+      Serial.println("Moving right (East)");
+      motors.moveRight(moveIntensity);
+      break;
+    case 5: // East-southeast
+      Serial.println("Moving backward-right (ESE)");
+      motors.moveRight(moveIntensity);
+      delay(200);
+      motors.moveBackward(moveIntensity/2);
+      break;
+    case 6: // Southeast
+      Serial.println("Moving backward-right (SE)");
+      motors.moveBackward(moveIntensity/2);
+      motors.moveRight(moveIntensity);
+      break;
+    case 7: // South-southeast
+      Serial.println("Moving backward (SSE)");
+      motors.moveBackward(moveIntensity);
+      break;
+    case 8: // South
+      Serial.println("Moving backward (South)");
+      motors.moveBackward(moveIntensity);
+      break;
+    case 9: // South-southwest
+      Serial.println("Moving backward-left (SSW)");
+      motors.moveBackward(moveIntensity);
+      delay(200);
+      motors.moveLeft(moveIntensity/2);
+      break;
+    case 10: // Southwest
+      Serial.println("Moving backward-left (SW)");
+      motors.moveBackward(moveIntensity/2);
+      motors.moveLeft(moveIntensity);
+      break;
+    case 11: // West-southwest
+      Serial.println("Moving left (WSW)");
+      motors.moveLeft(moveIntensity);
+      break;
+    default:
+      motors.stop();
+      Serial.println("Unknown sensor direction, stopping");
+      break;
+  }
+  
+  // Adjust movement duration based on signal strength
+  // Closer (lower value) = shorter movements
+  int moveDuration = map(strength, 0, 255, 200, 800);
+  delay(moveDuration);
+  motors.stop();
+}
+
+void trackBall() {
+  // Get IR ball direction from seeker (on I2C address 0x08)
+  Wire.requestFrom(0x08, 2);
+  
+  if (Wire.available() >= 2) {
+    int sensorNum = Wire.read();  // Sensor number (1-12)
+    int strength = Wire.read();   // Strength of the signal (0-255, 0 is closest)
+    
+    Serial.print("Ball - Sensor: ");
+    Serial.print(sensorNum);
+    Serial.print(", Strength: ");
     Serial.println(strength);
-    Serial.print("Heading: ");
-    Serial.print(heading.yaw);
-    Serial.print(",");
-    Serial.print(heading.pitch);
-    Serial.print(",");
-    Serial.print(heading.roll);
-    Serial.println();
-    Serial.print("Acceleration: ");
-    Serial.print(heading.x_accel);
-    Serial.print(",");
-    Serial.print(heading.y_accel);
-    Serial.print(",");
-    Serial.print(heading.z_accel);
-    Serial.println();
     
-    // Get Ultrasonic sensor data
+    // Only process if we have a valid sensor number
-    long uS1 = uSensor1.MeasureInCentimeters();
+    if (sensorNum >= 1 && sensorNum <= 12) {
-    Serial.print("Distance uS1: ");
+      lastBallSensor = sensorNum;
-    Serial.print(uS1);
+      lastBallStrength = strength;
-    Serial.println(" cm");
+      lastBallDetectionTime = millis();
+      ballLost = false;
       
-    long uS2 = uSensor2.MeasureInCentimeters();
+      // Move based on which sensor sees the ball
-    Serial.print("Distance uS2: ");
+      moveBasedOnSensor(sensorNum, strength);
-    Serial.print(uS2);
+    } else {
-    Serial.println(" cm");
+      Serial.println("Invalid sensor number from IR seeker");
       
-    long uS3 = uSensor3.MeasureInCentimeters();
+      // If signal is invalid, check if we lost the ball
-    Serial.print("Distance uS3: ");
+      if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT) {
-    Serial.print(uS3);
+        ballLost = true;
-    Serial.println(" cm");
-
-    // Get Pixy blocks
-    blocks = pixy.getBlocks();
-
-    if (blocks)
-    {
-      Serial.println("Detected blocks");
-      i++;
-      // Print every 50 frames to avoid overwhelming serial
-      if (i % 50 == 0)
-      {
-
-        // Print Pixy blocks
-        sprintf(buf, "Detected %d blocks:\n", blocks);
-        Serial.print(buf);
-        for (int j = 0; j < blocks; j++)
-        {
-          sprintf(buf, "  block %d: ", j);
-          Serial.print(buf);
-          pixy.blocks[j].print();
-        }
-        Serial.println();
       }
+    }
+  } else {
+    Serial.println("No data available from IR seeker");
     
-      delay(250);
+    // Check if we've lost the ball for too long
+    if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT) {
+      ballLost = true;
     }
   }
 }
+
+void searchForBall() {
+  Serial.println("Searching for ball...");
+  
+  // If we previously detected the ball, turn toward that direction first
+  if (lastBallSensor != -1) {
+    // 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
+    motors.stop();
+    delay(300); // Pause to get sensor readings
+  }
+}
+
+void loop() {
+  static unsigned long lastDebugPrint = 0;
+  
+  // Print debug info every second
+  if (millis() - lastDebugPrint > 1000) {
+    BNO08x_RVC_Data heading;
+    if (rvc.read(&heading)) {
+      Serial.print("IMU - Yaw: ");
+      Serial.print(heading.yaw);
+      Serial.print(", Pitch: ");
+      Serial.print(heading.pitch);
+      Serial.print(", Roll: ");
+      Serial.println(heading.roll);
+    }
+    
+    lastDebugPrint = millis();
+  }
+  
+  // Ball tracking or searching
+  if (!ballLost) {
+    trackBall();
+  } else {
+    searchForBall();
+  }
+  
+  // Small delay to stabilize readings
+  delay(50);
+}