@@ -0,0 +1,2397 @@
+package com.thegongoliers.output;%0A%0Aimport com.kylecorry.geometry.Path;%0Aimport com.thegongoliers.output.interfaces.DriveTrainInterface;%0Aimport edu.wpi.first.wpilibj.RobotDrive;%0A%0A/**%0A * Created by Kylec on 3/27/2017.%0A */%0Apublic class DriveUtil %7B%0A%0A    /**%0A     * Return the time to drive a given distance.%0A     *%0A     * @param distance      The distance to drive.%0A     * @param robotMaxSpeed The max speed of the robot.%0A     * @param targetSpeed   The max speed to drive this distance from 0 to 1 inclusive.%0A     * @return The time to drive a given distance.%0A     */%0A    public double timeToDriveDistanceDeadReckoning(double distance, double robotMaxSpeed, double targetSpeed) %7B%0A        double distancePerSecond = robotMaxSpeed * targetSpeed;%0A        return distance / distancePerSecond;%0A    %7D%0A%0A%0A    /**%0A     * Drive to a given distance using bang-bang control.%0A     *%0A     * @param distance        The distance to drive to.%0A     * @param currentDistance The current distance travelled (by encoders)%0A     * @param threshold       The threshold in which to stop driving.%0A     * @param maxSpeed        The max speed of the robot from 0 to 1 inclusive.%0A     * @param driveTrain      The drive train of the robot.%0A     * @return True if the robot is at the target distance within the threshold.%0A     */%0A    public boolean bangBangDriveDistance(double distance, double currentDistance, double threshold, double maxSpeed, DriveTrainInterface driveTrain) %7B%0A        double error = distance - currentDistance;%0A        if (error %3E threshold) %7B%0A            driveTrain.forward(maxSpeed);%0A        %7D else if (error %3C -threshold) %7B%0A            driveTrain.backward(maxSpeed);%0A        %7D else %7B%0A            driveTrain.stop();%0A            return true;%0A        %7D%0A        return false;%0A    %7D%0A%0A    /**%0A     * Drive along a precomputed trajectory with the given velocity and acceleration using feed forward control.%0A     *%0A     * @param velocity     The desired velocity.%0A     * @param acceleration The desired acceleration.%0A     * @param kv           The velocity constant (normally 1 / maxVelocity).%0A     * @param ka           The acceleration constant.%0A     * @param driveTrain   The drive train of the robot.%0A     */%0A    public void feedForwardControl(double velocity, double acceleration, double kv, double ka, DriveTrainInterface driveTrain) %7B%0A        driveTrain.forward(kv * velocity + ka * acceleration);%0A    %7D%0A%0A%0A%7D%0A

Add several driving control functions