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

317
src/main.cpp
View file

@ -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);
}