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