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

Refactor OmniWheelMotors to improve motor control logic, adjust speed constraints, and enhance movement methods for better maneuverability

Browse source
This commit is contained in:
Andrea Moro 2025-03-14 08:50:26 +01:00
parent 76f1105a8f
commit 790c7cdaa0
3 changed files with 148 additions and 170 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

88
lib/motor/OmniWheelMotors.cpp
View file

@ -7,8 +7,8 @@ OmniWheelMotors::OmniWheelMotors(int m1_r_pwm, int m1_l_pwm,
motor_2 = {m2_r_pwm, m2_l_pwm}; motor_2 = {m2_r_pwm, m2_l_pwm};
motor_3 = {m3_r_pwm, m3_l_pwm}; motor_3 = {m3_r_pwm, m3_l_pwm};
MAX_SPEED = 70; // Max PWM value MAX_SPEED = 255; // Max PWM value
MIN_SPEED = 255; // Default minimum speed MIN_SPEED = 70; // Default minimum speed
FULL_ROTATION_TIME = 2000; // Default time for 360 degrees in milliseconds FULL_ROTATION_TIME = 2000; // Default time for 360 degrees in milliseconds
} }
@ -25,14 +25,30 @@ void OmniWheelMotors::begin() {
stop(); stop();
} }
void OmniWheelMotors::setMotorSpeed(Motor motor, int rightSpeed, int leftSpeed) { void OmniWheelMotors::setMotorSpeed(Motor motor, int speed, int direction) {
// Constrain speed values to valid PWM range // direction: 0 = stop, 1 = forward only, 2 = left, 3 = right
rightSpeed = constrain(rightSpeed, 0, MAX_SPEED);
leftSpeed = constrain(leftSpeed, 0, MAX_SPEED);
// Set PWM values for motor pins // Constrain speed value to valid PWM range
analogWrite(motor.R_PWM, rightSpeed); speed = constrain(speed, 0, MAX_SPEED);
analogWrite(motor.L_PWM, leftSpeed);
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() { void OmniWheelMotors::stop() {
@ -47,19 +63,19 @@ void OmniWheelMotors::moveForward(int speed) {
speed = max(speed, MIN_SPEED); speed = max(speed, MIN_SPEED);
// Set motor directions for forward movement // Set motor directions for forward movement
setMotorSpeed(motor_1, 0, speed); // Motor 1 forward setMotorSpeed(motor_1, speed, 1); // Motor 1 forward
setMotorSpeed(motor_2, 0, speed); // Motor 2 forward setMotorSpeed(motor_2, speed, 1); // Motor 2 forward
setMotorSpeed(motor_3, 0, 0); // Motor 3 neutral setMotorSpeed(motor_3, 0, 0); // Motor 3 neutral (no steering)
} }
void OmniWheelMotors::moveBackward(int speed) { void OmniWheelMotors::moveBackward(int speed) {
// Ensure speed is at least minimum speed for stable movement // To move backward, we'll rotate 180 degrees and then move forward
speed = max(speed, MIN_SPEED);
// Set motor directions for backward movement // First, rotate 180 degrees
setMotorSpeed(motor_1, speed, 0); // Motor 1 backward rotateToAngle(180);
setMotorSpeed(motor_2, speed, 0); // Motor 2 backward
setMotorSpeed(motor_3, 0, 0); // Motor 3 neutral // Then move forward (which is now effectively backward relative to original position)
moveForward(speed);
} }
void OmniWheelMotors::moveLeft(int speed) { void OmniWheelMotors::moveLeft(int speed) {
@ -67,9 +83,9 @@ void OmniWheelMotors::moveLeft(int speed) {
speed = max(speed, MIN_SPEED); speed = max(speed, MIN_SPEED);
// Set motor directions for left movement // Set motor directions for left movement
setMotorSpeed(motor_1, speed, 0); // Motor 1 backward setMotorSpeed(motor_1, speed, 1); // Motor 1 forward
setMotorSpeed(motor_2, 0, speed); // Motor 2 forward setMotorSpeed(motor_2, speed, 1); // Motor 2 forward
setMotorSpeed(motor_3, 0, speed); // Motor 3 forward setMotorSpeed(motor_3, speed, 2); // Motor 3 steer left
} }
void OmniWheelMotors::moveRight(int speed) { void OmniWheelMotors::moveRight(int speed) {
@ -77,16 +93,14 @@ void OmniWheelMotors::moveRight(int speed) {
speed = max(speed, MIN_SPEED); speed = max(speed, MIN_SPEED);
// Set motor directions for right movement // Set motor directions for right movement
setMotorSpeed(motor_1, 0, speed); // Motor 1 forward setMotorSpeed(motor_1, speed, 1); // Motor 1 forward
setMotorSpeed(motor_2, speed, 0); // Motor 2 backward setMotorSpeed(motor_2, speed, 1); // Motor 2 forward
setMotorSpeed(motor_3, speed, 0); // Motor 3 backward setMotorSpeed(motor_3, speed, 3); // Motor 3 steer right
} }
void OmniWheelMotors::rotate360() { void OmniWheelMotors::rotate360() {
// Set motor directions for full rotation // Perform full 360 degree rotation
setMotorSpeed(motor_1, 0, MIN_SPEED); // Motor 1 forward rotateClockwise(MIN_SPEED);
setMotorSpeed(motor_2, MIN_SPEED, 0); // Motor 2 forward
setMotorSpeed(motor_3, 0, MIN_SPEED); // Motor 3 neutral
// Wait for approximately one full rotation // Wait for approximately one full rotation
delay(FULL_ROTATION_TIME); delay(FULL_ROTATION_TIME);
@ -100,9 +114,9 @@ void OmniWheelMotors::rotateClockwise(int speed) {
speed = max(speed, MIN_SPEED); speed = max(speed, MIN_SPEED);
// Set motor directions for clockwise rotation // Set motor directions for clockwise rotation
setMotorSpeed(motor_1, 0, speed); // Motor 1 forward setMotorSpeed(motor_1, speed, 1); // Motor 1 forward
setMotorSpeed(motor_2, speed, 0); // Motor 2 backward setMotorSpeed(motor_2, 0, 0); // Motor 2 stopped
setMotorSpeed(motor_3, speed, 0); // Motor 3 neutral setMotorSpeed(motor_3, speed, 3); // Motor 3 steer right
} }
void OmniWheelMotors::rotateCounterClockwise(int speed) { void OmniWheelMotors::rotateCounterClockwise(int speed) {
@ -110,9 +124,9 @@ void OmniWheelMotors::rotateCounterClockwise(int speed) {
speed = max(speed, MIN_SPEED); speed = max(speed, MIN_SPEED);
// Set motor directions for counter-clockwise rotation // Set motor directions for counter-clockwise rotation
setMotorSpeed(motor_1, speed, 0); // Motor 1 backward setMotorSpeed(motor_1, 0, 0); // Motor 1 stopped
setMotorSpeed(motor_2, 0, speed); // Motor 2 forward setMotorSpeed(motor_2, speed, 1); // Motor 2 forward
setMotorSpeed(motor_3, 0, speed); // Motor 3 neutral setMotorSpeed(motor_3, speed, 2); // Motor 3 steer left
} }
void OmniWheelMotors::rotateToAngle(int targetAngle) { void OmniWheelMotors::rotateToAngle(int targetAngle) {
@ -123,10 +137,8 @@ void OmniWheelMotors::rotateToAngle(int targetAngle) {
// Calculate rotation time based on angle // Calculate rotation time based on angle
int rotationTime = (targetAngle * FULL_ROTATION_TIME) / 360; int rotationTime = (targetAngle * FULL_ROTATION_TIME) / 360;
// Start rotation // Start rotation (use counter-clockwise for positive angles)
setMotorSpeed(motor_1, MIN_SPEED, 0); // Motor 1 backward rotateCounterClockwise(MIN_SPEED);
setMotorSpeed(motor_2, 0, MIN_SPEED); // Motor 2 forward
setMotorSpeed(motor_3, 0, 0); // Motor 3 neutral
// Wait for calculated time // Wait for calculated time
delay(rotationTime); delay(rotationTime);

3
lib/motor/OmniWheelMotors.hpp
View file

@ -6,6 +6,8 @@
class OmniWheelMotors { class OmniWheelMotors {
public: public:
int FULL_ROTATION_TIME; // Time for 360 degrees in milliseconds
struct Motor { struct Motor {
int R_PWM; int R_PWM;
int L_PWM; int L_PWM;
@ -48,7 +50,6 @@ private:
int MAX_SPEED; // Maximum speed value int MAX_SPEED; // Maximum speed value
int MIN_SPEED; // Minimum speed value for stable movement int MIN_SPEED; // Minimum speed value for stable movement
int FULL_ROTATION_TIME; // Time for 360 degrees in milliseconds
}; };
#endif // OMNIWHEEL_MOTORS_HPP #endif // OMNIWHEEL_MOTORS_HPP

227
src/main.cpp
View file

@ -13,23 +13,20 @@
#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
#define MOTOR1_R_PWM 5 #define MOTOR1_R_PWM 11
#define MOTOR1_L_PWM 3 #define MOTOR2_R_PWM 10
#define MOTOR2_R_PWM 9 #define MOTOR3_R_PWM 6
#define MOTOR2_L_PWM 6 #define MOTOR3_L_PWM 9
#define MOTOR3_R_PWM 10
#define MOTOR3_L_PWM 11
// Sensor objects // Sensor objects
PixyI2C pixy; PixyI2C pixy;
Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC(); Ultrasonic uSensor1 = Ultrasonic(2);
Ultrasonic uSensor1 = Ultrasonic(7); Ultrasonic uSensor2 = Ultrasonic(4); // Commented out
Ultrasonic uSensor2 = Ultrasonic(10); // Commented out Ultrasonic uSensor3 = Ultrasonic(3); // 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, 24,
MOTOR2_R_PWM, MOTOR2_L_PWM, MOTOR2_R_PWM, 24,
MOTOR3_R_PWM, MOTOR3_L_PWM); MOTOR3_R_PWM, MOTOR3_L_PWM);
// Display object // Display object
@ -39,9 +36,16 @@ DisplayLib display;
int lastBallSensor = -1; int lastBallSensor = -1;
int lastBallStrength = 255; // Start at max distance int lastBallStrength = 255; // Start at max distance
unsigned long lastBallDetectionTime = 0; unsigned long lastBallDetectionTime = 0;
bool ballLost = true; bool ballLost = false;
bool robotActive = false; // Flag to indicate if robot should be operating 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() void setup()
{ {
// Initialize serial communication // Initialize serial communication
@ -51,22 +55,11 @@ void setup()
Serial.println("ElBestia Robot Starting..."); Serial.println("ElBestia Robot Starting...");
// Initialize display
display.initDisplay();
display.welcomeScreen();
// Initialize I2C // Initialize I2C
Wire.begin(); Wire.begin();
// Initialize IMU // Initialize IMU
Serial1.begin(115200); Serial1.begin(115200);
if (!rvc.begin(&Serial1))
{
Serial.println("Failed to initialize IMU!");
while (1)
delay(10);
}
Serial.println("IMU initialized");
// Initialize motors // Initialize motors
motors.begin(); motors.begin();
@ -79,14 +72,6 @@ void setup()
delay(500); delay(500);
analogWrite(MOTOR1_R_PWM, 0); analogWrite(MOTOR1_R_PWM, 0);
delay(200); 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); analogWrite(MOTOR2_R_PWM, MOTOR_SPEED);
delay(500); delay(500);
analogWrite(MOTOR2_R_PWM, 0); analogWrite(MOTOR2_R_PWM, 0);
@ -114,10 +99,6 @@ void setup()
{ {
Serial.println("IR Seeker not found"); Serial.println("IR Seeker not found");
Serial.println("Stopping robot..."); 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...");
@ -125,8 +106,6 @@ void setup()
// wait for 'start' command from serial // wait for 'start' command from serial
// wait for 'start' command from serial // wait for 'start' command from serial
Serial.println("Send 'start' to begin operation or 'stop' to halt"); 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) while (!robotActive)
{ {
if (Serial.available() > 0) if (Serial.available() > 0)
@ -146,15 +125,6 @@ void setup()
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))
{
float yaw = heading.yaw;
Serial.print("Current Yaw: ");
Serial.println(yaw);
}
// Calculate intensity for movement - closer means slower movement // Calculate intensity for movement - closer means slower movement
// Invert the strength so 0 (close) becomes high intensity and 255 (far) becomes low // 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);
@ -162,74 +132,88 @@ void moveBasedOnSensor(int sensorNum, int strength)
// Maps the 12 sensors to movement directions // Maps the 12 sensors to movement directions
// North is 12, and we go clockwise (1, 2, 3...) // 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(); display.sendBuffer();
switch (sensorNum) switch (sensorNum)
{ {
case 12: // North - move forward switch (sensorNum)
Serial.println("Moving forward (North)"); {
motors.moveForward(moveIntensity); case 12: // North - move forward
break; Serial.println("Moving forward (North)");
case 1: // North-northeast motors.moveForward(moveIntensity);
Serial.println("Moving forward-right (NNE)"); break;
motors.moveForward(moveIntensity); case 1: // North-northeast
delay(200); Serial.println("Moving forward-right (NNE)");
motors.moveRight(moveIntensity); motors.moveForward(moveIntensity);
break; motors.moveRight(moveIntensity);
case 2: // Northeast break;
Serial.println("Moving forward-right (NE)"); case 2: // Northeast
motors.moveForward(moveIntensity / 2); Serial.println("Moving forward-right (NE)");
motors.moveRight(moveIntensity); // Both front wheels + steer right
break; motors.moveForward(moveIntensity);
case 3: // East-northeast motors.moveRight(moveIntensity);
Serial.println("Moving right (ENE)"); break;
motors.moveRight(moveIntensity); case 3: // East-northeast
break; Serial.println("Moving right (ENE)");
case 4: // East motors.moveRight(moveIntensity);
Serial.println("Moving right (East)"); break;
motors.moveRight(moveIntensity); case 4: // East
break; Serial.println("Moving right (East)");
case 5: // East-southeast motors.moveRight(moveIntensity);
Serial.println("Moving backward-right (ESE)"); break;
motors.moveRight(moveIntensity); case 5: // East-southeast
delay(200); Serial.println("Moving backward-right (ESE)");
motors.moveBackward(moveIntensity / 2); // Rotate ~135 degrees clockwise then move forward
break; motors.rotateClockwise(moveIntensity);
case 6: // Southeast delay(motors.FULL_ROTATION_TIME * 3/8); // 135 degrees
Serial.println("Moving backward-right (SE)"); motors.stop();
motors.moveBackward(moveIntensity / 2); motors.moveForward(moveIntensity);
motors.moveRight(moveIntensity); break;
break; case 6: // Southeast
case 7: // South-southeast Serial.println("Moving backward-right (SE)");
Serial.println("Moving backward (SSE)"); // Rotate ~135 degrees clockwise then move forward
motors.moveBackward(moveIntensity); motors.rotateClockwise(moveIntensity);
break; delay(motors.FULL_ROTATION_TIME * 3/8); // 135 degrees
case 8: // South motors.stop();
Serial.println("Moving backward (South)"); motors.moveForward(moveIntensity);
motors.moveBackward(moveIntensity); break;
break; case 7: // South-southeast
case 9: // South-southwest Serial.println("Moving backward (SSE)");
Serial.println("Moving backward-left (SSW)"); // Rotate 180 degrees then move forward
motors.moveBackward(moveIntensity); motors.rotateToAngle(180);
delay(200); motors.moveForward(moveIntensity);
motors.moveLeft(moveIntensity / 2); break;
break; case 8: // South
case 10: // Southwest Serial.println("Moving backward (South)");
Serial.println("Moving backward-left (SW)"); // Rotate 180 degrees then move forward
motors.moveBackward(moveIntensity / 2); motors.rotateToAngle(180);
motors.moveLeft(moveIntensity); motors.moveForward(moveIntensity);
break; break;
case 11: // West-southwest case 9: // South-southwest
Serial.println("Moving left (WSW)"); Serial.println("Moving backward-left (SSW)");
motors.moveLeft(moveIntensity); // Rotate ~225 degrees (180+45) then move forward
break; motors.rotateCounterClockwise(moveIntensity);
default: delay(motors.FULL_ROTATION_TIME * 5/8); // 225 degrees
motors.stop(); motors.stop();
Serial.println("Unknown sensor direction, stopping"); motors.moveForward(moveIntensity);
break; 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 // Adjust movement duration based on signal strength
// Closer (lower value) = shorter movements // Closer (lower value) = shorter movements
@ -297,9 +281,6 @@ void searchForBall()
static int searchPhase = 0; // 0-3: increases search area over time static int searchPhase = 0; // 0-3: increases search area over time
Serial.println("Searching for ball..."); 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 we previously detected the ball, first try turning toward that direction
if (lastBallSensor != -1 && millis() - lastBallDetectionTime < 5000) // Only use last position if recent if (lastBallSensor != -1 && millis() - lastBallDetectionTime < 5000) // Only use last position if recent
@ -417,17 +398,6 @@ void loop()
// Print debug info every second // Print debug info every second
if (millis() - lastDebugPrint > 1000) 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(); lastDebugPrint = millis();
} }
long RangeInCentimeters; long RangeInCentimeters;
@ -455,8 +425,3 @@ void loop()
delay(50); delay(50);
} }
// Very bad way to stop but the only way to stop the robot
void stop()
{
while (1);
}