Add OmniWheelMotors class and configuration files for motor control

.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

.vscode/extensions.json

@@ -0,0 +1,10 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

lib/motor/OmniWheelMotors.cpp

@@ -0,0 +1,127 @@
+#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 = 50;      // 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, 0, MIN_SPEED);  // Motor 2 forward
+    setMotorSpeed(motor_3, 0, 0);          // Motor 3 neutral
+    
+    // Wait for approximately one full rotation
+    delay(FULL_ROTATION_TIME);
+    
+    // Stop all motors
+    stop();
+}
+
+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);
+}

lib/motor/OmniWheelMotors.hpp

@@ -0,0 +1,53 @@
+#ifndef OMNIWHEEL_MOTORS_HPP
+#define OMNIWHEEL_MOTORS_HPP
+
+#include <Arduino.h>
+#include <Wire.h>
+
+class OmniWheelMotors {
+public:
+    struct Motor {
+        int R_PWM;
+        int L_PWM;
+    };
+
+    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);
+
+    // Initialize the motors
+    void begin();
+    
+    // Basic movement functions
+    void moveForward(int speed);
+    void moveBackward(int speed);
+    void moveLeft(int speed);
+    void moveRight(int speed);
+    
+    // Rotation functions
+    void rotate360();
+    void rotateToAngle(int targetAngle);
+    
+    // Stop all motors
+    void stop();
+    
+    // Direct motor control
+    void setMotorSpeed(Motor motor, int rightSpeed, int leftSpeed);
+    
+    // Set maximum and minimum speed limits
+    void setSpeedLimits(int min, int max);
+    
+    // Set time for full rotation
+    void setRotationTime(int milliseconds);
+
+private:
+    Motor motor_1;   // First motor
+    Motor motor_2;   // Second motor
+    Motor motor_3;   // Third motor
+    
+    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