Refactor OmniWheelMotors to improve motor control logic, adjust speed constraints, and enhance movement methods for better maneuverability
This commit is contained in:
parent
76f1105a8f
commit
790c7cdaa0
3 changed files with 148 additions and 170 deletions
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
227
src/main.cpp
227
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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue