Investigate CoPilot Suggestion about followTrajectory

[TaylerUva]

CoPilot made this suggestion. Might be worth investigate:

Unused parameter in followTrajectory method. The sample parameter contains velocity information (sample.vx, sample.vy, sample.omega) that should be used as feedforward values for the controller, but the code only uses sample.getPose() for feedback control. This could result in suboptimal trajectory following. Consider passing the velocity information to the controller as feedforward terms.

    // Calculate feedback control output
    ChassisSpeeds feedbackSpeeds = ConstDrivetrain.AUTO_ALIGN.PATH_AUTO_ALIGN_CONTROLLER.calculate(
        getPose(),
        desiredTarget,
        0,
        desiredTarget.getRotation());

    // Add feedforward (desired velocities from sample)
    ChassisSpeeds feedforwardSpeeds = new ChassisSpeeds(sample.vx, sample.vy, sample.omega);
    ChassisSpeeds automatedDTVelocity = new ChassisSpeeds(
        feedbackSpeeds.vxMetersPerSecond + feedforwardSpeeds.vxMetersPerSecond,
        feedbackSpeeds.vyMetersPerSecond + feedforwardSpeeds.vyMetersPerSecond,
        feedbackSpeeds.omegaRadiansPerSecond + feedforwardSpeeds.omegaRadiansPerSecond
    );

Originally posted by @Copilot in #41