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

62
src/main.cpp
View file

@ -1,7 +1,7 @@
#include <Arduino.h> #include <Arduino.h>
#include <Wire.h> #include <Wire.h>
#include "Adafruit_BNO08x_RVC.h" #include "Adafruit_BNO08x_RVC.h"
// #include <PixyI2C.h> // Commented out #include <PixyI2C.h> // Commented out
#include "Ultrasonic.h" // Commented out #include "Ultrasonic.h" // Commented out
#include "OmniWheelMotors.hpp" #include "OmniWheelMotors.hpp"
#include "DisplayLib.hpp" #include "DisplayLib.hpp"
@ -9,7 +9,7 @@
// Constants // Constants
#define MOTOR_SPEED 120 #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 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 #define SIGNAL_TIMEOUT 5000 // Time in ms to consider ball lost if no detection
// Pin definitions for the motors // Pin definitions for the motors
@ -21,11 +21,11 @@
#define MOTOR3_L_PWM 11 #define MOTOR3_L_PWM 11
// Sensor objects // Sensor objects
// PixyI2C pixy; // Commented out PixyI2C pixy;
Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC(); Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();
Ultrasonic uSensor1 = Ultrasonic(7); Ultrasonic uSensor1 = Ultrasonic(7);
// Ultrasonic uSensor2 = Ultrasonic(10); // Commented out Ultrasonic uSensor2 = Ultrasonic(10); // Commented out
// Ultrasonic uSensor3 = Ultrasonic(9); // Commented out Ultrasonic uSensor3 = Ultrasonic(9); // Commented out
// Motor control object // Motor control object
OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM, OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM,
@ -74,12 +74,6 @@ void setup()
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"); Serial.println("Motors initialized");
// Brief motor test
motors.moveForward(MOTOR_SPEED);
delay(500);
motors.stop();
delay(200);
// Manual motor test // Manual motor test
analogWrite(MOTOR1_R_PWM, MOTOR_SPEED); analogWrite(MOTOR1_R_PWM, MOTOR_SPEED);
delay(500); delay(500);
@ -105,6 +99,27 @@ void setup()
delay(500); delay(500);
analogWrite(MOTOR3_L_PWM, 0); 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..."); Serial.println("Initialization complete. Waiting for commands...");
// wait for 'start' command from serial // wait for 'start' command from serial
@ -299,11 +314,13 @@ void searchForBall()
// Check for ball after moving // Check for ball after moving
Wire.requestFrom(0x10 / 2, 2); Wire.requestFrom(0x10 / 2, 2);
if (Wire.available() >= 2) { if (Wire.available() >= 2)
{
int sensorNum = Wire.read(); int sensorNum = Wire.read();
int strength = Wire.read(); int strength = Wire.read();
if (sensorNum >= 1 && sensorNum <= 12) { if (sensorNum >= 1 && sensorNum <= 12)
{
// Ball found! Update tracking info // Ball found! Update tracking info
lastBallSensor = sensorNum; lastBallSensor = sensorNum;
lastBallStrength = strength; lastBallStrength = strength;
@ -316,7 +333,8 @@ void searchForBall()
// Systematic search pattern // Systematic search pattern
// Change direction periodically to avoid getting stuck // Change direction periodically to avoid getting stuck
if (millis() - lastSearchChange > 5000) { if (millis() - lastSearchChange > 5000)
{
searchDirection *= -1; // Reverse direction searchDirection *= -1; // Reverse direction
searchPhase = (searchPhase + 1) % 4; // Cycle through search phases searchPhase = (searchPhase + 1) % 4; // Cycle through search phases
lastSearchChange = millis(); lastSearchChange = millis();
@ -330,11 +348,13 @@ void searchForBall()
// Poll for ball after rotation // Poll for ball after rotation
Wire.requestFrom(0x10 / 2, 2); Wire.requestFrom(0x10 / 2, 2);
if (Wire.available() >= 2) { if (Wire.available() >= 2)
{
int sensorNum = Wire.read(); int sensorNum = Wire.read();
int strength = Wire.read(); int strength = Wire.read();
if (sensorNum >= 1 && sensorNum <= 12) { if (sensorNum >= 1 && sensorNum <= 12)
{
// Ball found! Update tracking info // Ball found! Update tracking info
lastBallSensor = sensorNum; lastBallSensor = sensorNum;
lastBallStrength = strength; lastBallStrength = strength;
@ -367,7 +387,9 @@ void loop()
robotActive = false; robotActive = false;
motors.stop(); motors.stop();
Serial.println("Robot stopped! Send 'start' to resume"); Serial.println("Robot stopped! Send 'start' to resume");
} else if (command.equalsIgnoreCase("test")) { }
else if (command.equalsIgnoreCase("test"))
{
motors.moveForward(100); motors.moveForward(100);
delay(1000); delay(1000);
motors.moveBackward(100); motors.moveBackward(100);
@ -432,3 +454,9 @@ void loop()
// Small delay to stabilize readings // Small delay to stabilize readings
delay(50); delay(50);
} }
// Very bad way to stop but the only way to stop the robot
void stop()
{
while (1);
}