Refactor main.cpp to integrate OmniWheelMotors for improved motor control and remove unused sensor code
This commit is contained in:
parent
ce889a702a
commit
8a5402570c
1 changed files with 226 additions and 96 deletions
322
src/main.cpp
322
src/main.cpp
|
|
@ -1,116 +1,246 @@
|
|||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
#include "Adafruit_BNO08x_RVC.h"
|
||||
#include <PixyI2C.h>
|
||||
#include "Ultrasonic.h"
|
||||
// #include <PixyI2C.h> // Commented out
|
||||
// #include "Ultrasonic.h" // Commented out
|
||||
#include "OmniWheelMotors.hpp"
|
||||
|
||||
PixyI2C pixy;
|
||||
// Constants
|
||||
#define MOTOR_SPEED 60
|
||||
#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
|
||||
|
||||
// Pin definitions for the motors
|
||||
#define MOTOR1_R_PWM 3
|
||||
#define MOTOR1_L_PWM 5
|
||||
#define MOTOR2_R_PWM 6
|
||||
#define MOTOR2_L_PWM 9
|
||||
#define MOTOR3_R_PWM 10
|
||||
#define MOTOR3_L_PWM 11
|
||||
|
||||
// Sensor objects
|
||||
// PixyI2C pixy; // Commented out
|
||||
Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();
|
||||
Ultrasonic uSensor1 = Ultrasonic(7);
|
||||
Ultrasonic uSensor2 = Ultrasonic(10);
|
||||
Ultrasonic uSensor3 = Ultrasonic(9);
|
||||
// Ultrasonic uSensor1 = Ultrasonic(7); // Commented out
|
||||
// Ultrasonic uSensor2 = Ultrasonic(10); // Commented out
|
||||
// Ultrasonic uSensor3 = Ultrasonic(9); // Commented out
|
||||
|
||||
void setup()
|
||||
{
|
||||
Wire.begin();
|
||||
pixy.init();
|
||||
// Motor control object
|
||||
OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM,
|
||||
MOTOR2_R_PWM, MOTOR2_L_PWM,
|
||||
MOTOR3_R_PWM, MOTOR3_L_PWM);
|
||||
|
||||
// Wait for serial monitor to open
|
||||
// Global variables
|
||||
int lastBallSensor = -1;
|
||||
int lastBallStrength = 255; // Start at max distance
|
||||
unsigned long lastBallDetectionTime = 0;
|
||||
bool ballLost = true;
|
||||
|
||||
void setup() {
|
||||
// Initialize serial communication
|
||||
Serial.begin(115200);
|
||||
while (!Serial)
|
||||
while (!Serial && millis() < 3000) // Wait up to 3 seconds for serial
|
||||
delay(10);
|
||||
|
||||
Serial1.begin(115200); // This is the baud rate specified by the datasheet
|
||||
while (!Serial1)
|
||||
delay(10);
|
||||
|
||||
if (!rvc.begin(&Serial1))
|
||||
{ // connect to the sensor over hardware serial
|
||||
while (1)
|
||||
delay(10);
|
||||
Serial.println("ElBestia Robot Starting...");
|
||||
|
||||
// Initialize I2C
|
||||
Wire.begin();
|
||||
|
||||
// Initialize IMU
|
||||
Serial1.begin(115200);
|
||||
if (!rvc.begin(&Serial1)) {
|
||||
Serial.println("Failed to initialize IMU!");
|
||||
while (1) delay(10);
|
||||
}
|
||||
// Print labels for the values
|
||||
Serial.print(F("Yaw"));
|
||||
Serial.print(F("\tPitch"));
|
||||
Serial.print(F("\tRoll"));
|
||||
Serial.print(F("\tX"));
|
||||
Serial.print(F("\tY"));
|
||||
Serial.println(F("\tZ"));
|
||||
Serial.println("IMU initialized");
|
||||
|
||||
// Initialize motors
|
||||
motors.begin();
|
||||
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.");
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
// Get IMU data
|
||||
Wire.requestFrom(0x10 / 2, 2);
|
||||
void moveBasedOnSensor(int sensorNum, int strength) {
|
||||
// Get current heading from IMU for reference
|
||||
BNO08x_RVC_Data heading;
|
||||
static int i = 0;
|
||||
uint16_t blocks;
|
||||
char buf[32];
|
||||
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);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// Adjust movement duration based on signal strength
|
||||
// Closer (lower value) = shorter movements
|
||||
int moveDuration = map(strength, 0, 255, 200, 800);
|
||||
delay(moveDuration);
|
||||
motors.stop();
|
||||
}
|
||||
|
||||
while (rvc.read(&heading))
|
||||
{
|
||||
int c = Wire.read(); // direction is the first byte
|
||||
int strength = Wire.read();
|
||||
// Print IMU data
|
||||
Serial.print("IMU - Direction: ");
|
||||
Serial.print(c);
|
||||
Serial.print(" Strength: ");
|
||||
void trackBall() {
|
||||
// Get IR ball direction from seeker (on I2C address 0x08)
|
||||
Wire.requestFrom(0x08, 2);
|
||||
|
||||
if (Wire.available() >= 2) {
|
||||
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);
|
||||
Serial.print("Heading: ");
|
||||
Serial.print(heading.yaw);
|
||||
Serial.print(",");
|
||||
Serial.print(heading.pitch);
|
||||
Serial.print(",");
|
||||
Serial.print(heading.roll);
|
||||
Serial.println();
|
||||
Serial.print("Acceleration: ");
|
||||
Serial.print(heading.x_accel);
|
||||
Serial.print(",");
|
||||
Serial.print(heading.y_accel);
|
||||
Serial.print(",");
|
||||
Serial.print(heading.z_accel);
|
||||
Serial.println();
|
||||
|
||||
// Get Ultrasonic sensor data
|
||||
long uS1 = uSensor1.MeasureInCentimeters();
|
||||
Serial.print("Distance uS1: ");
|
||||
Serial.print(uS1);
|
||||
Serial.println(" cm");
|
||||
|
||||
long uS2 = uSensor2.MeasureInCentimeters();
|
||||
Serial.print("Distance uS2: ");
|
||||
Serial.print(uS2);
|
||||
Serial.println(" cm");
|
||||
|
||||
long uS3 = uSensor3.MeasureInCentimeters();
|
||||
Serial.print("Distance uS3: ");
|
||||
Serial.print(uS3);
|
||||
Serial.println(" cm");
|
||||
|
||||
// Get Pixy blocks
|
||||
blocks = pixy.getBlocks();
|
||||
|
||||
if (blocks)
|
||||
{
|
||||
Serial.println("Detected blocks");
|
||||
i++;
|
||||
// Print every 50 frames to avoid overwhelming serial
|
||||
if (i % 50 == 0)
|
||||
{
|
||||
|
||||
// Print Pixy blocks
|
||||
sprintf(buf, "Detected %d blocks:\n", blocks);
|
||||
Serial.print(buf);
|
||||
for (int j = 0; j < blocks; j++)
|
||||
{
|
||||
sprintf(buf, " block %d: ", j);
|
||||
Serial.print(buf);
|
||||
pixy.blocks[j].print();
|
||||
}
|
||||
Serial.println();
|
||||
|
||||
// Only process if we have a valid sensor number
|
||||
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 {
|
||||
Serial.println("Invalid sensor number from IR seeker");
|
||||
|
||||
// If signal is invalid, check if we lost the ball
|
||||
if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT) {
|
||||
ballLost = true;
|
||||
}
|
||||
|
||||
delay(250);
|
||||
}
|
||||
} else {
|
||||
Serial.println("No data available from IR seeker");
|
||||
|
||||
// Check if we've lost the ball for too long
|
||||
if (millis() - lastBallDetectionTime > SIGNAL_TIMEOUT) {
|
||||
ballLost = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void searchForBall() {
|
||||
Serial.println("Searching for ball...");
|
||||
|
||||
// If we previously detected the ball, turn toward that direction first
|
||||
if (lastBallSensor != -1) {
|
||||
// Use the last known direction to guide search
|
||||
moveBasedOnSensor(lastBallSensor, 200); // Use medium strength value for searching
|
||||
} 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() {
|
||||
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);
|
||||
}
|
||||
|
||||
lastDebugPrint = millis();
|
||||
}
|
||||
|
||||
// Ball tracking or searching
|
||||
if (!ballLost) {
|
||||
trackBall();
|
||||
} else {
|
||||
searchForBall();
|
||||
}
|
||||
|
||||
// Small delay to stabilize readings
|
||||
delay(50);
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue