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

Refactor main.cpp to adjust ball detection threshold, clean up commented code, and add IR seeker test functionality

Browse source
This commit is contained in:
Andrea Moro 2025-03-07 10:26:28 +01:00
parent 5d0a067fba
commit 76f1105a8f
1 changed files with 63 additions and 35 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

98
src/main.cpp
View file

@ -1,7 +1,7 @@
#include <Arduino.h>
#include <Wire.h>
#include "Adafruit_BNO08x_RVC.h"
// #include <PixyI2C.h> // Commented out
#include <PixyI2C.h> // Commented out
#include "Ultrasonic.h" // Commented out
#include "OmniWheelMotors.hpp"
#include "DisplayLib.hpp"
@ -9,7 +9,7 @@
// Constants
#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_CLOSE_THRESHOLD 30 // 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
@ -21,11 +21,11 @@
#define MOTOR3_L_PWM 11
// Sensor objects
// PixyI2C pixy; // Commented out
PixyI2C pixy;
Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();
Ultrasonic uSensor1 = Ultrasonic(7);
// Ultrasonic uSensor2 = Ultrasonic(10); // Commented out
// Ultrasonic uSensor3 = Ultrasonic(9); // Commented out
Ultrasonic uSensor2 = Ultrasonic(10); // Commented out
Ultrasonic uSensor3 = Ultrasonic(9); // Commented out
// Motor control object
OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM,
@ -50,7 +50,7 @@ void setup()
delay(10);
Serial.println("ElBestia Robot Starting...");
// Initialize display
display.initDisplay();
display.welcomeScreen();
@ -71,15 +71,9 @@ void setup()
// Initialize motors
motors.begin();
motors.setSpeedLimits(150, 255); // Set min and max speeds
motors.setRotationTime(2000); // Set time for full rotation (2 seconds = 360°)
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);
// Manual motor test
analogWrite(MOTOR1_R_PWM, MOTOR_SPEED);
delay(500);
@ -105,6 +99,27 @@ void setup()
delay(500);
analogWrite(MOTOR3_L_PWM, 0);
// Test the IR Seeker
Wire.requestFrom(0x10 / 2, 2);
if (Wire.available())
{
int sensorNum = Wire.read();
int strength = Wire.read();
Serial.print("IR Seeker - Sensor: ");
Serial.print(sensorNum);
Serial.print(", Strength: ");
Serial.println(strength);
}
else
{
Serial.println("IR Seeker not found");
Serial.println("Stopping robot...");
display.clearDisplay();
display.displayMessage("IR Seeker not found");
display.displayError("Stopping robot...");
stop();
}
Serial.println("Initialization complete. Waiting for commands...");
// wait for 'start' command from serial
@ -280,7 +295,7 @@ void searchForBall()
static int searchDirection = 1; // 1 for clockwise, -1 for counter-clockwise
static unsigned long lastSearchChange = 0;
static int searchPhase = 0; // 0-3: increases search area over time
Serial.println("Searching for ball...");
display.clearDisplay();
display.printMessage("Searching for ball", 0, 10);
@ -291,19 +306,21 @@ void searchForBall()
{
Serial.print("Searching in last known direction: Sensor ");
Serial.println(lastBallSensor);
// Use the last known direction to guide search
moveBasedOnSensor(lastBallSensor, 200); // Use medium strength value
delay(300); // Move briefly in that direction
delay(300); // Move briefly in that direction
motors.stop();
// Check for ball after moving
Wire.requestFrom(0x10 / 2, 2);
if (Wire.available() >= 2) {
if (Wire.available() >= 2)
{
int sensorNum = Wire.read();
int strength = Wire.read();
if (sensorNum >= 1 && sensorNum <= 12) {
if (sensorNum >= 1 && sensorNum <= 12)
{
// Ball found! Update tracking info
lastBallSensor = sensorNum;
lastBallStrength = strength;
@ -313,41 +330,44 @@ void searchForBall()
}
}
}
// Systematic search pattern
// Change direction periodically to avoid getting stuck
if (millis() - lastSearchChange > 5000) {
searchDirection *= -1; // Reverse direction
if (millis() - lastSearchChange > 5000)
{
searchDirection *= -1; // Reverse direction
searchPhase = (searchPhase + 1) % 4; // Cycle through search phases
lastSearchChange = millis();
}
motors.rotate360();
// Short rotation then check
delay(100);
motors.stop();
// Poll for ball after rotation
Wire.requestFrom(0x10 / 2, 2);
if (Wire.available() >= 2) {
if (Wire.available() >= 2)
{
int sensorNum = Wire.read();
int strength = Wire.read();
if (sensorNum >= 1 && sensorNum <= 12) {
if (sensorNum >= 1 && sensorNum <= 12)
{
// Ball found! Update tracking info
lastBallSensor = sensorNum;
lastBallStrength = strength;
lastBallDetectionTime = millis();
ballLost = false;
Serial.print("Ball found at sensor ");
Serial.print(sensorNum);
Serial.print(" with strength ");
Serial.println(strength);
}
}
// Small delay before next search increment
delay(100);
}
@ -367,7 +387,9 @@ void loop()
robotActive = false;
motors.stop();
Serial.println("Robot stopped! Send 'start' to resume");
} else if (command.equalsIgnoreCase("test")) {
}
else if (command.equalsIgnoreCase("test"))
{
motors.moveForward(100);
delay(1000);
motors.moveBackward(100);
@ -380,7 +402,7 @@ void loop()
delay(1000);
motors.rotateToAngle(-90);
delay(1000);
motors.stop();
motors.stop();
}
else if (command.equalsIgnoreCase("start") && !robotActive)
{
@ -410,7 +432,7 @@ void loop()
}
long RangeInCentimeters;
RangeInCentimeters = uSensor1.MeasureInCentimeters(); // two measurements should keep an interval
Serial.print(RangeInCentimeters);//0~400cm
Serial.print(RangeInCentimeters); // 0~400cm
Serial.println(" cm");
// Ball tracking or searching
@ -431,4 +453,10 @@ void loop()
// Small delay to stabilize readings
delay(50);
}
}
// Very bad way to stop but the only way to stop the robot
void stop()
{
while (1);
}