Drive Vector Algorithm

Understanding how VingVing Robot calculates motor powers for path following

What is the Drive Vector?

The drive vector is the combination of translational (X, Y) and rotational (heading) movements that the follower calculates to keep the robot on path. Understanding this helps debug issues and optimize performance.

How It Works

The Calculation Process:

Find Closest Point: Follower identifies the closest point on the path to robot's current position
Calculate Error: Compute X, Y, and heading errors from closest point to current position
PID Correction: Apply PID controllers to each error component (translational, heading, drive, centripetal)
Feedforward: Add velocity-based feedforward term for smoother following
Combine Vectors: Merge translational and rotational components into drive vector
Convert to Motor Powers: Transform drive vector into individual wheel powers for mecanum drive

Vector Components

Translational Vector (X, Y)

Drives the robot toward the closest point on the path. Controlled by translational PID.

// Simplified calculation
double xError = targetX - currentX;
double yError = targetY - currentY;

double xPower = translationalPID.calculate(xError);
double yPower = translationalPID.calculate(yError);

// Translational vector = (xPower, yPower)

Rotational Vector (Heading)

Rotates the robot to match target heading. Controlled by heading PID.

// Heading error calculation
double headingError = targetHeading - currentHeading;

// Normalize to [-π, π]
headingError = Math.atan2(Math.sin(headingError), Math.cos(headingError));

double rotationPower = headingPID.calculate(headingError);

// Rotation vector adds/subtracts from wheel powers

Drive Vector (Along Path)

Pushes robot forward along the path direction. Controlled by drive PID.

// How far ahead/behind on path
double driveError = getProgressAlongPath();

double drivePower = drivePID.calculate(driveError);

// Adds forward momentum along path tangent

Centripetal Vector (Curve Correction)

Compensates for centrifugal force on curved paths. Controlled by centripetal PID.

// Calculate required centripetal force
double curvature = getCurvatureAtPoint();
double velocity = getCurrentVelocity();
double centripetalForce = curvature * velocity * velocity;

double centripetalPower = centripetalPID.calculate(centripetalForce);

// Pushes robot toward center of curve

Motor Power Conversion

Mecanum Drive Kinematics:

The drive vector (x, y, rotation) is converted to four wheel powers using mecanum drive inverse kinematics:

// Standard mecanum drive equations
double leftFront = x + y + rotation;
double rightFront = x - y - rotation;
double leftBack = x - y + rotation;
double rightBack = x + y - rotation;

// Normalize to [-1, 1]
double maxPower = Math.max(Math.max(Math.abs(leftFront), Math.abs(rightFront)),
                           Math.max(Math.abs(leftBack), Math.abs(rightBack)));

if (maxPower > 1.0) {
    leftFront /= maxPower;
    rightFront /= maxPower;
    leftBack /= maxPower;
    rightBack /= maxPower;
}

Field-Centric Conversion:

Drive vector is calculated in field coordinates but must be converted to robot-centric for motor powers:

// Rotate drive vector by robot heading
double robotX = fieldX * Math.cos(-heading) - fieldY * Math.sin(-heading);
double robotY = fieldX * Math.sin(-heading) + fieldY * Math.cos(-heading);

// Now apply mecanum equations with robot-relative x, y

Debugging with Drive Vectors

Telemetry for Vector Analysis:

while (!follower.isPathComplete() && opModeIsActive()) {
    robot.update();
    follower.update();
    robot.setDrivePowers(follower.getMotorPowers());

    // Debug drive vector components
    double[] driveVector = follower.getDriveVector();
    telemetry.addData("Drive X", "%.2f", driveVector[0]);
    telemetry.addData("Drive Y", "%.2f", driveVector[1]);
    telemetry.addData("Rotation", "%.2f", driveVector[2]);

    // Debug motor powers
    double[] powers = follower.getMotorPowers();
    telemetry.addData("LF Power", "%.2f", powers[0]);
    telemetry.addData("RF Power", "%.2f", powers[1]);
    telemetry.addData("LB Power", "%.2f", powers[2]);
    telemetry.addData("RB Power", "%.2f", powers[3]);

    telemetry.update();
}

Interpreting Vector Values:

Large X/Y values: Robot far from path - check localization or PID tuning
Oscillating rotation: Heading PID too aggressive - reduce P, increase D
All motor powers same: Pure rotation - not translating properly
Opposing wheel signs: Normal for strafing/rotation - verify direction is correct

Advanced: Custom Drive Vector Modifiers

For advanced users: modify drive vector before motor power conversion for custom behaviors.

// Example: Limit lateral movement (reduce strafe)
public class LimitedStrafeDrive extends Follower {
    @Override
    protected double[] calculateDriveVector() {
        double[] vector = super.calculateDriveVector();

        // Reduce Y (strafe) component by 50%
        vector[1] *= 0.5;

        return vector;
    }
}

// Useful for robots with poor lateral traction

Drive Vector Tips

💡 Monitor Vector Magnitude

Large drive vector components mean aggressive corrections - robot may oscillate. Tune PID to reduce magnitude.

💡 Balanced Components

Well-tuned follower shows balanced X, Y, and rotation values. If one dominates, that axis needs PID adjustment.

💡 Vector Saturation

If motor powers are always at ±1.0, drive vector is saturating. Reduce PID gains or velocity constraints.