Increase motor speed and add command handling for robot activation in main loop

src/main.cpp

@@ -6,7 +6,7 @@
 #include "OmniWheelMotors.hpp"
 
 // Constants
-#define MOTOR_SPEED 60
+#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_TIMEOUT 5000       // Time in ms to consider ball lost if no detection
@@ -36,8 +36,10 @@ int lastBallSensor = -1;
 int lastBallStrength = 255; // Start at max distance
 unsigned long lastBallDetectionTime = 0;
 bool ballLost = false;
+bool robotActive = false; // Flag to indicate if robot should be operating
 
-void setup() {
+void setup()
+{
   // Initialize serial communication
   Serial.begin(115200);
   while (!Serial && millis() < 3000) // Wait up to 3 seconds for serial
@@ -50,9 +52,11 @@ void setup() {
 
   // Initialize IMU
   Serial1.begin(115200);
-  if (!rvc.begin(&Serial1)) {
+  if (!rvc.begin(&Serial1))
+  {
     Serial.println("Failed to initialize IMU!");
-    while (1) delay(10);
+    while (1)
+      delay(10);
   }
   Serial.println("IMU initialized");
 
@@ -68,13 +72,34 @@ void setup() {
   motors.stop();
   delay(200);
 
-  Serial.println("Initialization complete. Starting main loop.");
+  Serial.println("Initialization complete. Waiting for commands...");
+
+  // wait for 'start' command from serial
+  // wait for 'start' command from serial
+  Serial.println("Send 'start' to begin operation or 'stop' to halt");
+  while (!robotActive)
+  {
+    if (Serial.available() > 0)
+    {
+      String command = Serial.readStringUntil('\n');
+      command.trim(); // Remove any whitespace
+
+      if (command.equalsIgnoreCase("start"))
+      {
+        robotActive = true;
+        Serial.println("Robot activated! Starting operation...");
+      }
+    }
+    delay(100); // Small delay to avoid consuming too much CPU while waiting
+  }
 }
 
-void moveBasedOnSensor(int sensorNum, int strength) {
+void moveBasedOnSensor(int sensorNum, int strength)
+{
   // Get current heading from IMU for reference
   BNO08x_RVC_Data heading;
-  if (rvc.read(&heading)) {
+  if (rvc.read(&heading))
+  {
     float yaw = heading.yaw;
     Serial.print("Current Yaw: ");
     Serial.println(yaw);
@@ -82,11 +107,12 @@ void moveBasedOnSensor(int sensorNum, int strength) {
 
   // Calculate intensity for movement - closer means slower movement
   // Invert the strength so 0 (close) becomes high intensity and 255 (far) becomes low
-  int moveIntensity = map(strength, 0, 255, MOTOR_SPEED, MOTOR_SPEED/2);
+  int moveIntensity = map(strength, 0, 255, MOTOR_SPEED, MOTOR_SPEED / 2);
 
   // Maps the 12 sensors to movement directions
   // North is 12, and we go clockwise (1, 2, 3...)
-  switch (sensorNum) {
+  switch (sensorNum)
+  {
   case 12: // North - move forward
     Serial.println("Moving forward (North)");
     motors.moveForward(moveIntensity);
@@ -99,7 +125,7 @@ void moveBasedOnSensor(int sensorNum, int strength) {
     break;
   case 2: // Northeast
     Serial.println("Moving forward-right (NE)");
-      motors.moveForward(moveIntensity/2);
+    motors.moveForward(moveIntensity / 2);
     motors.moveRight(moveIntensity);
     break;
   case 3: // East-northeast
@@ -114,11 +140,11 @@ void moveBasedOnSensor(int sensorNum, int strength) {
     Serial.println("Moving backward-right (ESE)");
     motors.moveRight(moveIntensity);
     delay(200);
-      motors.moveBackward(moveIntensity/2);
+    motors.moveBackward(moveIntensity / 2);
     break;
   case 6: // Southeast
     Serial.println("Moving backward-right (SE)");
-      motors.moveBackward(moveIntensity/2);
+    motors.moveBackward(moveIntensity / 2);
     motors.moveRight(moveIntensity);
     break;
   case 7: // South-southeast
@@ -133,11 +159,11 @@ void moveBasedOnSensor(int sensorNum, int strength) {
     Serial.println("Moving backward-left (SSW)");
     motors.moveBackward(moveIntensity);
     delay(200);
-      motors.moveLeft(moveIntensity/2);
+    motors.moveLeft(moveIntensity / 2);
     break;
   case 10: // Southwest
     Serial.println("Moving backward-left (SW)");
-      motors.moveBackward(moveIntensity/2);
+    motors.moveBackward(moveIntensity / 2);
     motors.moveLeft(moveIntensity);
     break;
   case 11: // West-southwest
@@ -157,11 +183,13 @@ void moveBasedOnSensor(int sensorNum, int strength) {
   motors.stop();
 }
 
-void trackBall() {
+void trackBall()
+{
   // Get IR ball direction from seeker (on I2C address 0x10 / 2)
   Wire.requestFrom(0x10 / 2, 2);
 
-  if (Wire.available()) {
+  if (Wire.available())
+  {
     int sensorNum = Wire.read(); // Sensor number (1-12)
     int strength = Wire.read();  // Strength of the signal (0-255, 0 is closest)
 
@@ -171,7 +199,8 @@ void trackBall() {
     Serial.println(strength);
 
     // Only process if we have a valid sensor number
-    if (sensorNum >= 1 && sensorNum <= 12) {
+    if (sensorNum >= 1 && sensorNum <= 12)
+    {
       lastBallSensor = sensorNum;
       lastBallStrength = strength;
       lastBallDetectionTime = millis();
@@ -179,32 +208,42 @@ void trackBall() {
 
       // Move based on which sensor sees the ball
       moveBasedOnSensor(sensorNum, strength);
-    } else {
+    }
+    else
+    {
       Serial.println("Invalid sensor number from IR seeker");
 
       // If signal is invalid, check if we lost the ball
-      if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT) {
+      if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT)
+      {
         ballLost = true;
       }
     }
-  } else {
+  }
+  else
+  {
     Serial.println("No data available from IR seeker");
 
     // Check if we've lost the ball for too long
-    if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT) {
+    if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT)
+    {
       ballLost = true;
     }
   }
 }
 
-void searchForBall() {
+void searchForBall()
+{
   Serial.println("Searching for ball...");
 
   // If we previously detected the ball, turn toward that direction first
-  if (lastBallSensor != -1) {
+  if (lastBallSensor != -1)
+  {
     // Use the last known direction to guide search
     moveBasedOnSensor(lastBallSensor, 200); // Use medium strength value for searching
-  } else {
+  }
+  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
@@ -216,13 +255,38 @@ void searchForBall() {
   }
 }
 
-void loop() {
+void loop()
+{
   static unsigned long lastDebugPrint = 0;
 
+  // Check for stop command from serial
+  if (Serial.available() > 0)
+  {
+    String command = Serial.readStringUntil('\n');
+    command.trim();
+
+    if (command.equalsIgnoreCase("stop"))
+    {
+      robotActive = false;
+      motors.stop();
+      Serial.println("Robot stopped! Send 'start' to resume");
+    }
+    else if (command.equalsIgnoreCase("start") && !robotActive)
+    {
+      robotActive = true;
+      Serial.println("Robot activated! Resuming operation...");
+    }
+  }
+
+  // Only perform operations when robot is active
+  if (robotActive)
+  {
     // Print debug info every second
-  if (millis() - lastDebugPrint > 1000) {
+    if (millis() - lastDebugPrint > 1000)
+    {
       BNO08x_RVC_Data heading;
-    if (rvc.read(&heading)) {
+      if (rvc.read(&heading))
+      {
         Serial.print("IMU - Yaw: ");
         Serial.print(heading.yaw);
         Serial.print(", Pitch: ");
@@ -235,11 +299,20 @@ void loop() {
     }
 
     // Ball tracking or searching
-  if (!ballLost) {
+    if (!ballLost)
+    {
       trackBall();
-  } else {
+    }
+    else
+    {
       searchForBall();
     }
+  }
+  else
+  {
+    // When not active, just wait for commands
+    delay(100);
+  }
 
   // Small delay to stabilize readings
   delay(50);