elbestia-robot/lib/motor/OmniWheelMotors.cpp

#include "OmniWheelMotors.hpp"

OmniWheelMotors::OmniWheelMotors(int m1_r_pwm, int m1_l_pwm, 
                               int m2_r_pwm, int m2_l_pwm, 
                               int m3_r_pwm, int m3_l_pwm) {
    motor_1 = {m1_r_pwm, 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
    FULL_ROTATION_TIME = 2000;  // Default time for 360 degrees in milliseconds
}

void OmniWheelMotors::begin() {
    // Set all motor pins as outputs
    pinMode(motor_1.R_PWM, OUTPUT);
    pinMode(motor_1.L_PWM, OUTPUT);
    pinMode(motor_2.R_PWM, OUTPUT);
    pinMode(motor_2.L_PWM, OUTPUT);
    pinMode(motor_3.R_PWM, OUTPUT);
    pinMode(motor_3.L_PWM, OUTPUT);
    
    // Initialize all motors to stopped
    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);
    
    // Set PWM values for motor pins
    analogWrite(motor.R_PWM, rightSpeed);
    analogWrite(motor.L_PWM, leftSpeed);
}

void OmniWheelMotors::stop() {
    // Stop all motors
    setMotorSpeed(motor_1, 0, 0);
    setMotorSpeed(motor_2, 0, 0);
    setMotorSpeed(motor_3, 0, 0);
}

void OmniWheelMotors::moveForward(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    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
}

void OmniWheelMotors::moveBackward(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    speed = max(speed, MIN_SPEED);
    
    // 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
}

void OmniWheelMotors::moveLeft(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    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
}

void OmniWheelMotors::moveRight(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    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
}

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
    
    // Wait for approximately one full rotation
    delay(FULL_ROTATION_TIME);
    
    // Stop all motors
    stop();
}

void OmniWheelMotors::rotateClockwise(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    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
}

void OmniWheelMotors::rotateCounterClockwise(int speed) {
    // Ensure speed is at least minimum speed for stable movement
    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
}

void OmniWheelMotors::rotateToAngle(int targetAngle) {
    // Ensure angle is between 0 and 360
    targetAngle = targetAngle % 360;
    if (targetAngle < 0) targetAngle += 360;
    
    // 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
    
    // Wait for calculated time
    delay(rotationTime);
    
    // Stop all motors
    stop();
}

void OmniWheelMotors::setSpeedLimits(int min, int max) {
    // Set minimum and maximum speed values with bounds checking
    MIN_SPEED = constrain(min, 0, 255);
    MAX_SPEED = constrain(max, MIN_SPEED, 255);
}

void OmniWheelMotors::setRotationTime(int milliseconds) {
    // Set time for full rotation (must be positive)
    FULL_ROTATION_TIME = max(1, milliseconds);
}