From 143c128053b809871b3db84d565f90957a87529b Mon Sep 17 00:00:00 2001 From: Andrea Date: Mon, 3 Mar 2025 11:07:05 +0100 Subject: [PATCH] Increase motor speed and add command handling for robot activation in main loop --- src/main.cpp | 317 +++++++++++++++++++++++++++++++-------------------- 1 file changed, 195 insertions(+), 122 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index c93b0be..f31365b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,10 +6,10 @@ #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 +#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 @@ -27,32 +27,36 @@ Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC(); // Ultrasonic uSensor3 = Ultrasonic(9); // Commented out // Motor control object -OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM, - MOTOR2_R_PWM, MOTOR2_L_PWM, +OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM, + MOTOR2_R_PWM, MOTOR2_L_PWM, MOTOR3_R_PWM, MOTOR3_L_PWM); // Global variables int lastBallSensor = -1; -int lastBallStrength = 255; // Start at max distance +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 delay(10); Serial.println("ElBestia Robot Starting..."); - + // Initialize I2C Wire.begin(); - + // 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"); @@ -61,95 +65,117 @@ void setup() { motors.setSpeedLimits(45, 70); // Set min and max speeds 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); - - 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); } - + // 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) { - 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; + switch (sensorNum) + { + 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); @@ -157,90 +183,137 @@ 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()) { - int sensorNum = Wire.read(); // Sensor number (1-12) - int strength = Wire.read(); // Strength of the signal (0-255, 0 is closest) - + if (Wire.available()) + { + 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); - + // 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(); ballLost = false; - + // 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 motors.rotateToAngle(10); // Rotate a small amount (10 degrees) - + delay(300); // Short rotation motors.stop(); delay(300); // Pause to get sensor readings } } -void loop() { +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); + + // 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..."); } - - lastDebugPrint = millis(); } - - // Ball tracking or searching - if (!ballLost) { - trackBall(); - } else { - searchForBall(); + + // Only perform operations when robot is active + if (robotActive) + { + // 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(); + } } - + else + { + // When not active, just wait for commands + delay(100); + } + // Small delay to stabilize readings delay(50); } \ No newline at end of file