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_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);

3
lib/motor/OmniWheelMotors.hpp
View file

@ -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

131
src/main.cpp
View file

@ -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,10 +132,9 @@ 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)
{
switch (sensorNum)
{
case 12: // North - move forward
@ -175,12 +144,12 @@ void moveBasedOnSensor(int sensorNum, int strength)
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);
// Both front wheels + steer right
motors.moveForward(moveIntensity);
motors.moveRight(moveIntensity);
break;
case 3: // East-northeast
@ -193,33 +162,47 @@ void moveBasedOnSensor(int sensorNum, int strength)
break;
case 5: // East-southeast
Serial.println("Moving backward-right (ESE)");
motors.moveRight(moveIntensity);
delay(200);
motors.moveBackward(moveIntensity / 2);
// 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)");
motors.moveBackward(moveIntensity / 2);
motors.moveRight(moveIntensity);
// 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)");
motors.moveBackward(moveIntensity);
// Rotate 180 degrees then move forward
motors.rotateToAngle(180);
motors.moveForward(moveIntensity);
break;
case 8: // South
Serial.println("Moving backward (South)");
motors.moveBackward(moveIntensity);
// Rotate 180 degrees then move forward
motors.rotateToAngle(180);
motors.moveForward(moveIntensity);
break;
case 9: // South-southwest
Serial.println("Moving backward-left (SSW)");
motors.moveBackward(moveIntensity);
delay(200);
motors.moveLeft(moveIntensity / 2);
// 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)");
motors.moveBackward(moveIntensity / 2);
motors.moveLeft(moveIntensity);
// 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)");
@ -230,6 +213,7 @@ void moveBasedOnSensor(int sensorNum, int strength)
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);
}