diff --git a/lib/motor/OmniWheelMotors.cpp b/lib/motor/OmniWheelMotors.cpp index 80d391d..6913f1f 100644 --- a/lib/motor/OmniWheelMotors.cpp +++ b/lib/motor/OmniWheelMotors.cpp @@ -7,8 +7,8 @@ OmniWheelMotors::OmniWheelMotors(int m1_r_pwm, int m1_l_pwm, motor_2 = {m2_r_pwm, m2_l_pwm}; motor_3 = {m3_r_pwm, m3_l_pwm}; - MAX_SPEED = 70; // Max PWM value - MIN_SPEED = 255; // Default minimum speed + MAX_SPEED = 255; // Max PWM value + MIN_SPEED = 70; // Default minimum speed FULL_ROTATION_TIME = 2000; // Default time for 360 degrees in milliseconds } @@ -25,14 +25,30 @@ void OmniWheelMotors::begin() { stop(); } -void OmniWheelMotors::setMotorSpeed(Motor motor, int rightSpeed, int leftSpeed) { - // Constrain speed values to valid PWM range - rightSpeed = constrain(rightSpeed, 0, MAX_SPEED); - leftSpeed = constrain(leftSpeed, 0, MAX_SPEED); +void OmniWheelMotors::setMotorSpeed(Motor motor, int speed, int direction) { + // direction: 0 = stop, 1 = forward only, 2 = left, 3 = right - // Set PWM values for motor pins - analogWrite(motor.R_PWM, rightSpeed); - analogWrite(motor.L_PWM, leftSpeed); + // Constrain speed value to valid PWM range + speed = constrain(speed, 0, MAX_SPEED); + + switch(direction) { + case 0: // Stop + analogWrite(motor.R_PWM, 0); + analogWrite(motor.L_PWM, 0); + break; + case 1: // Forward only (for motors 1 and 2) + analogWrite(motor.R_PWM, speed); + analogWrite(motor.L_PWM, 0); + break; + case 2: // Left (for motor 3) + analogWrite(motor.R_PWM, speed); + analogWrite(motor.L_PWM, 0); + break; + case 3: // Right (for motor 3) + analogWrite(motor.R_PWM, 0); + analogWrite(motor.L_PWM, speed); + break; + } } void OmniWheelMotors::stop() { @@ -47,19 +63,19 @@ void OmniWheelMotors::moveForward(int speed) { speed = max(speed, MIN_SPEED); // Set motor directions for forward movement - setMotorSpeed(motor_1, 0, speed); // Motor 1 forward - setMotorSpeed(motor_2, 0, speed); // Motor 2 forward - setMotorSpeed(motor_3, 0, 0); // Motor 3 neutral + setMotorSpeed(motor_1, speed, 1); // Motor 1 forward + setMotorSpeed(motor_2, speed, 1); // Motor 2 forward + setMotorSpeed(motor_3, 0, 0); // Motor 3 neutral (no steering) } void OmniWheelMotors::moveBackward(int speed) { - // Ensure speed is at least minimum speed for stable movement - speed = max(speed, MIN_SPEED); + // To move backward, we'll rotate 180 degrees and then move forward - // Set motor directions for backward movement - setMotorSpeed(motor_1, speed, 0); // Motor 1 backward - setMotorSpeed(motor_2, speed, 0); // Motor 2 backward - setMotorSpeed(motor_3, 0, 0); // Motor 3 neutral + // First, rotate 180 degrees + rotateToAngle(180); + + // Then move forward (which is now effectively backward relative to original position) + moveForward(speed); } void OmniWheelMotors::moveLeft(int speed) { @@ -67,9 +83,9 @@ void OmniWheelMotors::moveLeft(int speed) { speed = max(speed, MIN_SPEED); // Set motor directions for left movement - setMotorSpeed(motor_1, speed, 0); // Motor 1 backward - setMotorSpeed(motor_2, 0, speed); // Motor 2 forward - setMotorSpeed(motor_3, 0, speed); // Motor 3 forward + setMotorSpeed(motor_1, speed, 1); // Motor 1 forward + setMotorSpeed(motor_2, speed, 1); // Motor 2 forward + setMotorSpeed(motor_3, speed, 2); // Motor 3 steer left } void OmniWheelMotors::moveRight(int speed) { @@ -77,16 +93,14 @@ void OmniWheelMotors::moveRight(int speed) { speed = max(speed, MIN_SPEED); // Set motor directions for right movement - setMotorSpeed(motor_1, 0, speed); // Motor 1 forward - setMotorSpeed(motor_2, speed, 0); // Motor 2 backward - setMotorSpeed(motor_3, speed, 0); // Motor 3 backward + setMotorSpeed(motor_1, speed, 1); // Motor 1 forward + setMotorSpeed(motor_2, speed, 1); // Motor 2 forward + setMotorSpeed(motor_3, speed, 3); // Motor 3 steer right } void OmniWheelMotors::rotate360() { - // Set motor directions for full rotation - setMotorSpeed(motor_1, 0, MIN_SPEED); // Motor 1 forward - setMotorSpeed(motor_2, MIN_SPEED, 0); // Motor 2 forward - setMotorSpeed(motor_3, 0, MIN_SPEED); // Motor 3 neutral + // Perform full 360 degree rotation + rotateClockwise(MIN_SPEED); // Wait for approximately one full rotation delay(FULL_ROTATION_TIME); @@ -100,9 +114,9 @@ void OmniWheelMotors::rotateClockwise(int speed) { speed = max(speed, MIN_SPEED); // Set motor directions for clockwise rotation - setMotorSpeed(motor_1, 0, speed); // Motor 1 forward - setMotorSpeed(motor_2, speed, 0); // Motor 2 backward - setMotorSpeed(motor_3, speed, 0); // Motor 3 neutral + setMotorSpeed(motor_1, speed, 1); // Motor 1 forward + setMotorSpeed(motor_2, 0, 0); // Motor 2 stopped + setMotorSpeed(motor_3, speed, 3); // Motor 3 steer right } void OmniWheelMotors::rotateCounterClockwise(int speed) { @@ -110,9 +124,9 @@ void OmniWheelMotors::rotateCounterClockwise(int speed) { speed = max(speed, MIN_SPEED); // Set motor directions for counter-clockwise rotation - setMotorSpeed(motor_1, speed, 0); // Motor 1 backward - setMotorSpeed(motor_2, 0, speed); // Motor 2 forward - setMotorSpeed(motor_3, 0, speed); // Motor 3 neutral + setMotorSpeed(motor_1, 0, 0); // Motor 1 stopped + setMotorSpeed(motor_2, speed, 1); // Motor 2 forward + setMotorSpeed(motor_3, speed, 2); // Motor 3 steer left } void OmniWheelMotors::rotateToAngle(int targetAngle) { @@ -123,10 +137,8 @@ void OmniWheelMotors::rotateToAngle(int targetAngle) { // Calculate rotation time based on angle int rotationTime = (targetAngle * FULL_ROTATION_TIME) / 360; - // Start rotation - setMotorSpeed(motor_1, MIN_SPEED, 0); // Motor 1 backward - setMotorSpeed(motor_2, 0, MIN_SPEED); // Motor 2 forward - setMotorSpeed(motor_3, 0, 0); // Motor 3 neutral + // Start rotation (use counter-clockwise for positive angles) + rotateCounterClockwise(MIN_SPEED); // Wait for calculated time delay(rotationTime); diff --git a/lib/motor/OmniWheelMotors.hpp b/lib/motor/OmniWheelMotors.hpp index 3518926..019a425 100644 --- a/lib/motor/OmniWheelMotors.hpp +++ b/lib/motor/OmniWheelMotors.hpp @@ -6,6 +6,8 @@ class OmniWheelMotors { public: + int FULL_ROTATION_TIME; // Time for 360 degrees in milliseconds + struct Motor { int R_PWM; int L_PWM; @@ -48,7 +50,6 @@ private: int MAX_SPEED; // Maximum speed value int MIN_SPEED; // Minimum speed value for stable movement - int FULL_ROTATION_TIME; // Time for 360 degrees in milliseconds }; #endif // OMNIWHEEL_MOTORS_HPP \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 74f7f90..bf1ddbc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,23 +13,20 @@ #define SIGNAL_TIMEOUT 5000 // Time in ms to consider ball lost if no detection // Pin definitions for the motors -#define MOTOR1_R_PWM 5 -#define MOTOR1_L_PWM 3 -#define MOTOR2_R_PWM 9 -#define MOTOR2_L_PWM 6 -#define MOTOR3_R_PWM 10 -#define MOTOR3_L_PWM 11 +#define MOTOR1_R_PWM 11 +#define MOTOR2_R_PWM 10 +#define MOTOR3_R_PWM 6 +#define MOTOR3_L_PWM 9 // Sensor objects 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 uSensor1 = Ultrasonic(2); +Ultrasonic uSensor2 = Ultrasonic(4); // Commented out +Ultrasonic uSensor3 = Ultrasonic(3); // Commented out // Motor control object -OmniWheelMotors motors(MOTOR1_R_PWM, MOTOR1_L_PWM, - MOTOR2_R_PWM, MOTOR2_L_PWM, +OmniWheelMotors motors(MOTOR1_R_PWM, 24, + MOTOR2_R_PWM, 24, MOTOR3_R_PWM, MOTOR3_L_PWM); // Display object @@ -39,9 +36,16 @@ DisplayLib display; int lastBallSensor = -1; int lastBallStrength = 255; // Start at max distance unsigned long lastBallDetectionTime = 0; -bool ballLost = true; +bool ballLost = false; bool robotActive = false; // Flag to indicate if robot should be operating +// Very bad way to stop but the only way to stop the robot +void stop() +{ + while (1); +} + + void setup() { // Initialize serial communication @@ -51,22 +55,11 @@ void setup() Serial.println("ElBestia Robot Starting..."); - // Initialize display - display.initDisplay(); - display.welcomeScreen(); - // Initialize I2C Wire.begin(); // Initialize IMU Serial1.begin(115200); - if (!rvc.begin(&Serial1)) - { - Serial.println("Failed to initialize IMU!"); - while (1) - delay(10); - } - Serial.println("IMU initialized"); // Initialize motors motors.begin(); @@ -79,14 +72,6 @@ void setup() delay(500); analogWrite(MOTOR1_R_PWM, 0); delay(200); - analogWrite(MOTOR1_L_PWM, MOTOR_SPEED); - delay(500); - analogWrite(MOTOR1_L_PWM, 0); - delay(200); - analogWrite(MOTOR2_L_PWM, MOTOR_SPEED); - delay(500); - analogWrite(MOTOR2_L_PWM, 0); - delay(200); analogWrite(MOTOR2_R_PWM, MOTOR_SPEED); delay(500); analogWrite(MOTOR2_R_PWM, 0); @@ -114,10 +99,6 @@ void setup() { 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..."); @@ -125,8 +106,6 @@ void setup() // wait for 'start' command from serial // wait for 'start' command from serial Serial.println("Send 'start' to begin operation or 'stop' to halt"); - display.clearDisplay(); - display.displayMessage("Send 'start' to begin operation or 'stop' to halt"); while (!robotActive) { if (Serial.available() > 0) @@ -146,15 +125,6 @@ void setup() void moveBasedOnSensor(int sensorNum, int strength) { - // Get current heading from IMU for reference - BNO08x_RVC_Data 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); @@ -162,74 +132,88 @@ void moveBasedOnSensor(int sensorNum, int strength) // Maps the 12 sensors to movement directions // North is 12, and we go clockwise (1, 2, 3...) - display.clearDisplay(); - display.printMessage("Sensor: " + String(sensorNum), 0, 10); - display.printMessage("Strength: " + String(strength), 0, 30); display.sendBuffer(); 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); + motors.moveRight(moveIntensity); + break; + case 2: // Northeast + Serial.println("Moving forward-right (NE)"); + // Both front wheels + steer right + motors.moveForward(moveIntensity); + 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)"); + // Rotate ~135 degrees clockwise then move forward + motors.rotateClockwise(moveIntensity); + delay(motors.FULL_ROTATION_TIME * 3/8); // 135 degrees + motors.stop(); + motors.moveForward(moveIntensity); + break; + case 6: // Southeast + Serial.println("Moving backward-right (SE)"); + // Rotate ~135 degrees clockwise then move forward + motors.rotateClockwise(moveIntensity); + delay(motors.FULL_ROTATION_TIME * 3/8); // 135 degrees + motors.stop(); + motors.moveForward(moveIntensity); + break; + case 7: // South-southeast + Serial.println("Moving backward (SSE)"); + // Rotate 180 degrees then move forward + motors.rotateToAngle(180); + motors.moveForward(moveIntensity); + break; + case 8: // South + Serial.println("Moving backward (South)"); + // Rotate 180 degrees then move forward + motors.rotateToAngle(180); + motors.moveForward(moveIntensity); + break; + case 9: // South-southwest + Serial.println("Moving backward-left (SSW)"); + // Rotate ~225 degrees (180+45) then move forward + motors.rotateCounterClockwise(moveIntensity); + delay(motors.FULL_ROTATION_TIME * 5/8); // 225 degrees + motors.stop(); + motors.moveForward(moveIntensity); + break; + case 10: // Southwest + Serial.println("Moving backward-left (SW)"); + // Rotate ~225 degrees (180+45) then move forward + motors.rotateCounterClockwise(moveIntensity); + delay(motors.FULL_ROTATION_TIME * 5/8); // 225 degrees + motors.stop(); + motors.moveForward(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 @@ -297,9 +281,6 @@ void searchForBall() 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); - display.sendBuffer(); // If we previously detected the ball, first try turning toward that direction if (lastBallSensor != -1 && millis() - lastBallDetectionTime < 5000) // Only use last position if recent @@ -417,17 +398,6 @@ void loop() // 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(); } long RangeInCentimeters; @@ -455,8 +425,3 @@ void loop() delay(50); } -// Very bad way to stop but the only way to stop the robot -void stop() -{ - while (1); -}