andrea/elbestia-robot
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

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

Browse source
This commit is contained in:
Andrea Moro 2025-03-03 11:07:05 +01:00
parent 62d206f792
commit 143c128053
1 changed files with 195 additions and 122 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

279
src/main.cpp
View file

@ -6,10 +6,10 @@
#include "OmniWheelMotors.hpp" #include "OmniWheelMotors.hpp"
// Constants // Constants
#define MOTOR_SPEED 60 #define MOTOR_SPEED 120
#define ROTATION_ADJUSTMENT_SPEED 55 #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 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_TIMEOUT 5000 // Time in ms to consider ball lost if no detection
// Pin definitions for the motors // Pin definitions for the motors
#define MOTOR1_R_PWM 3 #define MOTOR1_R_PWM 3
@ -33,11 +33,13 @@ OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM,
// Global variables // Global variables
int lastBallSensor = -1; int lastBallSensor = -1;
int lastBallStrength = 255; // Start at max distance int lastBallStrength = 255; // Start at max distance
unsigned long lastBallDetectionTime = 0; unsigned long lastBallDetectionTime = 0;
bool ballLost = false; bool ballLost = false;
bool robotActive = false; // Flag to indicate if robot should be operating
void setup() { void setup()
{
// Initialize serial communication // Initialize serial communication
Serial.begin(115200); Serial.begin(115200);
while (!Serial && millis() < 3000) // Wait up to 3 seconds for serial while (!Serial && millis() < 3000) // Wait up to 3 seconds for serial
@ -50,9 +52,11 @@ void setup() {
// Initialize IMU // Initialize IMU
Serial1.begin(115200); Serial1.begin(115200);
if (!rvc.begin(&Serial1)) { if (!rvc.begin(&Serial1))
{
Serial.println("Failed to initialize IMU!"); Serial.println("Failed to initialize IMU!");
while (1) delay(10); while (1)
delay(10);
} }
Serial.println("IMU initialized"); Serial.println("IMU initialized");
@ -68,13 +72,34 @@ void setup() {
motors.stop(); motors.stop();
delay(200); 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 // Get current heading from IMU for reference
BNO08x_RVC_Data heading; BNO08x_RVC_Data heading;
if (rvc.read(&heading)) { if (rvc.read(&heading))
{
float yaw = heading.yaw; float yaw = heading.yaw;
Serial.print("Current Yaw: "); Serial.print("Current Yaw: ");
Serial.println(yaw); Serial.println(yaw);
@ -82,72 +107,73 @@ void moveBasedOnSensor(int sensorNum, int strength) {
// Calculate intensity for movement - closer means slower movement // Calculate intensity for movement - closer means slower movement
// Invert the strength so 0 (close) becomes high intensity and 255 (far) becomes low // 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 // Maps the 12 sensors to movement directions
// North is 12, and we go clockwise (1, 2, 3...) // North is 12, and we go clockwise (1, 2, 3...)
switch (sensorNum) { switch (sensorNum)
case 12: // North - move forward {
Serial.println("Moving forward (North)"); case 12: // North - move forward
motors.moveForward(moveIntensity); Serial.println("Moving forward (North)");
break; motors.moveForward(moveIntensity);
case 1: // North-northeast break;
Serial.println("Moving forward-right (NNE)"); case 1: // North-northeast
motors.moveForward(moveIntensity); Serial.println("Moving forward-right (NNE)");
delay(200); motors.moveForward(moveIntensity);
motors.moveRight(moveIntensity); delay(200);
break; motors.moveRight(moveIntensity);
case 2: // Northeast break;
Serial.println("Moving forward-right (NE)"); case 2: // Northeast
motors.moveForward(moveIntensity/2); Serial.println("Moving forward-right (NE)");
motors.moveRight(moveIntensity); motors.moveForward(moveIntensity / 2);
break; motors.moveRight(moveIntensity);
case 3: // East-northeast break;
Serial.println("Moving right (ENE)"); case 3: // East-northeast
motors.moveRight(moveIntensity); Serial.println("Moving right (ENE)");
break; motors.moveRight(moveIntensity);
case 4: // East break;
Serial.println("Moving right (East)"); case 4: // East
motors.moveRight(moveIntensity); Serial.println("Moving right (East)");
break; motors.moveRight(moveIntensity);
case 5: // East-southeast break;
Serial.println("Moving backward-right (ESE)"); case 5: // East-southeast
motors.moveRight(moveIntensity); Serial.println("Moving backward-right (ESE)");
delay(200); motors.moveRight(moveIntensity);
motors.moveBackward(moveIntensity/2); delay(200);
break; motors.moveBackward(moveIntensity / 2);
case 6: // Southeast break;
Serial.println("Moving backward-right (SE)"); case 6: // Southeast
motors.moveBackward(moveIntensity/2); Serial.println("Moving backward-right (SE)");
motors.moveRight(moveIntensity); motors.moveBackward(moveIntensity / 2);
break; motors.moveRight(moveIntensity);
case 7: // South-southeast break;
Serial.println("Moving backward (SSE)"); case 7: // South-southeast
motors.moveBackward(moveIntensity); Serial.println("Moving backward (SSE)");
break; motors.moveBackward(moveIntensity);
case 8: // South break;
Serial.println("Moving backward (South)"); case 8: // South
motors.moveBackward(moveIntensity); Serial.println("Moving backward (South)");
break; motors.moveBackward(moveIntensity);
case 9: // South-southwest break;
Serial.println("Moving backward-left (SSW)"); case 9: // South-southwest
motors.moveBackward(moveIntensity); Serial.println("Moving backward-left (SSW)");
delay(200); motors.moveBackward(moveIntensity);
motors.moveLeft(moveIntensity/2); delay(200);
break; motors.moveLeft(moveIntensity / 2);
case 10: // Southwest break;
Serial.println("Moving backward-left (SW)"); case 10: // Southwest
motors.moveBackward(moveIntensity/2); Serial.println("Moving backward-left (SW)");
motors.moveLeft(moveIntensity); motors.moveBackward(moveIntensity / 2);
break; motors.moveLeft(moveIntensity);
case 11: // West-southwest break;
Serial.println("Moving left (WSW)"); case 11: // West-southwest
motors.moveLeft(moveIntensity); Serial.println("Moving left (WSW)");
break; motors.moveLeft(moveIntensity);
default: break;
motors.stop(); default:
Serial.println("Unknown sensor direction, stopping"); motors.stop();
break; Serial.println("Unknown sensor direction, stopping");
break;
} }
// Adjust movement duration based on signal strength // Adjust movement duration based on signal strength
@ -157,13 +183,15 @@ void moveBasedOnSensor(int sensorNum, int strength) {
motors.stop(); motors.stop();
} }
void trackBall() { void trackBall()
{
// Get IR ball direction from seeker (on I2C address 0x10 / 2) // Get IR ball direction from seeker (on I2C address 0x10 / 2)
Wire.requestFrom(0x10 / 2, 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) 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("Ball - Sensor: ");
Serial.print(sensorNum); Serial.print(sensorNum);
@ -171,7 +199,8 @@ void trackBall() {
Serial.println(strength); Serial.println(strength);
// Only process if we have a valid sensor number // Only process if we have a valid sensor number
if (sensorNum >= 1 && sensorNum <= 12) { if (sensorNum >= 1 && sensorNum <= 12)
{
lastBallSensor = sensorNum; lastBallSensor = sensorNum;
lastBallStrength = strength; lastBallStrength = strength;
lastBallDetectionTime = millis(); lastBallDetectionTime = millis();
@ -179,32 +208,42 @@ void trackBall() {
// Move based on which sensor sees the ball // Move based on which sensor sees the ball
moveBasedOnSensor(sensorNum, strength); moveBasedOnSensor(sensorNum, strength);
} else { }
else
{
Serial.println("Invalid sensor number from IR seeker"); Serial.println("Invalid sensor number from IR seeker");
// If signal is invalid, check if we lost the ball // If signal is invalid, check if we lost the ball
if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT) { if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT)
{
ballLost = true; ballLost = true;
} }
} }
} else { }
else
{
Serial.println("No data available from IR seeker"); Serial.println("No data available from IR seeker");
// Check if we've lost the ball for too long // Check if we've lost the ball for too long
if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT) { if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT)
{
ballLost = true; ballLost = true;
} }
} }
} }
void searchForBall() { void searchForBall()
{
Serial.println("Searching for ball..."); Serial.println("Searching for ball...");
// If we previously detected the ball, turn toward that direction first // 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 // Use the last known direction to guide search
moveBasedOnSensor(lastBallSensor, 200); // Use medium strength value for searching moveBasedOnSensor(lastBallSensor, 200); // Use medium strength value for searching
} else { }
else
{
// Execute a search pattern - rotate slowly clockwise // Execute a search pattern - rotate slowly clockwise
// Use public methods instead of accessing private motor members // Use public methods instead of accessing private motor members
// This will rotate the robot clockwise to scan for the ball // This will rotate the robot clockwise to scan for the ball
@ -216,29 +255,63 @@ void searchForBall() {
} }
} }
void loop() { void loop()
{
static unsigned long lastDebugPrint = 0; static unsigned long lastDebugPrint = 0;
// Print debug info every second // Check for stop command from serial
if (millis() - lastDebugPrint > 1000) { if (Serial.available() > 0)
BNO08x_RVC_Data heading; {
if (rvc.read(&heading)) { String command = Serial.readStringUntil('\n');
Serial.print("IMU - Yaw: "); command.trim();
Serial.print(heading.yaw);
Serial.print(", Pitch: ");
Serial.print(heading.pitch);
Serial.print(", Roll: ");
Serial.println(heading.roll);
}
lastDebugPrint = millis(); 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...");
}
} }
// Ball tracking or searching // Only perform operations when robot is active
if (!ballLost) { if (robotActive)
trackBall(); {
} else { // Print debug info every second
searchForBall(); 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 // Small delay to stabilize readings