Merge remote-tracking branch 'origin/master'

# Conflicts:
#	TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java

FtcRobotController/build.gradle

@@ -8,7 +8,7 @@ apply plugin: 'com.android.library'
 android {
 
     defaultConfig {
-        minSdkVersion 23
+        minSdkVersion 24
         //noinspection ExpiredTargetSdkVersion
         targetSdkVersion 28
         buildConfigField "String", "APP_BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"'

TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java

@@ -1,83 +0,0 @@
-package org.RoadRunner.drive;
-
-import com.qualcomm.robotcore.hardware.PIDFCoefficients;
-
-/*
- * Constants shared between multiple drive types.
- *
- * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
- * fields may also be edited through the dashboard (connect to the robot's WiFi network and
- * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
- * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
- *
- * These are not the only parameters; some are located in the localizer classes, drive base classes,
- * and op modes themselves.
- */
-public class DriveConstants {
-
-    /*
-     * These are motor constants that should be listed online for your motors.
-     */
-    public static final double TICKS_PER_REV = 560;
-    public static final double MAX_RPM = 300;
-
-    /*
-     * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
-     * Set this flag to false if drive encoders are not present and an alternative localization
-     * method is in use (e.g., tracking wheels).
-     *
-     * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
-     * from DriveVelocityPIDTuner.
-     */
-    public static final boolean RUN_USING_ENCODER = true;
-    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
-            getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
-
-    /*
-     * These are physical constants that can be determined from your robot (including the track
-     * width; it will be tune empirically later although a rough estimate is important). Users are
-     * free to chose whichever linear distance unit they would like so long as it is consistently
-     * used. The default values were selected with inches in mind. Road runner uses radians for
-     * angular distances although most angular parameters are wrapped in Math.toRadians() for
-     * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
-     */
-    public static double WHEEL_RADIUS = 1.47637795; // in
-    public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
-    public static double TRACK_WIDTH = 10; // in
-
-    /*
-     * These are the feedforward parameters used to model the drive motor behavior. If you are using
-     * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
-     * motor encoders or have elected not to use them for velocity control, these values should be
-     * empirically tuned.
-     */
-    public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
-    public static double kA = 0;
-    public static double kStatic = 0;
-
-    /*
-     * These values are used to generate the trajectories for you robot. To ensure proper operation,
-     * the constraints should never exceed ~80% of the robot's actual capabilities. While Road
-     * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
-     * small and gradually increase them later after everything is working. All distance units are
-     * inches.
-     */
-    public static double MAX_VEL = 37.10542498;
-    public static double MAX_ACCEL = 37.10542498;
-    public static double MAX_ANG_VEL = Math.toRadians(60);
-    public static double MAX_ANG_ACCEL = Math.toRadians(60);
-
-
-    public static double encoderTicksToInches(double ticks) {
-        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
-    }
-
-    public static double rpmToVelocity(double rpm) {
-        return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
-    }
-
-    public static double getMotorVelocityF(double ticksPerSecond) {
-        // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
-        return 32767 / ticksPerSecond;
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java

@@ -1,335 +0,0 @@
-package org.RoadRunner.drive;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.control.PIDCoefficients;
-import com.acmerobotics.roadrunner.drive.DriveSignal;
-import com.acmerobotics.roadrunner.drive.MecanumDrive;
-import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
-import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
-import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
-import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.hardware.lynx.LynxModule;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.PIDFCoefficients;
-import com.qualcomm.robotcore.hardware.VoltageSensor;
-import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-import static org.RoadRunner.drive.DriveConstants.MAX_ACCEL;
-import static org.RoadRunner.drive.DriveConstants.MAX_ANG_ACCEL;
-import static org.RoadRunner.drive.DriveConstants.MAX_ANG_VEL;
-import static org.RoadRunner.drive.DriveConstants.MAX_VEL;
-import static org.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
-import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.RoadRunner.drive.DriveConstants.TRACK_WIDTH;
-import static org.RoadRunner.drive.DriveConstants.encoderTicksToInches;
-import static org.RoadRunner.drive.DriveConstants.kA;
-import static org.RoadRunner.drive.DriveConstants.kStatic;
-import static org.RoadRunner.drive.DriveConstants.kV;
-
-import org.RoadRunner.trajectorysequence.TrajectorySequence;
-import org.RoadRunner.trajectorysequence.TrajectorySequenceBuilder;
-import org.RoadRunner.trajectorysequence.TrajectorySequenceRunner;
-import org.RoadRunner.util.LynxModuleUtil;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-/*
- * Simple mecanum drive hardware implementation for REV hardware.
- */
-@Config
-public class SampleMecanumDrive extends MecanumDrive {
-    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
-    public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
-
-    public static double LATERAL_MULTIPLIER = 1;
-
-    public static double VX_WEIGHT = 1;
-    public static double VY_WEIGHT = 1;
-    public static double OMEGA_WEIGHT = 1;
-    public final StandardTrackingWheelLocalizer localizer;
-
-    private TrajectorySequenceRunner trajectorySequenceRunner;
-
-    private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
-    private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
-
-    private TrajectoryFollower follower;
-
-    private DcMotorEx leftFront, leftRear, rightRear, rightFront;
-    private List<DcMotorEx> motors;
-
-    private BNO055IMU imu;
-    private VoltageSensor batteryVoltageSensor;
-
-    public SampleMecanumDrive(HardwareMap hardwareMap) {
-        super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
-
-        follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
-                new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
-
-        LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
-
-        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
-
-        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
-            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
-        }
-
-        // TODO: adjust the names of the following hardware devices to match your configuration
-        imu = hardwareMap.get(BNO055IMU.class, "imu");
-        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
-        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
-        imu.initialize(parameters);
-
-        // TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does
-        // not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.)
-        //
-        //             | +Z axis
-        //             |
-        //             |
-        //             |
-        //      _______|_____________     +Y axis
-        //     /       |_____________/|__________
-        //    /   REV / EXPANSION   //
-        //   /       / HUB         //
-        //  /_______/_____________//
-        // |_______/_____________|/
-        //        /
-        //       / +X axis
-        //
-        // This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
-        // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
-        //
-        // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
-        // BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y);
-
-        leftFront = hardwareMap.get(DcMotorEx.class, "Front Left");
-        leftRear = hardwareMap.get(DcMotorEx.class, "Back Left");
-        rightRear = hardwareMap.get(DcMotorEx.class, "Back Right");
-        rightFront = hardwareMap.get(DcMotorEx.class, "Front Right");
-
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
-        rightRear.setDirection(DcMotorSimple.Direction.FORWARD);
-        rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
-
-        motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
-
-        for (DcMotorEx motor : motors) {
-            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
-            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
-            motor.setMotorType(motorConfigurationType);
-        }
-
-        if (RUN_USING_ENCODER) {
-            setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        }
-
-        setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
-        if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
-            setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
-        }
-
-        // TODO: reverse any motors using DcMotor.setDirection()
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
-        rightRear.setDirection(DcMotorSimple.Direction.FORWARD);
-        rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
-
-        // TODO: if desired, use setLocalizer() to change the localization method
-        // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
-        localizer = new StandardTrackingWheelLocalizer(hardwareMap);
-        setLocalizer(localizer);
-
-        trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
-    }
-
-    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
-        return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
-    }
-
-    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
-        return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
-    }
-
-    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
-        return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
-    }
-
-    public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
-        return new TrajectorySequenceBuilder(
-                startPose,
-                VEL_CONSTRAINT, ACCEL_CONSTRAINT,
-                MAX_ANG_VEL, MAX_ANG_ACCEL
-        );
-    }
-
-    public void turnAsync(double angle) {
-        trajectorySequenceRunner.followTrajectorySequenceAsync(
-                trajectorySequenceBuilder(getPoseEstimate())
-                        .turn(angle)
-                        .build()
-        );
-    }
-
-    public void turn(double angle) {
-        turnAsync(angle);
-        waitForIdle();
-    }
-
-    public void followTrajectoryAsync(Trajectory trajectory) {
-        trajectorySequenceRunner.followTrajectorySequenceAsync(
-                trajectorySequenceBuilder(trajectory.start())
-                        .addTrajectory(trajectory)
-                        .build()
-        );
-    }
-
-    public void followTrajectory(Trajectory trajectory) {
-        followTrajectoryAsync(trajectory);
-        waitForIdle();
-    }
-
-    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
-        trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
-    }
-
-    public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
-        followTrajectorySequenceAsync(trajectorySequence);
-        waitForIdle();
-    }
-
-    public Pose2d getLastError() {
-        return trajectorySequenceRunner.getLastPoseError();
-    }
-
-    public void update() {
-        updatePoseEstimate();
-        DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
-        if (signal != null) setDriveSignal(signal);
-    }
-
-    public void waitForIdle() {
-        while (!Thread.currentThread().isInterrupted() && isBusy())
-            update();
-    }
-
-    public boolean isBusy() {
-        return trajectorySequenceRunner.isBusy();
-    }
-
-    public void setMode(DcMotor.RunMode runMode) {
-        for (DcMotorEx motor : motors) {
-            motor.setMode(runMode);
-        }
-    }
-
-    public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
-        for (DcMotorEx motor : motors) {
-            motor.setZeroPowerBehavior(zeroPowerBehavior);
-        }
-    }
-
-    public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
-        PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
-                coefficients.p, coefficients.i, coefficients.d,
-                coefficients.f * 12 / batteryVoltageSensor.getVoltage()
-        );
-
-        for (DcMotorEx motor : motors) {
-            motor.setPIDFCoefficients(runMode, compensatedCoefficients);
-        }
-    }
-
-    public void setWeightedDrivePower(Pose2d drivePower) {
-        Pose2d vel = drivePower;
-
-        if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
-                + Math.abs(drivePower.getHeading()) > 1) {
-            // re-normalize the powers according to the weights
-            double denom = VX_WEIGHT * Math.abs(drivePower.getX())
-                    + VY_WEIGHT * Math.abs(drivePower.getY())
-                    + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
-
-            vel = new Pose2d(
-                    VX_WEIGHT * drivePower.getX(),
-                    VY_WEIGHT * drivePower.getY(),
-                    OMEGA_WEIGHT * drivePower.getHeading()
-            ).div(denom);
-        }
-
-        setDrivePower(vel);
-    }
-
-    @NonNull
-    @Override
-    public List<Double> getWheelPositions() {
-        List<Double> wheelPositions = new ArrayList<>();
-        for (DcMotorEx motor : motors) {
-            wheelPositions.add(encoderTicksToInches(motor.getCurrentPosition()));
-        }
-        return wheelPositions;
-    }
-
-    @Override
-    public List<Double> getWheelVelocities() {
-        List<Double> wheelVelocities = new ArrayList<>();
-        for (DcMotorEx motor : motors) {
-            wheelVelocities.add(encoderTicksToInches(motor.getVelocity()));
-        }
-        return wheelVelocities;
-    }
-
-    @Override
-    public void setMotorPowers(double v, double v1, double v2, double v3) {
-        leftFront.setPower(v);
-        leftRear.setPower(v1);
-        rightRear.setPower(v2);
-        rightFront.setPower(v3);
-    }
-
-    @Override
-    public double getRawExternalHeading() {
-        return imu.getAngularOrientation().firstAngle;
-    }
-
-    public void telemetry(Telemetry telemetry) {
-        telemetry.addData("front left", leftFront.getPower());
-        telemetry.addData("back left", leftRear.getPower());
-        telemetry.addData("front right", rightFront.getPower());
-        telemetry.addData("back right", rightRear.getPower());
-    }
-
-    @Override
-    public Double getExternalHeadingVelocity() {
-        return (double) imu.getAngularVelocity().zRotationRate;
-    }
-
-    public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
-        return new MinVelocityConstraint(Arrays.asList(
-                new AngularVelocityConstraint(maxAngularVel),
-                new MecanumVelocityConstraint(maxVel, trackWidth)
-        ));
-    }
-
-    public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
-        return new ProfileAccelerationConstraint(maxAccel);
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/SampleTankDrive.java

@@ -1,319 +0,0 @@
-package org.RoadRunner.drive;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.roadrunner.control.PIDCoefficients;
-import com.acmerobotics.roadrunner.drive.DriveSignal;
-import com.acmerobotics.roadrunner.drive.TankDrive;
-import com.acmerobotics.roadrunner.followers.TankPIDVAFollower;
-import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
-import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
-import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.hardware.lynx.LynxModule;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.PIDFCoefficients;
-import com.qualcomm.robotcore.hardware.VoltageSensor;
-import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
-
-import org.RoadRunner.trajectorysequence.TrajectorySequence;
-import org.RoadRunner.trajectorysequence.TrajectorySequenceBuilder;
-import org.RoadRunner.trajectorysequence.TrajectorySequenceRunner;
-import org.RoadRunner.util.LynxModuleUtil;
-
-import java.util.Arrays;
-import java.util.List;
-
-import static org.RoadRunner.drive.DriveConstants.MAX_ACCEL;
-import static org.RoadRunner.drive.DriveConstants.MAX_ANG_ACCEL;
-import static org.RoadRunner.drive.DriveConstants.MAX_ANG_VEL;
-import static org.RoadRunner.drive.DriveConstants.MAX_VEL;
-import static org.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
-import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.RoadRunner.drive.DriveConstants.TRACK_WIDTH;
-import static org.RoadRunner.drive.DriveConstants.encoderTicksToInches;
-import static org.RoadRunner.drive.DriveConstants.kA;
-import static org.RoadRunner.drive.DriveConstants.kStatic;
-import static org.RoadRunner.drive.DriveConstants.kV;
-
-/*
- * Simple tank drive hardware implementation for REV hardware.
- */
-public class SampleTankDrive extends TankDrive {
-    public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0);
-    public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0);
-    public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
-
-    public static double VX_WEIGHT = 1;
-    public static double OMEGA_WEIGHT = 1;
-
-    private TrajectorySequenceRunner trajectorySequenceRunner;
-
-    private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
-    private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL);
-
-    private TrajectoryFollower follower;
-
-    private List<DcMotorEx> motors, leftMotors, rightMotors;
-    private BNO055IMU imu;
-
-    private VoltageSensor batteryVoltageSensor;
-
-    public SampleTankDrive(HardwareMap hardwareMap) {
-        super(kV, kA, kStatic, TRACK_WIDTH);
-
-        follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID,
-                new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
-
-        LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
-
-        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
-
-        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
-            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
-        }
-
-        // TODO: adjust the names of the following hardware devices to match your configuration
-        imu = hardwareMap.get(BNO055IMU.class, "imu");
-        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
-        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
-        imu.initialize(parameters);
-
-        // TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does
-        // not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.)
-        //
-        //             | +Z axis
-        //             |
-        //             |
-        //             |
-        //      _______|_____________     +Y axis
-        //     /       |_____________/|__________
-        //    /   REV / EXPANSION   //
-        //   /       / HUB         //
-        //  /_______/_____________//
-        // |_______/_____________|/
-        //        /
-        //       / +X axis
-        //
-        // This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
-        // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
-        //
-        // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
-        // BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y);
-
-        // add/remove motors depending on your robot (e.g., 6WD)
-        DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
-        DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
-        DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
-        DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
-
-        motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
-        leftMotors = Arrays.asList(leftFront, leftRear);
-        rightMotors = Arrays.asList(rightFront, rightRear);
-
-        for (DcMotorEx motor : motors) {
-            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
-            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
-            motor.setMotorType(motorConfigurationType);
-        }
-
-        if (RUN_USING_ENCODER) {
-            setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        }
-
-        setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
-        if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
-            setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
-        }
-
-        // TODO: reverse any motors using DcMotor.setDirection()
-
-        // TODO: if desired, use setLocalizer() to change the localization method
-        // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
-
-        trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
-    }
-
-    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
-        return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint);
-    }
-
-    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
-        return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint);
-    }
-
-    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
-        return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint);
-    }
-
-    public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
-        return new TrajectorySequenceBuilder(
-                startPose,
-                VEL_CONSTRAINT, accelConstraint,
-                MAX_ANG_VEL, MAX_ANG_ACCEL
-        );
-    }
-
-    public void turnAsync(double angle) {
-        trajectorySequenceRunner.followTrajectorySequenceAsync(
-                trajectorySequenceBuilder(getPoseEstimate())
-                        .turn(angle)
-                        .build()
-        );
-    }
-
-    public void turn(double angle) {
-        turnAsync(angle);
-        waitForIdle();
-    }
-
-    public void followTrajectoryAsync(Trajectory trajectory) {
-        trajectorySequenceRunner.followTrajectorySequenceAsync(
-                trajectorySequenceBuilder(trajectory.start())
-                        .addTrajectory(trajectory)
-                        .build()
-        );
-    }
-
-    public void followTrajectory(Trajectory trajectory) {
-        followTrajectoryAsync(trajectory);
-        waitForIdle();
-    }
-
-    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
-        trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
-    }
-
-    public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
-        followTrajectorySequenceAsync(trajectorySequence);
-        waitForIdle();
-    }
-
-    public Pose2d getLastError() {
-        return trajectorySequenceRunner.getLastPoseError();
-    }
-
-
-    public void update() {
-        updatePoseEstimate();
-        DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
-        if (signal != null) setDriveSignal(signal);
-    }
-
-    public void waitForIdle() {
-        while (!Thread.currentThread().isInterrupted() && isBusy())
-            update();
-    }
-
-    public boolean isBusy() {
-        return trajectorySequenceRunner.isBusy();
-    }
-
-    public void setMode(DcMotor.RunMode runMode) {
-        for (DcMotorEx motor : motors) {
-            motor.setMode(runMode);
-        }
-    }
-
-    public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
-        for (DcMotorEx motor : motors) {
-            motor.setZeroPowerBehavior(zeroPowerBehavior);
-        }
-    }
-
-    public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
-        PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
-                coefficients.p, coefficients.i, coefficients.d,
-                coefficients.f * 12 / batteryVoltageSensor.getVoltage()
-        );
-        for (DcMotorEx motor : motors) {
-            motor.setPIDFCoefficients(runMode, compensatedCoefficients);
-        }
-    }
-
-    public void setWeightedDrivePower(Pose2d drivePower) {
-        Pose2d vel = drivePower;
-
-        if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) {
-            // re-normalize the powers according to the weights
-            double denom = VX_WEIGHT * Math.abs(drivePower.getX())
-                    + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
-
-            vel = new Pose2d(
-                    VX_WEIGHT * drivePower.getX(),
-                    0,
-                    OMEGA_WEIGHT * drivePower.getHeading()
-            ).div(denom);
-        } else {
-            // Ensure the y axis is zeroed out.
-            vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading());
-        }
-
-        setDrivePower(vel);
-    }
-
-    @NonNull
-    @Override
-    public List<Double> getWheelPositions() {
-        double leftSum = 0, rightSum = 0;
-        for (DcMotorEx leftMotor : leftMotors) {
-            leftSum += encoderTicksToInches(leftMotor.getCurrentPosition());
-        }
-        for (DcMotorEx rightMotor : rightMotors) {
-            rightSum += encoderTicksToInches(rightMotor.getCurrentPosition());
-        }
-        return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
-    }
-
-    public List<Double> getWheelVelocities() {
-        double leftSum = 0, rightSum = 0;
-        for (DcMotorEx leftMotor : leftMotors) {
-            leftSum += encoderTicksToInches(leftMotor.getVelocity());
-        }
-        for (DcMotorEx rightMotor : rightMotors) {
-            rightSum += encoderTicksToInches(rightMotor.getVelocity());
-        }
-        return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
-    }
-
-    @Override
-    public void setMotorPowers(double v, double v1) {
-        for (DcMotorEx leftMotor : leftMotors) {
-            leftMotor.setPower(v);
-        }
-        for (DcMotorEx rightMotor : rightMotors) {
-            rightMotor.setPower(v1);
-        }
-    }
-
-    @Override
-    public double getRawExternalHeading() {
-        return imu.getAngularOrientation().firstAngle;
-    }
-
-    @Override
-    public Double getExternalHeadingVelocity() {
-        return (double) imu.getAngularVelocity().zRotationRate;
-    }
-
-    public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
-        return new MinVelocityConstraint(Arrays.asList(
-                new AngularVelocityConstraint(maxAngularVel),
-                new TankVelocityConstraint(maxVel, trackWidth)
-        ));
-    }
-
-    public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
-        return new ProfileAccelerationConstraint(maxAccel);
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java

@@ -1,101 +0,0 @@
-package org.RoadRunner.drive;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.RoadRunner.util.Encoder;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-import java.util.Arrays;
-import java.util.List;
-
-/*
- * Sample tracking wheel localizer implementation assuming the standard configuration:
- *
- *    /--------------\
- *    |     ____     |
- *    |     ----     |
- *    | ||        || |
- *    | ||        || |
- *    |              |
- *    |              |
- *    \--------------/
- *
- */
-@Config
-public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
-    public static double TICKS_PER_REV = 8192;
-    public static double WHEEL_RADIUS = 1.47637795; // in
-    public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
-
-    public static double LATERAL_DISTANCE = 9.2913393; // in; distance between the left and right wheels
-    public static double FORWARD_OFFSET = 3.54833; // in; offset of the lateral wheel
-
-    public static double X_MULTIPLIER = 0.8039; // Multiplier in the X direction
-    //   9.14228723
-    //   8.65411042
-
-    public static double Y_MULTIPLIER = 0.8039; // Multiplier in the Y direction
-
-    private Encoder leftEncoder, rightEncoder, frontEncoder;
-
-    public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
-        super(Arrays.asList(
-                new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
-                new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
-                new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
-        ));
-
-        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderL"));
-        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderR"));
-        frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderH"));
-
-        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
-        rightEncoder.setDirection(Encoder.Direction.REVERSE);
-        leftEncoder.setDirection(Encoder.Direction.REVERSE);
-        frontEncoder.setDirection(Encoder.Direction.REVERSE);
-
-
-    }
-
-    public static double encoderTicksToInches(double ticks) {
-        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
-    }
-
-    @NonNull
-    @Override
-    public List<Double> getWheelPositions() {
-        return Arrays.asList(
-                encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER,
-                encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER,
-                encoderTicksToInches(frontEncoder.getCurrentPosition()) * Y_MULTIPLIER
-        );
-    }
-
-    @NonNull
-    @Override
-    public List<Double> getWheelVelocities() {
-        // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
-        //  competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
-        //  compensation method
-
-        return Arrays.asList(
-                encoderTicksToInches(leftEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
-                encoderTicksToInches(rightEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
-                encoderTicksToInches(frontEncoder.getCorrectedVelocity()) * Y_MULTIPLIER
-        );
-    }
-
-    public void telemetry(Telemetry telemetry){
-        telemetry.addData("right", rightEncoder.getCurrentPosition());
-        telemetry.addData("left", leftEncoder.getCurrentPosition());
-        telemetry.addData("front", frontEncoder.getCurrentPosition());
-    }
-
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/AutomaticFeedforwardTuner.java

@@ -1,221 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import static org.RoadRunner.drive.DriveConstants.MAX_RPM;
-import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.RoadRunner.drive.DriveConstants.rpmToVelocity;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.util.NanoClock;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.util.RobotLog;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.RoadRunner.util.LoggingUtil;
-import org.RoadRunner.util.RegressionUtil;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.robotcore.internal.system.Misc;
-
-import java.util.ArrayList;
-import java.util.List;
-
-/*
- * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
- * outline of the procedure:
- *   1. Slowly ramp the motor power and record encoder values along the way.
- *   2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
- *      and an optional intercept (kStatic).
- *   3. Accelerate the robot (apply constant power) and record the encoder counts.
- *   4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
- *      regression.
- */
-@Config
-@Autonomous(group = "drive")
-public class AutomaticFeedforwardTuner extends LinearOpMode {
-    public static double MAX_POWER = 0.7;
-    public static double DISTANCE = 100; // in
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        if (RUN_USING_ENCODER) {
-            RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
-                    "when using the built-in drive motor velocity PID.");
-        }
-
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        NanoClock clock = NanoClock.system();
-
-        telemetry.addLine("Press play to begin the feedforward tuning routine");
-        telemetry.update();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        telemetry.clearAll();
-        telemetry.addLine("Would you like to fit kStatic?");
-        telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
-        telemetry.update();
-
-        boolean fitIntercept = false;
-        while (!isStopRequested()) {
-            if (gamepad1.y) {
-                fitIntercept = true;
-                while (!isStopRequested() && gamepad1.y) {
-                    idle();
-                }
-                break;
-            } else if (gamepad1.b) {
-                while (!isStopRequested() && gamepad1.b) {
-                    idle();
-                }
-                break;
-            }
-            idle();
-        }
-
-        telemetry.clearAll();
-        telemetry.addLine(Misc.formatInvariant(
-                "Place your robot on the field with at least %.2f in of room in front", DISTANCE));
-        telemetry.addLine("Press (Y/Δ) to begin");
-        telemetry.update();
-
-        while (!isStopRequested() && !gamepad1.y) {
-            idle();
-        }
-        while (!isStopRequested() && gamepad1.y) {
-            idle();
-        }
-
-        telemetry.clearAll();
-        telemetry.addLine("Running...");
-        telemetry.update();
-
-        double maxVel = rpmToVelocity(MAX_RPM);
-        double finalVel = MAX_POWER * maxVel;
-        double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
-        double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
-
-        List<Double> timeSamples = new ArrayList<>();
-        List<Double> positionSamples = new ArrayList<>();
-        List<Double> powerSamples = new ArrayList<>();
-
-        drive.setPoseEstimate(new Pose2d());
-
-        double startTime = clock.seconds();
-        while (!isStopRequested()) {
-            double elapsedTime = clock.seconds() - startTime;
-            if (elapsedTime > rampTime) {
-                break;
-            }
-            double vel = accel * elapsedTime;
-            double power = vel / maxVel;
-
-            timeSamples.add(elapsedTime);
-            positionSamples.add(drive.getPoseEstimate().getX());
-            powerSamples.add(power);
-
-            drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
-            drive.updatePoseEstimate();
-        }
-        drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
-
-        RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
-                timeSamples, positionSamples, powerSamples, fitIntercept,
-                LoggingUtil.getLogFile(Misc.formatInvariant(
-                        "DriveRampRegression-%d.csv", System.currentTimeMillis())));
-
-        telemetry.clearAll();
-        telemetry.addLine("Quasi-static ramp up test complete");
-        if (fitIntercept) {
-            telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
-                    rampResult.kV, rampResult.kStatic, rampResult.rSquare));
-        } else {
-            telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
-                    rampResult.kStatic, rampResult.rSquare));
-        }
-        telemetry.addLine("Would you like to fit kA?");
-        telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
-        telemetry.update();
-
-        boolean fitAccelFF = false;
-        while (!isStopRequested()) {
-            if (gamepad1.y) {
-                fitAccelFF = true;
-                while (!isStopRequested() && gamepad1.y) {
-                    idle();
-                }
-                break;
-            } else if (gamepad1.b) {
-                while (!isStopRequested() && gamepad1.b) {
-                    idle();
-                }
-                break;
-            }
-            idle();
-        }
-
-        if (fitAccelFF) {
-            telemetry.clearAll();
-            telemetry.addLine("Place the robot back in its starting position");
-            telemetry.addLine("Press (Y/Δ) to continue");
-            telemetry.update();
-
-            while (!isStopRequested() && !gamepad1.y) {
-                idle();
-            }
-            while (!isStopRequested() && gamepad1.y) {
-                idle();
-            }
-
-            telemetry.clearAll();
-            telemetry.addLine("Running...");
-            telemetry.update();
-
-            double maxPowerTime = DISTANCE / maxVel;
-
-            timeSamples.clear();
-            positionSamples.clear();
-            powerSamples.clear();
-
-            drive.setPoseEstimate(new Pose2d());
-            drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
-
-            startTime = clock.seconds();
-            while (!isStopRequested()) {
-                double elapsedTime = clock.seconds() - startTime;
-                if (elapsedTime > maxPowerTime) {
-                    break;
-                }
-
-                timeSamples.add(elapsedTime);
-                positionSamples.add(drive.getPoseEstimate().getX());
-                powerSamples.add(MAX_POWER);
-
-                drive.updatePoseEstimate();
-            }
-            drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
-
-            RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
-                    timeSamples, positionSamples, powerSamples, rampResult,
-                    LoggingUtil.getLogFile(Misc.formatInvariant(
-                            "DriveAccelRegression-%d.csv", System.currentTimeMillis())));
-
-            telemetry.clearAll();
-            telemetry.addLine("Constant power test complete");
-            telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
-                    accelResult.kA, accelResult.rSquare));
-            telemetry.update();
-        }
-
-        while (!isStopRequested()) {
-            idle();
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/BackAndForth.java

@@ -1,52 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-
-/*
- * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
- * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
- * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
- * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
- * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
- * you've successfully connected, start the program, and your robot will begin moving forward and
- * backward. You should observe the target position (green) and your pose estimate (blue) and adjust
- * your follower PID coefficients such that you follow the target position as accurately as possible.
- * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
- * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
- * These coefficients can be tuned live in dashboard.
- *
- * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
- * is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
- */
-@Config
-@Autonomous(group = "drive")
-public class BackAndForth extends LinearOpMode {
-
-    public static double DISTANCE = 50;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
-                .forward(DISTANCE)
-                .build();
-
-        Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
-                .back(DISTANCE)
-                .build();
-
-        waitForStart();
-
-        while (opModeIsActive() && !isStopRequested()) {
-            drive.followTrajectory(trajectoryForward);
-            drive.followTrajectory(trajectoryBackward);
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/DriveVelocityPIDTuner.java

@@ -1,171 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import static org.RoadRunner.drive.DriveConstants.MAX_ACCEL;
-import static org.RoadRunner.drive.DriveConstants.MAX_VEL;
-import static org.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
-import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.RoadRunner.drive.DriveConstants.kV;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.profile.MotionProfile;
-import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
-import com.acmerobotics.roadrunner.profile.MotionState;
-import com.acmerobotics.roadrunner.util.NanoClock;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.util.RobotLog;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-import java.util.List;
-
-/*
- * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
- * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
- * important as the positional parameters. Like the other manual tuning routines, this op mode
- * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
- * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
- * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
- * connected, start the program, and your robot will begin moving forward and backward according to
- * a motion profile. Your job is to graph the velocity errors over time and adjust the PID
- * coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
- * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
- * MOTOR_VELO_PID field.
- *
- * Recommended tuning process:
- *
- * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
- *    mitigate oscillations.
- * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
- * 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
- *
- * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
- * user to reset the position of the bot in the event that it drifts off the path.
- * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
- */
-@Config
-@Autonomous(group = "drive")
-public class DriveVelocityPIDTuner extends LinearOpMode {
-    public static double DISTANCE = 72; // in
-
-    enum Mode {
-        DRIVER_MODE,
-        TUNING_MODE
-    }
-
-    private static MotionProfile generateProfile(boolean movingForward) {
-        MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
-        MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
-        return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
-    }
-
-    @Override
-    public void runOpMode() {
-        if (!RUN_USING_ENCODER) {
-            RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
-                    "PID is not in use", getClass().getSimpleName());
-        }
-
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        Mode mode = Mode.TUNING_MODE;
-
-        double lastKp = MOTOR_VELO_PID.p;
-        double lastKi = MOTOR_VELO_PID.i;
-        double lastKd = MOTOR_VELO_PID.d;
-        double lastKf = MOTOR_VELO_PID.f;
-
-        drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
-
-        NanoClock clock = NanoClock.system();
-
-        telemetry.addLine("Ready!");
-        telemetry.update();
-        telemetry.clearAll();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        boolean movingForwards = true;
-        MotionProfile activeProfile = generateProfile(true);
-        double profileStart = clock.seconds();
-
-
-        while (!isStopRequested()) {
-            telemetry.addData("mode", mode);
-
-            switch (mode) {
-                case TUNING_MODE:
-                    if (gamepad1.y) {
-                        mode = Mode.DRIVER_MODE;
-                        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-                    }
-
-                    // calculate and set the motor power
-                    double profileTime = clock.seconds() - profileStart;
-
-                    if (profileTime > activeProfile.duration()) {
-                        // generate a new profile
-                        movingForwards = !movingForwards;
-                        activeProfile = generateProfile(movingForwards);
-                        profileStart = clock.seconds();
-                    }
-
-                    MotionState motionState = activeProfile.get(profileTime);
-                    double targetPower = kV * motionState.getV();
-                    drive.setDrivePower(new Pose2d(targetPower, 0, 0));
-
-                    List<Double> velocities = drive.getWheelVelocities();
-
-                    // update telemetry
-                    telemetry.addData("targetVelocity", motionState.getV());
-                    for (int i = 0; i < velocities.size(); i++) {
-                        telemetry.addData("measuredVelocity" + i, velocities.get(i));
-                        telemetry.addData(
-                                "error" + i,
-                                motionState.getV() - velocities.get(i)
-                        );
-                    }
-                    break;
-                case DRIVER_MODE:
-                    if (gamepad1.b) {
-                        drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-
-                        mode = Mode.TUNING_MODE;
-                        movingForwards = true;
-                        activeProfile = generateProfile(movingForwards);
-                        profileStart = clock.seconds();
-                    }
-
-                    drive.setWeightedDrivePower(
-                            new Pose2d(
-                                    -gamepad1.left_stick_y,
-                                    -gamepad1.left_stick_x,
-                                    -gamepad1.right_stick_x
-                            )
-                    );
-                    break;
-            }
-
-            if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
-                    || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
-                drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
-
-                lastKp = MOTOR_VELO_PID.p;
-                lastKi = MOTOR_VELO_PID.i;
-                lastKd = MOTOR_VELO_PID.d;
-                lastKf = MOTOR_VELO_PID.f;
-            }
-
-            telemetry.update();
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/FollowerPIDTuner.java

@@ -1,55 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.RoadRunner.trajectorysequence.TrajectorySequence;
-
-/*
- * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
- * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
- * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
- * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
- * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
- * you've successfully connected, start the program, and your robot will begin driving in a square.
- * You should observe the target position (green) and your pose estimate (blue) and adjust your
- * follower PID coefficients such that you follow the target position as accurately as possible.
- * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
- * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
- * These coefficients can be tuned live in dashboard.
- */
-@Config
-@Autonomous(group = "drive")
-public class FollowerPIDTuner extends LinearOpMode {
-    public static double DISTANCE = 48; // in
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
-
-        drive.setPoseEstimate(startPose);
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        while (!isStopRequested()) {
-            TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
-                    .forward(DISTANCE)
-                    .turn(Math.toRadians(90))
-                    .forward(DISTANCE)
-                    .turn(Math.toRadians(90))
-                    .forward(DISTANCE)
-                    .turn(Math.toRadians(90))
-                    .forward(DISTANCE)
-                    .turn(Math.toRadians(90))
-                    .build();
-            drive.followTrajectorySequence(trajSeq);
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java

@@ -1,48 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-
-/**
- * This is a simple teleop routine for testing localization. Drive the robot around like a normal
- * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
- * errors are not out of the ordinary, especially with sudden drive motions). The goal of this
- * exercise is to ascertain whether the localizer has been configured properly (note: the pure
- * encoder localizer heading may be significantly off if the track width has not been tuned).
- */
-@TeleOp(name = "TC LocalizationTest", group = "drive")
-public class LocalizationTest extends LinearOpMode {
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        waitForStart();
-
-        while (!isStopRequested()) {
-            drive.setWeightedDrivePower(
-                    new Pose2d(
-                            -gamepad1.left_stick_y,
-                            -gamepad1.left_stick_x,
-                            -gamepad1.right_stick_x
-                    )
-            );
-
-            drive.update();
-
-            Pose2d poseEstimate = drive.getPoseEstimate();
-            telemetry.addData("x", poseEstimate.getX());
-            telemetry.addData("y", poseEstimate.getY());
-            telemetry.addData("heading", poseEstimate.getHeading());
-
-            drive.localizer.telemetry(telemetry);
-
-            telemetry.update();
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/ManualFeedforwardTuner.java

@@ -1,147 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import static org.RoadRunner.drive.DriveConstants.MAX_ACCEL;
-import static org.RoadRunner.drive.DriveConstants.MAX_VEL;
-import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.RoadRunner.drive.DriveConstants.kA;
-import static org.RoadRunner.drive.DriveConstants.kStatic;
-import static org.RoadRunner.drive.DriveConstants.kV;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.kinematics.Kinematics;
-import com.acmerobotics.roadrunner.profile.MotionProfile;
-import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
-import com.acmerobotics.roadrunner.profile.MotionState;
-import com.acmerobotics.roadrunner.util.NanoClock;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.util.RobotLog;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-import java.util.Objects;
-
-/*
- * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
- * tuning these coefficients is just as important as the positional parameters. Like the other
- * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
- * connect your computer to the RC's WiFi network. In your browser, navigate to
- * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
- * you are using the Control Hub. Once you've successfully connected, start the program, and your
- * robot will begin moving forward and backward according to a motion profile. Your job is to graph
- * the velocity errors over time and adjust the feedforward coefficients. Once you've found a
- * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
- *
- * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
- * user to reset the position of the bot in the event that it drifts off the path.
- * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
- */
-@Config
-@Autonomous(group = "drive")
-public class ManualFeedforwardTuner extends LinearOpMode {
-    public static double DISTANCE = 72; // in
-
-    private FtcDashboard dashboard = FtcDashboard.getInstance();
-
-    private SampleMecanumDrive drive;
-
-    enum Mode {
-        DRIVER_MODE,
-        TUNING_MODE
-    }
-
-    private Mode mode;
-
-    private static MotionProfile generateProfile(boolean movingForward) {
-        MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
-        MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
-        return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
-    }
-
-    @Override
-    public void runOpMode() {
-        if (RUN_USING_ENCODER) {
-            RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
-                    "when using the built-in drive motor velocity PID.");
-        }
-
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
-
-        drive = new SampleMecanumDrive(hardwareMap);
-
-        mode = Mode.TUNING_MODE;
-
-        NanoClock clock = NanoClock.system();
-
-        telemetry.addLine("Ready!");
-        telemetry.update();
-        telemetry.clearAll();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        boolean movingForwards = true;
-        MotionProfile activeProfile = generateProfile(true);
-        double profileStart = clock.seconds();
-
-
-        while (!isStopRequested()) {
-            telemetry.addData("mode", mode);
-
-            switch (mode) {
-                case TUNING_MODE:
-                    if (gamepad1.y) {
-                        mode = Mode.DRIVER_MODE;
-                    }
-
-                    // calculate and set the motor power
-                    double profileTime = clock.seconds() - profileStart;
-
-                    if (profileTime > activeProfile.duration()) {
-                        // generate a new profile
-                        movingForwards = !movingForwards;
-                        activeProfile = generateProfile(movingForwards);
-                        profileStart = clock.seconds();
-                    }
-
-                    MotionState motionState = activeProfile.get(profileTime);
-                    double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
-
-                    drive.setDrivePower(new Pose2d(targetPower, 0, 0));
-                    drive.updatePoseEstimate();
-
-                    Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
-                    double currentVelo = poseVelo.getX();
-
-                    // update telemetry
-                    telemetry.addData("targetVelocity", motionState.getV());
-                    telemetry.addData("measuredVelocity", currentVelo);
-                    telemetry.addData("error", motionState.getV() - currentVelo);
-                    break;
-                case DRIVER_MODE:
-                    if (gamepad1.b) {
-                        mode = Mode.TUNING_MODE;
-                        movingForwards = true;
-                        activeProfile = generateProfile(movingForwards);
-                        profileStart = clock.seconds();
-                    }
-
-                    drive.setWeightedDrivePower(
-                            new Pose2d(
-                                    -gamepad1.left_stick_y,
-                                    -gamepad1.left_stick_x,
-                                    -gamepad1.right_stick_x
-                            )
-                    );
-                    break;
-            }
-
-            telemetry.update();
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxAngularVeloTuner.java

@@ -1,73 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-import java.util.Objects;
-
-/**
- * This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
- * <p>
- * Upon pressing start, your bot will turn at max power for RUNTIME seconds.
- * <p>
- * Further fine tuning of MAX_ANG_VEL may be desired.
- */
-
-@Config
-@Autonomous(group = "drive")
-public class MaxAngularVeloTuner extends LinearOpMode {
-    public static double RUNTIME = 4.0;
-
-    private ElapsedTime timer;
-    private double maxAngVelocity = 0.0;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
-        telemetry.addLine("Please ensure you have enough space cleared.");
-        telemetry.addLine("");
-        telemetry.addLine("Press start when ready.");
-        telemetry.update();
-
-        waitForStart();
-
-        telemetry.clearAll();
-        telemetry.update();
-
-        drive.setDrivePower(new Pose2d(0, 0, 1));
-        timer = new ElapsedTime();
-
-        while (!isStopRequested() && timer.seconds() < RUNTIME) {
-            drive.updatePoseEstimate();
-
-            Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
-
-            maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
-        }
-
-        drive.setDrivePower(new Pose2d());
-
-        telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
-        telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
-        telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8);
-        telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8));
-        telemetry.update();
-
-        while (!isStopRequested()) idle();
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxVelocityTuner.java

@@ -1,84 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.VoltageSensor;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.RoadRunner.drive.DriveConstants;
-
-import java.util.Objects;
-
-/**
- * This routine is designed to calculate the maximum velocity your bot can achieve under load. It
- * will also calculate the effective kF value for your velocity PID.
- * <p>
- * Upon pressing start, your bot will run at max power for RUNTIME seconds.
- * <p>
- * Further fine tuning of kF may be desired.
- */
-@Config
-@Autonomous(group = "drive")
-public class MaxVelocityTuner extends LinearOpMode {
-    public static double RUNTIME = 2.0;
-
-    private ElapsedTime timer;
-    private double maxVelocity = 0.0;
-
-    private VoltageSensor batteryVoltageSensor;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
-
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
-        telemetry.addLine("Please ensure you have enough space cleared.");
-        telemetry.addLine("");
-        telemetry.addLine("Press start when ready.");
-        telemetry.update();
-
-        waitForStart();
-
-        telemetry.clearAll();
-        telemetry.update();
-
-        drive.setDrivePower(new Pose2d(1, 0, 0));
-        timer = new ElapsedTime();
-
-        while (!isStopRequested() && timer.seconds() < RUNTIME) {
-            drive.updatePoseEstimate();
-
-            Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
-
-            maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
-        }
-
-        drive.setDrivePower(new Pose2d());
-
-        double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
-
-        telemetry.addData("Max Velocity", maxVelocity);
-        telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8);
-        telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
-        telemetry.update();
-
-        while (!isStopRequested() && opModeIsActive()) idle();
-    }
-
-    private double veloInchesToTicks(double inchesPerSec) {
-        return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/MotorDirectionDebugger.java

@@ -1,93 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-/**
- * This is a simple teleop routine for debugging your motor configuration.
- * Pressing each of the buttons will power its respective motor.
- *
- * Button Mappings:
- *
- * Xbox/PS4 Button - Motor
- *   X / ▢         - Front Left
- *   Y / Δ         - Front Right
- *   B / O         - Rear  Right
- *   A / X         - Rear  Left
- *                                    The buttons are mapped to match the wheels spatially if you
- *                                    were to rotate the gamepad 45deg°. x/square is the front left
- *                    ________        and each button corresponds to the wheel as you go clockwise
- *                   / ______ \
- *     ------------.-'   _  '-..+              Front of Bot
- *              /   _  ( Y )  _  \                  ^
- *             |  ( X )  _  ( B ) |     Front Left   \    Front Right
- *        ___  '.      ( A )     /|       Wheel       \      Wheel
- *      .'    '.    '-._____.-'  .'       (x/▢)        \     (Y/Δ)
- *     |       |                 |                      \
- *      '.___.' '.               |          Rear Left    \   Rear Right
- *               '.             /             Wheel       \    Wheel
- *                \.          .'              (A/X)        \   (B/O)
- *                  \________/
- *
- * Uncomment the @Disabled tag below to use this opmode.
- */
-@Disabled
-@Config
-@TeleOp(group = "drive")
-public class MotorDirectionDebugger extends LinearOpMode {
-    public static double MOTOR_POWER = 0.7;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        telemetry.addLine("Press play to begin the debugging opmode");
-        telemetry.update();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        telemetry.clearAll();
-        telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
-
-        while (!isStopRequested()) {
-            telemetry.addLine("Press each button to turn on its respective motor");
-            telemetry.addLine();
-            telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
-            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
-            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
-            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
-            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
-            telemetry.addLine();
-
-            if(gamepad1.x) {
-                drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
-                telemetry.addLine("Running Motor: Front Left");
-            } else if(gamepad1.y) {
-                drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
-                telemetry.addLine("Running Motor: Front Right");
-            } else if(gamepad1.b) {
-                drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
-                telemetry.addLine("Running Motor: Rear Right");
-            } else if(gamepad1.a) {
-                drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
-                telemetry.addLine("Running Motor: Rear Left");
-            } else {
-                drive.setMotorPowers(0, 0, 0, 0);
-                telemetry.addLine("Running Motor: None");
-            }
-
-            telemetry.update();
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/SplineTest.java

@@ -1,38 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-
-/*
- * This is an example of a more complex path to really test the tuning.
- */
-@Autonomous(group = "drive")
-public class SplineTest extends LinearOpMode {
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        Trajectory traj = drive.trajectoryBuilder(new Pose2d())
-                .splineTo(new Vector2d(30, 30), 0)
-                .build();
-
-        drive.followTrajectory(traj);
-
-        sleep(2000);
-
-        drive.followTrajectory(
-                drive.trajectoryBuilder(traj.end(), true)
-                        .splineTo(new Vector2d(0, 0), Math.toRadians(180))
-                        .build()
-        );
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/StrafeTest.java

@@ -1,46 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-/*
- * This is a simple routine to test translational drive capabilities.
- */
-@Config
-@Autonomous(group = "drive")
-public class StrafeTest extends LinearOpMode {
-    public static double DISTANCE = 60; // in
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
-                .strafeRight(DISTANCE)
-                .build();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        drive.followTrajectory(trajectory);
-
-        Pose2d poseEstimate = drive.getPoseEstimate();
-        telemetry.addData("finalX", poseEstimate.getX());
-        telemetry.addData("finalY", poseEstimate.getY());
-        telemetry.addData("finalHeading", poseEstimate.getHeading());
-        telemetry.update();
-
-        while (!isStopRequested() && opModeIsActive()) ;
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java

@@ -1,48 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-/*
- * This is a simple routine to test translational drive capabilities.
- */
-@Config
-@Autonomous(group = "drive")
-public class StraightTest extends LinearOpMode {
-    public static double DISTANCE = 60; // in
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
-                .forward(DISTANCE)
-                .build();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        drive.followTrajectory(trajectory);
-
-        Pose2d poseEstimate = drive.getPoseEstimate();
-        telemetry.addData("finalX", poseEstimate.getX());
-        telemetry.addData("finalY", poseEstimate.getY());
-        telemetry.addData("finalHeading", poseEstimate.getHeading());
-
-        drive.telemetry(telemetry);
-
-        telemetry.update();
-
-        while (!isStopRequested() && opModeIsActive()) ;
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackWidthTuner.java

@@ -1,88 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.util.Angle;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.util.MovingStatistics;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.robotcore.internal.system.Misc;
-import org.RoadRunner.drive.DriveConstants;
-
-/*
- * This routine determines the effective track width. The procedure works by executing a point turn
- * with a given angle and measuring the difference between that angle and the actual angle (as
- * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
- * given angle / actual angle gives a multiplicative adjustment to the estimated track width
- * (effective track width = estimated track width * given angle / actual angle). The routine repeats
- * this procedure a few times and averages the values for additional accuracy. Note: a relatively
- * accurate track width estimate is important or else the angular constraints will be thrown off.
- */
-@Config
-@Autonomous(group = "drive")
-public class TrackWidthTuner extends LinearOpMode {
-    public static double ANGLE = 180; // deg
-    public static int NUM_TRIALS = 5;
-    public static int DELAY = 1000; // ms
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-        // TODO: if you haven't already, set the localizer to something that doesn't depend on
-        // drive encoders for computing the heading
-
-        telemetry.addLine("Press play to begin the track width tuner routine");
-        telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
-        telemetry.update();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        telemetry.clearAll();
-        telemetry.addLine("Running...");
-        telemetry.update();
-
-        MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
-        for (int i = 0; i < NUM_TRIALS; i++) {
-            drive.setPoseEstimate(new Pose2d());
-
-            // it is important to handle heading wraparounds
-            double headingAccumulator = 0;
-            double lastHeading = 0;
-
-            drive.turnAsync(Math.toRadians(ANGLE));
-
-            while (!isStopRequested() && drive.isBusy()) {
-                double heading = drive.getPoseEstimate().getHeading();
-                headingAccumulator += Angle.normDelta(heading - lastHeading);
-                lastHeading = heading;
-
-                drive.update();
-            }
-
-            double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
-            trackWidthStats.add(trackWidth);
-
-            sleep(DELAY);
-        }
-
-        telemetry.clearAll();
-        telemetry.addLine("Tuning complete");
-        telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
-                trackWidthStats.getMean(),
-                trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
-        telemetry.update();
-
-        while (!isStopRequested()) {
-            idle();
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelForwardOffsetTuner.java

@@ -1,104 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.util.Angle;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.util.MovingStatistics;
-import com.qualcomm.robotcore.util.RobotLog;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.RoadRunner.drive.StandardTrackingWheelLocalizer;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.robotcore.internal.system.Misc;
-
-/**
- * This routine determines the effective forward offset for the lateral tracking wheel.
- * The procedure executes a point turn at a given angle for a certain number of trials,
- * along with a specified delay in milliseconds. The purpose of this is to track the
- * change in the y position during the turn. The offset, or distance, of the lateral tracking
- * wheel from the center or rotation allows the wheel to spin during a point turn, leading
- * to an incorrect measurement for the y position. This creates an arc around around
- * the center of rotation with an arc length of change in y and a radius equal to the forward
- * offset. We can compute this offset by calculating (change in y position) / (change in heading)
- * which returns the radius if the angle (change in heading) is in radians. This is based
- * on the arc length formula of length = theta * radius.
- *
- * To run this routine, simply adjust the desired angle and specify the number of trials
- * and the desired delay. Then, run the procedure. Once it finishes, it will print the
- * average of all the calculated forward offsets derived from the calculation. This calculated
- * forward offset is then added onto the current forward offset to produce an overall estimate
- * for the forward offset. You can run this procedure as many times as necessary until a
- * satisfactory result is produced.
- */
-@Config
-@Autonomous(group="drive")
-public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
-    public static double ANGLE = 180; // deg
-    public static int NUM_TRIALS = 5;
-    public static int DELAY = 1000; // ms
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
-            RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
-                    + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
-                    + "(hardwareMap));\" is called in SampleMecanumDrive.java");
-        }
-
-        telemetry.addLine("Press play to begin the forward offset tuner");
-        telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
-        telemetry.update();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        telemetry.clearAll();
-        telemetry.addLine("Running...");
-        telemetry.update();
-
-        MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
-        for (int i = 0; i < NUM_TRIALS; i++) {
-            drive.setPoseEstimate(new Pose2d());
-
-            // it is important to handle heading wraparounds
-            double headingAccumulator = 0;
-            double lastHeading = 0;
-
-            drive.turnAsync(Math.toRadians(ANGLE));
-
-            while (!isStopRequested() && drive.isBusy()) {
-                double heading = drive.getPoseEstimate().getHeading();
-                headingAccumulator += Angle.norm(heading - lastHeading);
-                lastHeading = heading;
-
-                drive.update();
-            }
-
-            double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
-                    drive.getPoseEstimate().getY() / headingAccumulator;
-            forwardOffsetStats.add(forwardOffset);
-
-            sleep(DELAY);
-        }
-
-        telemetry.clearAll();
-        telemetry.addLine("Tuning complete");
-        telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
-                forwardOffsetStats.getMean(),
-                forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
-        telemetry.update();
-
-        while (!isStopRequested()) {
-            idle();
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelLateralDistanceTuner.java

@@ -1,130 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.util.Angle;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.util.RobotLog;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-import org.RoadRunner.drive.StandardTrackingWheelLocalizer;
-
-/**
- * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
- * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
- * wheels.
- *
- * Tuning Routine:
- *
- * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
- * measured value. This need only be an estimated value as you will be tuning it anyways.
- *
- * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
- * similar mark right below the indicator on your bot. This will be your reference point to
- * ensure you've turned exactly 360°.
- *
- * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
- * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
- * connect your computer to the RC's WiFi network. In your browser, navigate to
- * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
- * if you are using the Control Hub.
- * Ensure the field is showing (select the field view in top right of the page).
- *
- * 4. Press play to begin the tuning routine.
- *
- * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
- *
- * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
- *
- * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
- * on the bot and on the ground you created earlier should be lined up.
- *
- * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
- * StandardTrackingWheelLocalizer.java class.
- *
- * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
- * yourself. Read the heading output and follow the advice stated in the note below to manually
- * nudge the values yourself.
- *
- * Note:
- * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
- * a line from the circumference to the center should be present, representing the bot. The line
- * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
- * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
- * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
- * actual bot, the LATERAL_DISTANCE should be increased.
- *
- * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
- * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
- * effective center of rotation. You can ignore this effect. The center of rotation will be offset
- * slightly but your heading will still be fine. This does not affect your overall tracking
- * precision. The heading should still line up.
- */
-@Config
-@TeleOp(group = "drive")
-public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
-    public static int NUM_TURNS = 10;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
-            RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
-                    + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
-                    + "(hardwareMap));\" is called in SampleMecanumDrive.java");
-        }
-
-        telemetry.addLine("Prior to beginning the routine, please read the directions "
-                + "located in the comments of the opmode file.");
-        telemetry.addLine("Press play to begin the tuning routine.");
-        telemetry.addLine("");
-        telemetry.addLine("Press Y/△ to stop the routine.");
-        telemetry.update();
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        telemetry.clearAll();
-        telemetry.update();
-
-        double headingAccumulator = 0;
-        double lastHeading = 0;
-
-        boolean tuningFinished = false;
-
-        while (!isStopRequested() && !tuningFinished) {
-            Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
-            drive.setDrivePower(vel);
-
-            drive.update();
-
-            double heading = drive.getPoseEstimate().getHeading();
-            double deltaHeading = heading - lastHeading;
-
-            headingAccumulator += Angle.normDelta(deltaHeading);
-            lastHeading = heading;
-
-            telemetry.clearAll();
-            telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
-            telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
-            telemetry.addLine();
-            telemetry.addLine("Press Y/△ to conclude routine");
-            telemetry.update();
-
-            if (gamepad1.y)
-                tuningFinished = true;
-        }
-
-        telemetry.clearAll();
-        telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
-        telemetry.addLine("Effective LATERAL_DISTANCE: " +
-                (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
-
-        telemetry.update();
-
-        while (!isStopRequested()) idle();
-    }
-}

TeamCode/src/main/java/org/RoadRunner/drive/opmode/TurnTest.java

@@ -1,28 +0,0 @@
-package org.RoadRunner.drive.opmode;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.RoadRunner.drive.SampleMecanumDrive;
-
-
-/*
- * This is a simple routine to test turning capabilities.
- */
-@Config
-@Autonomous(group = "drive")
-public class TurnTest extends LinearOpMode {
-    public static double ANGLE = 90; // deg
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        drive.turn(Math.toRadians(ANGLE));
-    }
-}

TeamCode/src/main/java/org/RoadRunner/trajectorysequence/EmptySequenceException.java

@@ -1,4 +0,0 @@
-package org.RoadRunner.trajectorysequence;
-
-
-public class EmptySequenceException extends RuntimeException { }

TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequence.java

@@ -1,44 +0,0 @@
-package org.RoadRunner.trajectorysequence;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-
-import org.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
-
-import java.util.Collections;
-import java.util.List;
-
-public class TrajectorySequence {
-    private final List<SequenceSegment> sequenceList;
-
-    public TrajectorySequence(List<SequenceSegment> sequenceList) {
-        if (sequenceList.size() == 0) throw new EmptySequenceException();
-
-        this.sequenceList = Collections.unmodifiableList(sequenceList);
-    }
-
-    public Pose2d start() {
-        return sequenceList.get(0).getStartPose();
-    }
-
-    public Pose2d end() {
-        return sequenceList.get(sequenceList.size() - 1).getEndPose();
-    }
-
-    public double duration() {
-        double total = 0.0;
-
-        for (SequenceSegment segment : sequenceList) {
-            total += segment.getDuration();
-        }
-
-        return total;
-    }
-
-    public SequenceSegment get(int i) {
-        return sequenceList.get(i);
-    }
-
-    public int size() {
-        return sequenceList.size();
-    }
-}

TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java

@@ -1,711 +0,0 @@
-package org.RoadRunner.trajectorysequence;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
-import com.acmerobotics.roadrunner.path.PathContinuityViolationException;
-import com.acmerobotics.roadrunner.profile.MotionProfile;
-import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
-import com.acmerobotics.roadrunner.profile.MotionState;
-import com.acmerobotics.roadrunner.trajectory.DisplacementMarker;
-import com.acmerobotics.roadrunner.trajectory.DisplacementProducer;
-import com.acmerobotics.roadrunner.trajectory.MarkerCallback;
-import com.acmerobotics.roadrunner.trajectory.SpatialMarker;
-import com.acmerobotics.roadrunner.trajectory.TemporalMarker;
-import com.acmerobotics.roadrunner.trajectory.TimeProducer;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
-import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
-import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
-import com.acmerobotics.roadrunner.util.Angle;
-
-import org.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
-import org.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment;
-import org.RoadRunner.trajectorysequence.sequencesegment.TurnSegment;
-import org.RoadRunner.trajectorysequence.sequencesegment.WaitSegment;
-
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.List;
-
-public class TrajectorySequenceBuilder {
-    private final double resolution = 0.25;
-
-    private final TrajectoryVelocityConstraint baseVelConstraint;
-    private final TrajectoryAccelerationConstraint baseAccelConstraint;
-
-    private TrajectoryVelocityConstraint currentVelConstraint;
-    private TrajectoryAccelerationConstraint currentAccelConstraint;
-
-    private final double baseTurnConstraintMaxAngVel;
-    private final double baseTurnConstraintMaxAngAccel;
-
-    private double currentTurnConstraintMaxAngVel;
-    private double currentTurnConstraintMaxAngAccel;
-
-    private final List<SequenceSegment> sequenceSegments;
-
-    private final List<TemporalMarker> temporalMarkers;
-    private final List<DisplacementMarker> displacementMarkers;
-    private final List<SpatialMarker> spatialMarkers;
-
-    private Pose2d lastPose;
-
-    private double tangentOffset;
-
-    private boolean setAbsoluteTangent;
-    private double absoluteTangent;
-
-    private TrajectoryBuilder currentTrajectoryBuilder;
-
-    private double currentDuration;
-    private double currentDisplacement;
-
-    private double lastDurationTraj;
-    private double lastDisplacementTraj;
-
-    public TrajectorySequenceBuilder(
-            Pose2d startPose,
-            Double startTangent,
-            TrajectoryVelocityConstraint baseVelConstraint,
-            TrajectoryAccelerationConstraint baseAccelConstraint,
-            double baseTurnConstraintMaxAngVel,
-            double baseTurnConstraintMaxAngAccel
-    ) {
-        this.baseVelConstraint = baseVelConstraint;
-        this.baseAccelConstraint = baseAccelConstraint;
-
-        this.currentVelConstraint = baseVelConstraint;
-        this.currentAccelConstraint = baseAccelConstraint;
-
-        this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
-        this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
-
-        this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
-        this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
-
-        sequenceSegments = new ArrayList<>();
-
-        temporalMarkers = new ArrayList<>();
-        displacementMarkers = new ArrayList<>();
-        spatialMarkers = new ArrayList<>();
-
-        lastPose = startPose;
-
-        tangentOffset = 0.0;
-
-        setAbsoluteTangent = (startTangent != null);
-        absoluteTangent = startTangent != null ? startTangent : 0.0;
-
-        currentTrajectoryBuilder = null;
-
-        currentDuration = 0.0;
-        currentDisplacement = 0.0;
-
-        lastDurationTraj = 0.0;
-        lastDisplacementTraj = 0.0;
-    }
-
-    public TrajectorySequenceBuilder(
-            Pose2d startPose,
-            TrajectoryVelocityConstraint baseVelConstraint,
-            TrajectoryAccelerationConstraint baseAccelConstraint,
-            double baseTurnConstraintMaxAngVel,
-            double baseTurnConstraintMaxAngAccel
-    ) {
-        this(
-                startPose, null,
-                baseVelConstraint, baseAccelConstraint,
-                baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
-        );
-    }
-
-    public TrajectorySequenceBuilder lineTo(Vector2d endPosition) {
-        return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder lineTo(
-            Vector2d endPosition,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) {
-        return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder lineToConstantHeading(
-            Vector2d endPosition,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) {
-        return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder lineToLinearHeading(
-            Pose2d endPose,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) {
-        return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder lineToSplineHeading(
-            Pose2d endPose,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) {
-        return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder strafeTo(
-            Vector2d endPosition,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder forward(double distance) {
-        return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder forward(
-            double distance,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder back(double distance) {
-        return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder back(
-            double distance,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder strafeLeft(double distance) {
-        return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder strafeLeft(
-            double distance,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder strafeRight(double distance) {
-        return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder strafeRight(
-            double distance,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) {
-        return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder splineTo(
-            Vector2d endPosition,
-            double endHeading,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) {
-        return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder splineToConstantHeading(
-            Vector2d endPosition,
-            double endHeading,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) {
-        return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder splineToLinearHeading(
-            Pose2d endPose,
-            double endHeading,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint));
-    }
-
-    public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) {
-        return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
-    }
-
-    public TrajectorySequenceBuilder splineToSplineHeading(
-            Pose2d endPose,
-            double endHeading,
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint));
-    }
-
-    private TrajectorySequenceBuilder addPath(AddPathCallback callback) {
-        if (currentTrajectoryBuilder == null) newPath();
-
-        try {
-            callback.run();
-        } catch (PathContinuityViolationException e) {
-            newPath();
-            callback.run();
-        }
-
-        Trajectory builtTraj = currentTrajectoryBuilder.build();
-
-        double durationDifference = builtTraj.duration() - lastDurationTraj;
-        double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj;
-
-        lastPose = builtTraj.end();
-        currentDuration += durationDifference;
-        currentDisplacement += displacementDifference;
-
-        lastDurationTraj = builtTraj.duration();
-        lastDisplacementTraj = builtTraj.getPath().length();
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder setTangent(double tangent) {
-        setAbsoluteTangent = true;
-        absoluteTangent = tangent;
-
-        pushPath();
-
-        return this;
-    }
-
-    private TrajectorySequenceBuilder setTangentOffset(double offset) {
-        setAbsoluteTangent = false;
-
-        this.tangentOffset = offset;
-        this.pushPath();
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder setReversed(boolean reversed) {
-        return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0);
-    }
-
-    public TrajectorySequenceBuilder setConstraints(
-            TrajectoryVelocityConstraint velConstraint,
-            TrajectoryAccelerationConstraint accelConstraint
-    ) {
-        this.currentVelConstraint = velConstraint;
-        this.currentAccelConstraint = accelConstraint;
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder resetConstraints() {
-        this.currentVelConstraint = this.baseVelConstraint;
-        this.currentAccelConstraint = this.baseAccelConstraint;
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) {
-        this.currentVelConstraint = velConstraint;
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder resetVelConstraint() {
-        this.currentVelConstraint = this.baseVelConstraint;
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) {
-        this.currentAccelConstraint = accelConstraint;
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder resetAccelConstraint() {
-        this.currentAccelConstraint = this.baseAccelConstraint;
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) {
-        this.currentTurnConstraintMaxAngVel = maxAngVel;
-        this.currentTurnConstraintMaxAngAccel = maxAngAccel;
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder resetTurnConstraint() {
-        this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
-        this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) {
-        return this.addTemporalMarker(currentDuration, callback);
-    }
-
-    public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) {
-        return this.addTemporalMarker(currentDuration + offset, callback);
-    }
-
-    public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) {
-        return this.addTemporalMarker(0.0, time, callback);
-    }
-
-    public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) {
-        return this.addTemporalMarker(time -> scale * time + offset, callback);
-    }
-
-    public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) {
-        this.temporalMarkers.add(new TemporalMarker(time, callback));
-        return this;
-    }
-
-    public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) {
-        this.spatialMarkers.add(new SpatialMarker(point, callback));
-        return this;
-    }
-
-    public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) {
-        return this.addDisplacementMarker(currentDisplacement, callback);
-    }
-
-    public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) {
-        return this.addDisplacementMarker(currentDisplacement + offset, callback);
-    }
-
-    public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) {
-        return this.addDisplacementMarker(0.0, displacement, callback);
-    }
-
-    public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) {
-        return addDisplacementMarker((displacement -> scale * displacement + offset), callback);
-    }
-
-    public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) {
-        displacementMarkers.add(new DisplacementMarker(displacement, callback));
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder turn(double angle) {
-        return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
-    }
-
-    public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) {
-        pushPath();
-
-        MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile(
-                new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0),
-                new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0),
-                maxAngVel,
-                maxAngAccel
-        );
-
-        sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList()));
-
-        lastPose = new Pose2d(
-                lastPose.getX(), lastPose.getY(),
-                Angle.norm(lastPose.getHeading() + angle)
-        );
-
-        currentDuration += turnProfile.duration();
-
-        return this;
-    }
-
-    public TrajectorySequenceBuilder waitSeconds(double seconds) {
-        pushPath();
-        sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
-
-        currentDuration += seconds;
-        return this;
-    }
-
-    public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) {
-        pushPath();
-
-        sequenceSegments.add(new TrajectorySegment(trajectory));
-        return this;
-    }
-
-    private void pushPath() {
-        if (currentTrajectoryBuilder != null) {
-            Trajectory builtTraj = currentTrajectoryBuilder.build();
-            sequenceSegments.add(new TrajectorySegment(builtTraj));
-        }
-
-        currentTrajectoryBuilder = null;
-    }
-
-    private void newPath() {
-        if (currentTrajectoryBuilder != null)
-            pushPath();
-
-        lastDurationTraj = 0.0;
-        lastDisplacementTraj = 0.0;
-
-        double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
-
-        currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
-    }
-
-    public TrajectorySequence build() {
-        pushPath();
-
-        List<TrajectoryMarker> globalMarkers = convertMarkersToGlobal(
-                sequenceSegments,
-                temporalMarkers, displacementMarkers, spatialMarkers
-        );
-
-        return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments));
-    }
-
-    private List<TrajectoryMarker> convertMarkersToGlobal(
-            List<SequenceSegment> sequenceSegments,
-            List<TemporalMarker> temporalMarkers,
-            List<DisplacementMarker> displacementMarkers,
-            List<SpatialMarker> spatialMarkers
-    ) {
-        ArrayList<TrajectoryMarker> trajectoryMarkers = new ArrayList<>();
-
-        // Convert temporal markers
-        for (TemporalMarker marker : temporalMarkers) {
-            trajectoryMarkers.add(
-                    new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback())
-            );
-        }
-
-        // Convert displacement markers
-        for (DisplacementMarker marker : displacementMarkers) {
-            double time = displacementToTime(
-                    sequenceSegments,
-                    marker.getProducer().produce(currentDisplacement)
-            );
-
-            trajectoryMarkers.add(
-                    new TrajectoryMarker(
-                            time,
-                            marker.getCallback()
-                    )
-            );
-        }
-
-        // Convert spatial markers
-        for (SpatialMarker marker : spatialMarkers) {
-            trajectoryMarkers.add(
-                    new TrajectoryMarker(
-                            pointToTime(sequenceSegments, marker.getPoint()),
-                            marker.getCallback()
-                    )
-            );
-        }
-
-        return trajectoryMarkers;
-    }
-
-    private List<SequenceSegment> projectGlobalMarkersToLocalSegments(List<TrajectoryMarker> markers, List<SequenceSegment> sequenceSegments) {
-        if (sequenceSegments.isEmpty()) return Collections.emptyList();
-
-        double totalSequenceDuration = 0;
-        for (SequenceSegment segment : sequenceSegments) {
-            totalSequenceDuration += segment.getDuration();
-        }
-
-        for (TrajectoryMarker marker : markers) {
-            SequenceSegment segment = null;
-            int segmentIndex = 0;
-            double segmentOffsetTime = 0;
-
-            double currentTime = 0;
-            for (int i = 0; i < sequenceSegments.size(); i++) {
-                SequenceSegment seg = sequenceSegments.get(i);
-
-                double markerTime = Math.min(marker.getTime(), totalSequenceDuration);
-
-                if (currentTime + seg.getDuration() >= markerTime) {
-                    segment = seg;
-                    segmentIndex = i;
-                    segmentOffsetTime = markerTime - currentTime;
-
-                    break;
-                } else {
-                    currentTime += seg.getDuration();
-                }
-            }
-
-            SequenceSegment newSegment = null;
-
-            if (segment instanceof WaitSegment) {
-                List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
-
-                newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
-                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
-
-                WaitSegment thisSegment = (WaitSegment) segment;
-                newSegment = new WaitSegment(thisSegment.getStartPose(), thisSegment.getDuration(), newMarkers);
-            } else if (segment instanceof TurnSegment) {
-                List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
-
-                newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
-                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
-
-                TurnSegment thisSegment = (TurnSegment) segment;
-                newSegment = new TurnSegment(thisSegment.getStartPose(), thisSegment.getTotalRotation(), thisSegment.getMotionProfile(), newMarkers);
-            } else if (segment instanceof TrajectorySegment) {
-                TrajectorySegment thisSegment = (TrajectorySegment) segment;
-
-                List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers());
-                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
-
-                newSegment = new TrajectorySegment(new Trajectory(thisSegment.getTrajectory().getPath(), thisSegment.getTrajectory().getProfile(), newMarkers));
-            }
-
-            sequenceSegments.set(segmentIndex, newSegment);
-        }
-
-        return sequenceSegments;
-    }
-
-    // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
-    // note: this assumes that the profile position is monotonic increasing
-    private Double motionProfileDisplacementToTime(MotionProfile profile, double s) {
-        double tLo = 0.0;
-        double tHi = profile.duration();
-        while (!(Math.abs(tLo - tHi) < 1e-6)) {
-            double tMid = 0.5 * (tLo + tHi);
-            if (profile.get(tMid).getX() > s) {
-                tHi = tMid;
-            } else {
-                tLo = tMid;
-            }
-        }
-        return 0.5 * (tLo + tHi);
-    }
-
-    private Double displacementToTime(List<SequenceSegment> sequenceSegments, double s) {
-        double currentTime = 0.0;
-        double currentDisplacement = 0.0;
-
-        for (SequenceSegment segment : sequenceSegments) {
-            if (segment instanceof TrajectorySegment) {
-                TrajectorySegment thisSegment = (TrajectorySegment) segment;
-
-                double segmentLength = thisSegment.getTrajectory().getPath().length();
-
-                if (currentDisplacement + segmentLength > s) {
-                    double target = s - currentDisplacement;
-                    double timeInSegment = motionProfileDisplacementToTime(
-                            thisSegment.getTrajectory().getProfile(),
-                            target
-                    );
-
-                    return currentTime + timeInSegment;
-                } else {
-                    currentDisplacement += segmentLength;
-                    currentTime += thisSegment.getTrajectory().duration();
-                }
-            } else {
-                currentTime += segment.getDuration();
-            }
-        }
-
-        return 0.0;
-    }
-
-    private Double pointToTime(List<SequenceSegment> sequenceSegments, Vector2d point) {
-        class ComparingPoints {
-            private final double distanceToPoint;
-            private final double totalDisplacement;
-            private final double thisPathDisplacement;
-
-            public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) {
-                this.distanceToPoint = distanceToPoint;
-                this.totalDisplacement = totalDisplacement;
-                this.thisPathDisplacement = thisPathDisplacement;
-            }
-        }
-
-        List<ComparingPoints> projectedPoints = new ArrayList<>();
-
-        for (SequenceSegment segment : sequenceSegments) {
-            if (segment instanceof TrajectorySegment) {
-                TrajectorySegment thisSegment = (TrajectorySegment) segment;
-
-                double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25);
-                Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec();
-                double distanceToPoint = point.minus(projectedPoint).norm();
-
-                double totalDisplacement = 0.0;
-
-                for (ComparingPoints comparingPoint : projectedPoints) {
-                    totalDisplacement += comparingPoint.totalDisplacement;
-                }
-
-                totalDisplacement += displacement;
-
-                projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement));
-            }
-        }
-
-        ComparingPoints closestPoint = null;
-
-        for (ComparingPoints comparingPoint : projectedPoints) {
-            if (closestPoint == null) {
-                closestPoint = comparingPoint;
-                continue;
-            }
-
-            if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint)
-                closestPoint = comparingPoint;
-        }
-
-        return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement);
-    }
-
-    private interface AddPathCallback {
-        void run();
-    }
-}

TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java

@@ -1,276 +0,0 @@
-package org.RoadRunner.trajectorysequence;
-
-import androidx.annotation.Nullable;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.control.PIDCoefficients;
-import com.acmerobotics.roadrunner.control.PIDFController;
-import com.acmerobotics.roadrunner.drive.DriveSignal;
-import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.profile.MotionState;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
-import com.acmerobotics.roadrunner.util.NanoClock;
-
-import org.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
-import org.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment;
-import org.RoadRunner.trajectorysequence.sequencesegment.TurnSegment;
-import org.RoadRunner.trajectorysequence.sequencesegment.WaitSegment;
-import org.RoadRunner.util.DashboardUtil;
-
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.LinkedList;
-import java.util.List;
-
-@Config
-public class TrajectorySequenceRunner {
-    public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
-    public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
-    public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
-
-    public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
-    public static String COLOR_ACTIVE_TURN = "#7c4dff";
-    public static String COLOR_ACTIVE_WAIT = "#dd2c00";
-
-    public static int POSE_HISTORY_LIMIT = 100;
-
-    private final TrajectoryFollower follower;
-
-    private final PIDFController turnController;
-
-    private final NanoClock clock;
-
-    private TrajectorySequence currentTrajectorySequence;
-    private double currentSegmentStartTime;
-    private int currentSegmentIndex;
-    private int lastSegmentIndex;
-
-    private Pose2d lastPoseError = new Pose2d();
-
-    List<TrajectoryMarker> remainingMarkers = new ArrayList<>();
-
-    private final FtcDashboard dashboard;
-    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
-
-    public TrajectorySequenceRunner(TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients) {
-        this.follower = follower;
-
-        turnController = new PIDFController(headingPIDCoefficients);
-        turnController.setInputBounds(0, 2 * Math.PI);
-
-        clock = NanoClock.system();
-
-        dashboard = FtcDashboard.getInstance();
-        dashboard.setTelemetryTransmissionInterval(25);
-    }
-
-//    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
-//        currentTrajectorySequence = trajectorySequence;
-//        currentSegmentStartTime = clock.seconds();
-//        currentSegmentIndex = 0;
-//        lastSegmentIndex = -1;
-//    }
-
-    public @Nullable
-    DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
-        Pose2d targetPose = null;
-        DriveSignal driveSignal = null;
-
-        TelemetryPacket packet = new TelemetryPacket();
-        Canvas fieldOverlay = packet.fieldOverlay();
-
-        SequenceSegment currentSegment = null;
-
-        if (currentTrajectorySequence != null) {
-            if (currentSegmentIndex >= currentTrajectorySequence.size()) {
-                for (TrajectoryMarker marker : remainingMarkers) {
-                    marker.getCallback().onMarkerReached();
-                }
-
-                remainingMarkers.clear();
-
-                currentTrajectorySequence = null;
-            }
-
-            if (currentTrajectorySequence == null)
-                return new DriveSignal();
-
-            double now = clock.seconds();
-            boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
-
-            currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
-
-            if (isNewTransition) {
-                currentSegmentStartTime = now;
-                lastSegmentIndex = currentSegmentIndex;
-
-                for (TrajectoryMarker marker : remainingMarkers) {
-                    marker.getCallback().onMarkerReached();
-                }
-
-                remainingMarkers.clear();
-
-                remainingMarkers.addAll(currentSegment.getMarkers());
-                Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
-            }
-
-            double deltaTime = now - currentSegmentStartTime;
-
-            if (currentSegment instanceof TrajectorySegment) {
-                Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
-
-                if (isNewTransition)
-                    follower.followTrajectory(currentTrajectory);
-
-                if (!follower.isFollowing()) {
-                    currentSegmentIndex++;
-
-                    driveSignal = new DriveSignal();
-                } else {
-                    driveSignal = follower.update(poseEstimate, poseVelocity);
-                    lastPoseError = follower.getLastError();
-                }
-
-                targetPose = currentTrajectory.get(deltaTime);
-            } else if (currentSegment instanceof TurnSegment) {
-                MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
-
-                turnController.setTargetPosition(targetState.getX());
-
-                double correction = turnController.update(poseEstimate.getHeading());
-
-                double targetOmega = targetState.getV();
-                double targetAlpha = targetState.getA();
-
-                lastPoseError = new Pose2d(0, 0, turnController.getLastError());
-
-                Pose2d startPose = currentSegment.getStartPose();
-                targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
-
-                driveSignal = new DriveSignal(
-                        new Pose2d(0, 0, targetOmega + correction),
-                        new Pose2d(0, 0, targetAlpha)
-                );
-
-                if (deltaTime >= currentSegment.getDuration()) {
-                    currentSegmentIndex++;
-                    driveSignal = new DriveSignal();
-                }
-            } else if (currentSegment instanceof WaitSegment) {
-                lastPoseError = new Pose2d();
-
-                targetPose = currentSegment.getStartPose();
-                driveSignal = new DriveSignal();
-
-                if (deltaTime >= currentSegment.getDuration()) {
-                    currentSegmentIndex++;
-                }
-            }
-
-            while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
-                remainingMarkers.get(0).getCallback().onMarkerReached();
-                remainingMarkers.remove(0);
-            }
-        }
-
-        poseHistory.add(poseEstimate);
-
-        if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
-            poseHistory.removeFirst();
-        }
-
-        packet.put("x", poseEstimate.getX());
-        packet.put("y", poseEstimate.getY());
-        packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading()));
-
-        packet.put("xError", getLastPoseError().getX());
-        packet.put("yError", getLastPoseError().getY());
-        packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading()));
-
-        draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate);
-
-        dashboard.sendTelemetryPacket(packet);
-
-        return driveSignal;
-    }
-
-    private void draw(
-            Canvas fieldOverlay,
-            TrajectorySequence sequence, SequenceSegment currentSegment,
-            Pose2d targetPose, Pose2d poseEstimate
-    ) {
-        if (sequence != null) {
-            for (int i = 0; i < sequence.size(); i++) {
-                SequenceSegment segment = sequence.get(i);
-
-                if (segment instanceof TrajectorySegment) {
-                    fieldOverlay.setStrokeWidth(1);
-                    fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY);
-
-                    DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath());
-                } else if (segment instanceof TurnSegment) {
-                    Pose2d pose = segment.getStartPose();
-
-                    fieldOverlay.setFill(COLOR_INACTIVE_TURN);
-                    fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2);
-                } else if (segment instanceof WaitSegment) {
-                    Pose2d pose = segment.getStartPose();
-
-                    fieldOverlay.setStrokeWidth(1);
-                    fieldOverlay.setStroke(COLOR_INACTIVE_WAIT);
-                    fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
-                }
-            }
-        }
-
-        if (currentSegment != null) {
-            if (currentSegment instanceof TrajectorySegment) {
-                Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
-
-                fieldOverlay.setStrokeWidth(1);
-                fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY);
-
-                DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath());
-            } else if (currentSegment instanceof TurnSegment) {
-                Pose2d pose = currentSegment.getStartPose();
-
-                fieldOverlay.setFill(COLOR_ACTIVE_TURN);
-                fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3);
-            } else if (currentSegment instanceof WaitSegment) {
-                Pose2d pose = currentSegment.getStartPose();
-
-                fieldOverlay.setStrokeWidth(1);
-                fieldOverlay.setStroke(COLOR_ACTIVE_WAIT);
-                fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
-            }
-        }
-
-        if (targetPose != null) {
-            fieldOverlay.setStrokeWidth(1);
-            fieldOverlay.setStroke("#4CAF50");
-            DashboardUtil.drawRobot(fieldOverlay, targetPose);
-        }
-
-        fieldOverlay.setStroke("#3F51B5");
-        DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory);
-
-        fieldOverlay.setStroke("#3F51B5");
-        DashboardUtil.drawRobot(fieldOverlay, poseEstimate);
-    }
-
-    public Pose2d getLastPoseError() {
-        return lastPoseError;
-    }
-
-    public boolean isBusy() {
-        return currentTrajectorySequence != null;
-    }
-
-    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
-    }
-}

TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java

@@ -1,40 +0,0 @@
-package org.RoadRunner.trajectorysequence.sequencesegment;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
-
-import java.util.List;
-
-public abstract class SequenceSegment {
-    private final double duration;
-    private final Pose2d startPose;
-    private final Pose2d endPose;
-    private final List<TrajectoryMarker> markers;
-
-    protected SequenceSegment(
-            double duration,
-            Pose2d startPose, Pose2d endPose,
-            List<TrajectoryMarker> markers
-    ) {
-        this.duration = duration;
-        this.startPose = startPose;
-        this.endPose = endPose;
-        this.markers = markers;
-    }
-
-    public double getDuration() {
-        return this.duration;
-    }
-
-    public Pose2d getStartPose() {
-        return startPose;
-    }
-
-    public Pose2d getEndPose() {
-        return endPose;
-    }
-
-    public List<TrajectoryMarker> getMarkers() {
-        return markers;
-    }
-}

TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java

@@ -1,20 +0,0 @@
-package org.RoadRunner.trajectorysequence.sequencesegment;
-
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-
-import java.util.Collections;
-
-public final class TrajectorySegment extends SequenceSegment {
-    private final Trajectory trajectory;
-
-    public TrajectorySegment(Trajectory trajectory) {
-        // Note: Markers are already stored in the `Trajectory` itself.
-        // This class should not hold any markers
-        super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
-        this.trajectory = trajectory;
-    }
-
-    public Trajectory getTrajectory() {
-        return this.trajectory;
-    }
-}

TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java

@@ -1,36 +0,0 @@
-package org.RoadRunner.trajectorysequence.sequencesegment;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.profile.MotionProfile;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
-import com.acmerobotics.roadrunner.util.Angle;
-
-import java.util.List;
-
-public final class TurnSegment extends SequenceSegment {
-    private final double totalRotation;
-    private final MotionProfile motionProfile;
-
-    public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List<TrajectoryMarker> markers) {
-        super(
-                motionProfile.duration(),
-                startPose,
-                new Pose2d(
-                        startPose.getX(), startPose.getY(),
-                        Angle.norm(startPose.getHeading() + totalRotation)
-                ),
-                markers
-        );
-
-        this.totalRotation = totalRotation;
-        this.motionProfile = motionProfile;
-    }
-
-    public final double getTotalRotation() {
-        return this.totalRotation;
-    }
-
-    public final MotionProfile getMotionProfile() {
-        return this.motionProfile;
-    }
-}

TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java

@@ -1,12 +0,0 @@
-package org.RoadRunner.trajectorysequence.sequencesegment;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
-
-import java.util.List;
-
-public final class WaitSegment extends SequenceSegment {
-    public WaitSegment(Pose2d pose, double seconds, List<TrajectoryMarker> markers) {
-        super(seconds, pose, pose, markers);
-    }
-}

TeamCode/src/main/java/org/RoadRunner/util/AssetsTrajectoryManager.java

@@ -1,70 +0,0 @@
-package org.RoadRunner.util;
-
-import androidx.annotation.Nullable;
-
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
-import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
-import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
-import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
-
-import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
-
-import java.io.IOException;
-import java.io.InputStream;
-
-/**
- * Set of utilities for loading trajectories from assets (the plugin save location).
- */
-public class AssetsTrajectoryManager {
-
-    /**
-     * Loads the group config.
-     */
-    public static @Nullable
-    TrajectoryGroupConfig loadGroupConfig() {
-        try {
-            InputStream inputStream = AppUtil.getDefContext().getAssets().open(
-                    "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
-            return TrajectoryConfigManager.loadGroupConfig(inputStream);
-        } catch (IOException e) {
-            return null;
-        }
-    }
-
-    /**
-     * Loads a trajectory config with the given name.
-     */
-    public static @Nullable TrajectoryConfig loadConfig(String name) {
-        try {
-            InputStream inputStream = AppUtil.getDefContext().getAssets().open(
-                    "trajectory/" + name + ".yaml");
-            return TrajectoryConfigManager.loadConfig(inputStream);
-        } catch (IOException e) {
-            return null;
-        }
-    }
-
-    /**
-     * Loads a trajectory builder with the given name.
-     */
-    public static @Nullable TrajectoryBuilder loadBuilder(String name) {
-        TrajectoryGroupConfig groupConfig = loadGroupConfig();
-        TrajectoryConfig config = loadConfig(name);
-        if (groupConfig == null || config == null) {
-            return null;
-        }
-        return config.toTrajectoryBuilder(groupConfig);
-    }
-
-    /**
-     * Loads a trajectory with the given name.
-     */
-    public static @Nullable Trajectory load(String name) {
-        TrajectoryBuilder builder = loadBuilder(name);
-        if (builder == null) {
-            return null;
-        }
-        return builder.build();
-    }
-}

TeamCode/src/main/java/org/RoadRunner/util/AxesSigns.java

@@ -1,45 +0,0 @@
-package org.RoadRunner.util;
-
-/**
- * IMU axes signs in the order XYZ (after remapping).
- */
-public enum AxesSigns {
-    PPP(0b000),
-    PPN(0b001),
-    PNP(0b010),
-    PNN(0b011),
-    NPP(0b100),
-    NPN(0b101),
-    NNP(0b110),
-    NNN(0b111);
-
-    public final int bVal;
-
-    AxesSigns(int bVal) {
-        this.bVal = bVal;
-    }
-
-    public static AxesSigns fromBinaryValue(int bVal) {
-        int maskedVal = bVal & 0x07;
-        switch (maskedVal) {
-            case 0b000:
-                return AxesSigns.PPP;
-            case 0b001:
-                return AxesSigns.PPN;
-            case 0b010:
-                return AxesSigns.PNP;
-            case 0b011:
-                return AxesSigns.PNN;
-            case 0b100:
-                return AxesSigns.NPP;
-            case 0b101:
-                return AxesSigns.NPN;
-            case 0b110:
-                return AxesSigns.NNP;
-            case 0b111:
-                return AxesSigns.NNN;
-            default:
-                throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal);
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/util/AxisDirection.java

@@ -1,8 +0,0 @@
-package org.RoadRunner.util;
-
-/**
- * A direction for an axis to be remapped to
- */
-public enum AxisDirection {
-    POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
-}

TeamCode/src/main/java/org/RoadRunner/util/BNO055IMUUtil.java

@@ -1,128 +0,0 @@
-package org.RoadRunner.util;
-
-import com.qualcomm.hardware.bosch.BNO055IMU;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
-
-/**
- * Various utility functions for the BNO055 IMU.
- */
-public class BNO055IMUUtil {
-    /**
-     * Error for attempting an illegal remapping (lhs or multiple same axes)
-     */
-    public static class InvalidAxisRemapException extends RuntimeException {
-        public InvalidAxisRemapException(String detailMessage) {
-            super(detailMessage);
-        }
-    }
-
-    /**
-     * Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
-     * Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
-     * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
-     *
-     * Adapted from <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
-     * Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> for details.
-     *
-     * NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if appropriate.
-     *
-     * @param imu IMU
-     * @param order axes order
-     * @param signs axes signs
-     */
-    public static void swapThenFlipAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
-        try {
-            // the indices correspond with the 2-bit axis encodings specified in the datasheet
-            int[] indices = order.indices();
-            // AxesSign's values align with the datasheet
-            int axisMapSigns = signs.bVal;
-
-            if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
-                throw new InvalidAxisRemapException("Same axis cannot be included in AxesOrder twice");
-            }
-
-            // ensure right-handed coordinate system
-            boolean isXSwapped = indices[0] != 0;
-            boolean isYSwapped = indices[1] != 1;
-            boolean isZSwapped = indices[2] != 2;
-            boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
-                    && (!isXSwapped || !isYSwapped || !isZSwapped);
-            boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
-            // != functions as xor
-            if (areTwoAxesSwapped != oddNumOfFlips) {
-                throw new InvalidAxisRemapException("Coordinate system is left-handed");
-            }
-
-            // Bit:  7  6 |  5  4  |  3  2  |  1  0  |
-            //   reserved | z axis | y axis | x axis |
-            int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
-
-            // Enter CONFIG mode
-            imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
-
-            Thread.sleep(100);
-
-            // Write the AXIS_MAP_CONFIG register
-            imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
-
-            // Write the AXIS_MAP_SIGN register
-            imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
-
-            // Switch back to the previous mode
-            imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F);
-
-            Thread.sleep(100);
-        } catch (InterruptedException e) {
-            Thread.currentThread().interrupt();
-        }
-    }
-
-    /**
-     * Remaps the IMU coordinate system so that the remapped +Z faces the provided
-     * {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
-     *
-     * @param imu IMU
-     * @param direction axis direction
-     */
-    public static void remapZAxis(BNO055IMU imu, AxisDirection direction) {
-        switch (direction) {
-            case POS_X:
-                swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.NPP);
-                break;
-            case NEG_X:
-                swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.PPN);
-                break;
-            case POS_Y:
-                swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PNP);
-                break;
-            case NEG_Y:
-                swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PPN);
-                break;
-            case POS_Z:
-                swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PPP);
-                break;
-            case NEG_Z:
-                swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PNN);
-                break;
-        }
-    }
-
-    /**
-     * Now deprecated due to unintuitive parameter order.
-     * Use {@link #swapThenFlipAxes} or {@link #remapZAxis} instead.
-     *
-     * @param imu IMU
-     * @param order axes order
-     * @param signs axes signs
-     */
-    @Deprecated
-    public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
-        AxesOrder adjustedAxesOrder = order.reverse();
-        int[] indices = order.indices();
-        int axisSignValue = signs.bVal ^ (0b100 >> indices[0]);
-        AxesSigns adjustedAxesSigns = AxesSigns.fromBinaryValue(axisSignValue);
-
-        swapThenFlipAxes(imu, adjustedAxesOrder, adjustedAxesSigns);
-    }
-}

TeamCode/src/main/java/org/RoadRunner/util/DashboardUtil.java

@@ -1,54 +0,0 @@
-package org.RoadRunner.util;
-
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
-import com.acmerobotics.roadrunner.path.Path;
-
-import java.util.List;
-
-/**
- * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
- */
-public class DashboardUtil {
-    private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
-    private static final double ROBOT_RADIUS = 9; // in
-
-
-    public static void drawPoseHistory(Canvas canvas, List<Pose2d> poseHistory) {
-        double[] xPoints = new double[poseHistory.size()];
-        double[] yPoints = new double[poseHistory.size()];
-        for (int i = 0; i < poseHistory.size(); i++) {
-            Pose2d pose = poseHistory.get(i);
-            xPoints[i] = pose.getX();
-            yPoints[i] = pose.getY();
-        }
-        canvas.strokePolyline(xPoints, yPoints);
-    }
-
-    public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
-        int samples = (int) Math.ceil(path.length() / resolution);
-        double[] xPoints = new double[samples];
-        double[] yPoints = new double[samples];
-        double dx = path.length() / (samples - 1);
-        for (int i = 0; i < samples; i++) {
-            double displacement = i * dx;
-            Pose2d pose = path.get(displacement);
-            xPoints[i] = pose.getX();
-            yPoints[i] = pose.getY();
-        }
-        canvas.strokePolyline(xPoints, yPoints);
-    }
-
-    public static void drawSampledPath(Canvas canvas, Path path) {
-        drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
-    }
-
-    public static void drawRobot(Canvas canvas, Pose2d pose) {
-        canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
-        Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
-        double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
-        double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
-        canvas.strokeLine(x1, y1, x2, y2);
-    }
-}

TeamCode/src/main/java/org/RoadRunner/util/Encoder.java

@@ -1,125 +0,0 @@
-package org.RoadRunner.util;
-
-import com.acmerobotics.roadrunner.util.NanoClock;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-
-/**
- * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
- * slot's motor direction
- */
-public class Encoder {
-    private final static int CPS_STEP = 0x10000;
-
-    private static double inverseOverflow(double input, double estimate) {
-        // convert to uint16
-        int real = (int) input & 0xffff;
-        // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
-        // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
-        real += ((real % 20) / 4) * CPS_STEP;
-        // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
-        real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
-        return real;
-    }
-
-    public enum Direction {
-        FORWARD(1),
-        REVERSE(-1);
-
-        private int multiplier;
-
-        Direction(int multiplier) {
-            this.multiplier = multiplier;
-        }
-
-        public int getMultiplier() {
-            return multiplier;
-        }
-    }
-
-    private DcMotorEx motor;
-    private NanoClock clock;
-
-    private Direction direction;
-
-    private int lastPosition;
-    private int velocityEstimateIdx;
-    private double[] velocityEstimates;
-    private double lastUpdateTime;
-
-    public Encoder(DcMotorEx motor, NanoClock clock) {
-        this.motor = motor;
-        this.clock = clock;
-
-        this.direction = Direction.FORWARD;
-
-        this.lastPosition = 0;
-        this.velocityEstimates = new double[3];
-        this.lastUpdateTime = clock.seconds();
-    }
-
-    public Encoder(DcMotorEx motor) {
-        this(motor, NanoClock.system());
-    }
-
-    public Direction getDirection() {
-        return direction;
-    }
-
-    private int getMultiplier() {
-        return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
-    }
-
-    /**
-     * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
-     * @param direction either reverse or forward depending on if encoder counts should be negated
-     */
-    public void setDirection(Direction direction) {
-        this.direction = direction;
-    }
-
-    /**
-     * Gets the position from the underlying motor and adjusts for the set direction.
-     * Additionally, this method updates the velocity estimates used for compensated velocity
-     *
-     * @return encoder position
-     */
-    public int getCurrentPosition() {
-        int multiplier = getMultiplier();
-        int currentPosition = motor.getCurrentPosition() * multiplier;
-        if (currentPosition != lastPosition) {
-            double currentTime = clock.seconds();
-            double dt = currentTime - lastUpdateTime;
-            velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
-            velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
-            lastPosition = currentPosition;
-            lastUpdateTime = currentTime;
-        }
-        return currentPosition;
-    }
-
-    /**
-     * Gets the velocity directly from the underlying motor and compensates for the direction
-     * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
-     *
-     * @return raw velocity
-     */
-    public double getRawVelocity() {
-        int multiplier = getMultiplier();
-        return motor.getVelocity() * multiplier;
-    }
-
-    /**
-     * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
-     * that are lost in overflow due to velocity being transmitted as 16 bits.
-     * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
-     *
-     * @return corrected velocity
-     */
-    public double getCorrectedVelocity() {
-        double median = velocityEstimates[0] > velocityEstimates[1]
-                ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
-                : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
-        return inverseOverflow(getRawVelocity(), median);
-    }
-}

TeamCode/src/main/java/org/RoadRunner/util/LoggingUtil.java

@@ -1,60 +0,0 @@
-package org.RoadRunner.util;
-
-import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
-
-import java.io.File;
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.List;
-
-/**
- * Utility functions for log files.
- */
-public class LoggingUtil {
-    public static final File ROAD_RUNNER_FOLDER =
-            new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
-
-    private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
-
-    private static void buildLogList(List<File> logFiles, File dir) {
-        for (File file : dir.listFiles()) {
-            if (file.isDirectory()) {
-                buildLogList(logFiles, file);
-            } else {
-                logFiles.add(file);
-            }
-        }
-    }
-
-    private static void pruneLogsIfNecessary() {
-        List<File> logFiles = new ArrayList<>();
-        buildLogList(logFiles, ROAD_RUNNER_FOLDER);
-        Collections.sort(logFiles, (lhs, rhs) ->
-                Long.compare(lhs.lastModified(), rhs.lastModified()));
-
-        long dirSize = 0;
-        for (File file: logFiles) {
-            dirSize += file.length();
-        }
-
-        while (dirSize > LOG_QUOTA) {
-            if (logFiles.size() == 0) break;
-            File fileToRemove = logFiles.remove(0);
-            dirSize -= fileToRemove.length();
-            //noinspection ResultOfMethodCallIgnored
-            fileToRemove.delete();
-        }
-    }
-
-    /**
-     * Obtain a log file with the provided name
-     */
-    public static File getLogFile(String name) {
-        //noinspection ResultOfMethodCallIgnored
-        ROAD_RUNNER_FOLDER.mkdirs();
-
-        pruneLogsIfNecessary();
-
-        return new File(ROAD_RUNNER_FOLDER, name);
-    }
-}

TeamCode/src/main/java/org/RoadRunner/util/LynxModuleUtil.java

@@ -1,124 +0,0 @@
-package org.RoadRunner.util;
-
-import com.qualcomm.hardware.lynx.LynxModule;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.internal.system.Misc;
-
-import java.util.HashMap;
-import java.util.Map;
-
-/**
- * Collection of utilites for interacting with Lynx modules.
- */
-public class LynxModuleUtil {
-
-    private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
-
-    /**
-     * Parsed representation of a Lynx module firmware version.
-     */
-    public static class LynxFirmwareVersion implements Comparable<LynxFirmwareVersion> {
-        public final int major;
-        public final int minor;
-        public final int eng;
-
-        public LynxFirmwareVersion(int major, int minor, int eng) {
-            this.major = major;
-            this.minor = minor;
-            this.eng = eng;
-        }
-
-        @Override
-        public boolean equals(Object other) {
-            if (other instanceof LynxFirmwareVersion) {
-                LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
-                return major == otherVersion.major && minor == otherVersion.minor &&
-                        eng == otherVersion.eng;
-            } else {
-                return false;
-            }
-        }
-
-        @Override
-        public int compareTo(LynxFirmwareVersion other) {
-            int majorComp = Integer.compare(major, other.major);
-            if (majorComp == 0) {
-                int minorComp = Integer.compare(minor, other.minor);
-                if (minorComp == 0) {
-                    return Integer.compare(eng, other.eng);
-                } else {
-                    return minorComp;
-                }
-            } else {
-                return majorComp;
-            }
-        }
-
-        @Override
-        public String toString() {
-            return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
-        }
-    }
-
-    /**
-     * Retrieve and parse Lynx module firmware version.
-     * @param module Lynx module
-     * @return parsed firmware version
-     */
-    public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
-        String versionString = module.getNullableFirmwareVersionString();
-        if (versionString == null) {
-            return null;
-        }
-
-        String[] parts = versionString.split("[ :,]+");
-        try {
-            // note: for now, we ignore the hardware entry
-            return new LynxFirmwareVersion(
-                    Integer.parseInt(parts[3]),
-                    Integer.parseInt(parts[5]),
-                    Integer.parseInt(parts[7])
-            );
-        } catch (NumberFormatException e) {
-            return null;
-        }
-    }
-
-    /**
-     * Exception indicating an outdated Lynx firmware version.
-     */
-    public static class LynxFirmwareVersionException extends RuntimeException {
-        public LynxFirmwareVersionException(String detailMessage) {
-            super(detailMessage);
-        }
-    }
-
-    /**
-     * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
-     * @param hardwareMap hardware map containing Lynx modules
-     */
-    public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
-        Map<String, LynxFirmwareVersion> outdatedModules = new HashMap<>();
-        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
-            LynxFirmwareVersion version = getFirmwareVersion(module);
-            if (version == null || version.compareTo(MIN_VERSION) < 0) {
-                for (String name : hardwareMap.getNamesOf(module)) {
-                    outdatedModules.put(name, version);
-                }
-            }
-        }
-        if (outdatedModules.size() > 0) {
-            StringBuilder msgBuilder = new StringBuilder();
-            msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
-            msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
-                    MIN_VERSION.toString()));
-            for (Map.Entry<String, LynxFirmwareVersion> entry : outdatedModules.entrySet()) {
-                msgBuilder.append(Misc.formatInvariant(
-                        "\t%s: %s\n", entry.getKey(),
-                        entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
-            }
-            throw new LynxFirmwareVersionException(msgBuilder.toString());
-        }
-    }
-}

TeamCode/src/main/java/org/RoadRunner/util/RegressionUtil.java

@@ -1,156 +0,0 @@
-package org.RoadRunner.util;
-
-import androidx.annotation.Nullable;
-
-import com.acmerobotics.roadrunner.kinematics.Kinematics;
-
-import org.apache.commons.math3.stat.regression.SimpleRegression;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.io.PrintWriter;
-import java.util.ArrayList;
-import java.util.List;
-
-/**
- * Various regression utilities.
- */
-public class RegressionUtil {
-
-    /**
-     * Feedforward parameter estimates from the ramp regression and additional summary statistics
-     */
-    public static class RampResult {
-        public final double kV, kStatic, rSquare;
-
-        public RampResult(double kV, double kStatic, double rSquare) {
-            this.kV = kV;
-            this.kStatic = kStatic;
-            this.rSquare = rSquare;
-        }
-    }
-
-    /**
-     * Feedforward parameter estimates from the ramp regression and additional summary statistics
-     */
-    public static class AccelResult {
-        public final double kA, rSquare;
-
-        public AccelResult(double kA, double rSquare) {
-            this.kA = kA;
-            this.rSquare = rSquare;
-        }
-    }
-
-    /**
-     * Numerically compute dy/dx from the given x and y values. The returned list is padded to match
-     * the length of the original sequences.
-     *
-     * @param x x-values
-     * @param y y-values
-     * @return derivative values
-     */
-    private static List<Double> numericalDerivative(List<Double> x, List<Double> y) {
-        List<Double> deriv = new ArrayList<>(x.size());
-        for (int i = 1; i < x.size() - 1; i++) {
-            deriv.add(
-                    (y.get(i + 1) - y.get(i - 1)) /
-                    (x.get(i + 1) - x.get(i - 1))
-            );
-        }
-        // copy endpoints to pad output
-        deriv.add(0, deriv.get(0));
-        deriv.add(deriv.get(deriv.size() - 1));
-        return deriv;
-    }
-
-    /**
-     * Run regression to compute velocity and static feedforward from ramp test data.
-     *
-     * Here's the general procedure for gathering the requisite data:
-     *   1. Slowly ramp the motor power/voltage and record encoder values along the way.
-     *   2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
-     *      (kV) and an optional intercept (kStatic).
-     *
-     * @param timeSamples time samples
-     * @param positionSamples position samples
-     * @param powerSamples power samples
-     * @param fitStatic fit kStatic
-     * @param file log file
-     */
-    public static RampResult fitRampData(List<Double> timeSamples, List<Double> positionSamples,
-                                         List<Double> powerSamples, boolean fitStatic,
-                                         @Nullable File file) {
-        if (file != null) {
-            try (PrintWriter pw = new PrintWriter(file)) {
-                pw.println("time,position,power");
-                for (int i = 0; i < timeSamples.size(); i++) {
-                    double time = timeSamples.get(i);
-                    double pos = positionSamples.get(i);
-                    double power = powerSamples.get(i);
-                    pw.println(time + "," + pos + "," + power);
-                }
-            } catch (FileNotFoundException e) {
-                // ignore
-            }
-        }
-
-        List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
-
-        SimpleRegression rampReg = new SimpleRegression(fitStatic);
-        for (int i = 0; i < timeSamples.size(); i++) {
-            double vel = velSamples.get(i);
-            double power = powerSamples.get(i);
-
-            rampReg.addData(vel, power);
-        }
-
-        return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()),
-                              rampReg.getRSquare());
-    }
-
-    /**
-     * Run regression to compute acceleration feedforward.
-     *
-     * @param timeSamples time samples
-     * @param positionSamples position samples
-     * @param powerSamples power samples
-     * @param rampResult ramp result
-     * @param file log file
-     */
-    public static AccelResult fitAccelData(List<Double> timeSamples, List<Double> positionSamples,
-                                           List<Double> powerSamples, RampResult rampResult,
-                                           @Nullable File file) {
-        if (file != null) {
-            try (PrintWriter pw = new PrintWriter(file)) {
-                pw.println("time,position,power");
-                for (int i = 0; i < timeSamples.size(); i++) {
-                    double time = timeSamples.get(i);
-                    double pos = positionSamples.get(i);
-                    double power = powerSamples.get(i);
-                    pw.println(time + "," + pos + "," + power);
-                }
-            } catch (FileNotFoundException e) {
-                // ignore
-            }
-        }
-
-        List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
-        List<Double> accelSamples = numericalDerivative(timeSamples, velSamples);
-
-        SimpleRegression accelReg = new SimpleRegression(false);
-        for (int i = 0; i < timeSamples.size(); i++) {
-            double vel = velSamples.get(i);
-            double accel = accelSamples.get(i);
-            double power = powerSamples.get(i);
-
-            double powerFromVel = Kinematics.calculateMotorFeedforward(
-                    vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic);
-            double powerFromAccel = power - powerFromVel;
-
-            accelReg.addData(accel, powerFromAccel);
-        }
-
-        return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare());
-    }
-}

TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java

@@ -411,7 +411,7 @@ public abstract class CyberarmEngine extends OpMode {
    * state must have a construction that takes 3 arguments: object, groupName, and actionName
    * @param configuration TimeCraftersConfiguration
    * @param packageName Package name where states are defined
-   * @param object Object to pass to as first argument to states constructor
+   * @param object Object to pass as first argument to states constructor
    * @param objectClass Class to cast object to
    * @param groupName Group name
    */
@@ -456,7 +456,10 @@ public abstract class CyberarmEngine extends OpMode {
       } catch (ClassNotFoundException | NoSuchMethodException | IllegalAccessException | InstantiationException | InvocationTargetException e) {
         e.printStackTrace();
 
-        throw(new RuntimeException(e));
+        RuntimeException exception = new RuntimeException(e.getMessage(), e.getCause());
+        exception.setStackTrace(e.getStackTrace());
+
+        throw(exception);
       }
     }
   }

TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java

@@ -1,109 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class DriverParkPlaceState extends CyberarmState {
-    private final boolean stateDisabled;
-    PhoenixBot1 robot;
-    private int RampUpDistance;
-    private int RampDownDistance;
-    private String intendedPlacement;
-
-    public DriverParkPlaceState(PhoenixBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
-        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
-        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
-        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
-        this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
-        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
-    }
-    private double drivePower, targetDrivePower;
-    private int traveledDistance;
-
-    @Override
-    public void start() {
-        robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-    }
-
-    @Override
-    public void exec() {
-        if (stateDisabled) {
-            setHasFinished(true);
-            return;
-        }
-        String placement = engine.blackboardGetString("parkPlace");
-        if (placement != null) {
-            if (!placement.equals(intendedPlacement)){
-                    setHasFinished(true);
-            }
-            if (placement.equals(intendedPlacement)) {
-                double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
-
-                if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
-                    // ramping up
-                    drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.25;
-                } else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
-                    // ramping down
-                    drivePower = ((delta / RampDownDistance) + 0.25);
-                } else {
-                    // middle ground
-                    drivePower = targetDrivePower;
-                }
-
-                if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
-                    // This is limiting drive power to the targeted drive power
-                    drivePower = targetDrivePower;
-                }
-
-                if (targetDrivePower < 0 && drivePower != targetDrivePower) {
-                    drivePower = drivePower * -1;
-                }
-
-                if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
-                    robot.backLeftDrive.setPower(drivePower);
-                    robot.backRightDrive.setPower(drivePower);
-                    robot.frontLeftDrive.setPower(drivePower);
-                    robot.frontRightDrive.setPower(drivePower);
-                } else {
-                    robot.backLeftDrive.setPower(0);
-                    robot.backRightDrive.setPower(0);
-                    robot.frontLeftDrive.setPower(0);
-                    robot.frontRightDrive.setPower(0);
-                    setHasFinished(true);
-                } // else ending
-            } // placement equals if statement
-//            setHasFinished(true);
-        } // end of placement doesn't equal null
-
-    } // end of exec
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("Position", intendedPlacement);
-        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
-        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
-        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
-
-        engine.telemetry.addData("drivePower", drivePower);
-        engine.telemetry.addData("targetDrivePower", targetDrivePower);
-
-        engine.telemetry.addData("traveledDistance", traveledDistance);
-        engine.telemetry.addData("RampUpDistance", RampUpDistance);
-        engine.telemetry.addData("RampDownDistance", RampDownDistance);
-
-    } // end of telemetry
-} // end of class

TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java

@@ -1,195 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import android.util.Log;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class DriverStateWithOdometer extends CyberarmState {
-    private final boolean stateDisabled;
-    PhoenixBot1 robot;
-    private double RampUpDistance;
-    private double RampDownDistance;
-    private int maximumTolerance;
-    private float direction;
-    private boolean targetAchieved = false;
-    private double CurrentPosition;
-    public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
-        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
-        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
-        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
-        this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
-        this.direction = robot.configuration.variable(groupName, actionName, "direction").value();
-
-        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
-    }
-    private double drivePower, targetDrivePower;
-    private int traveledDistance;
-
-    @Override
-    public void start() {
-        robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-
-
-    }
-
-    @Override
-    public void exec() {
-
-        if (stateDisabled) {
-            setHasFinished(true);
-            return;
-        }
-
-        double RightCurrentPosition = Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
-        double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition());
-
-        if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
-        if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
-
-
-        if (Math.abs(CurrentPosition) <= RampUpDistance){
-            // ramping up
-//            double ratio = (Math.abs(CurrentPosition) / RampUpDistance);
-            if (targetDrivePower > 0) {
-                drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25;
-            } else {
-                drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25;
-            }
-        }
-        else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
-            // ramping down
-            if (targetDrivePower > 0){
-                drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25);
-            } else {
-                drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25);
-            }
-
-        } else {
-            // middle ground
-            drivePower = targetDrivePower;
-        }
-
-        if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
-            // This is limiting drive power to the targeted drive power
-            drivePower = targetDrivePower;
-        }
-
-        if (targetDrivePower < 0 && drivePower > 0) {
-            drivePower = drivePower * -1;
-        }
-
-        if (Math.abs(CurrentPosition) < traveledDistance - maximumTolerance){
-            if (targetAchieved) {
-                drivePower = drivePower * 0.25;
-
-                if (Math.abs(drivePower) < 0.25){
-                    if (drivePower < 0) {
-                        drivePower = -0.25;
-                    } else {
-                        drivePower = 0.25;
-                    }
-                }
-            }
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-
-        }
-        else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
-            targetAchieved = true;
-
-            drivePower = targetDrivePower * -0.25;
-
-            if (Math.abs(drivePower) < 0.25){
-                if (drivePower < 0) {
-                    drivePower = -0.25;
-                } else {
-                    drivePower = 0.25;
-                }
-            }
-
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-
-
-        } else {
-            robot.backLeftDrive.setPower(0);
-            robot.backRightDrive.setPower(0);
-            robot.frontLeftDrive.setPower(0);
-            robot.frontRightDrive.setPower(0);
-            setHasFinished(true);
-        }
-
-        if (!getHasFinished() && !targetAchieved){
-            float angle = robot.imu.getAngularOrientation().firstAngle - direction;
-
-            if (targetDrivePower < 0) { angle = angle * -1;}
-
-            if (angle < -0.25){
-                robot.backLeftDrive.setPower(drivePower * 0);
-                robot.backRightDrive.setPower(drivePower * 1.25);
-                robot.frontLeftDrive.setPower(drivePower * 0);
-                robot.frontRightDrive.setPower(drivePower * 1.25);
-            }
-            if (angle > 0.25) {
-                robot.backLeftDrive.setPower(drivePower * 1.25);
-                robot.backRightDrive.setPower(drivePower * 0);
-                robot.frontLeftDrive.setPower(drivePower * 1.25);
-                robot.frontRightDrive.setPower(drivePower * 0);
-            }
-        }
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
-        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
-        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
-        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
-        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
-        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
-        engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition());
-        engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
-        engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
-        engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
-
-        engine.telemetry.addData("Target Achieved", targetAchieved);
-
-
-
-        engine.telemetry.addData("drivePower", drivePower);
-        engine.telemetry.addData("targetDrivePower", targetDrivePower);
-
-        engine.telemetry.addData("traveledDistance", traveledDistance);
-        engine.telemetry.addData("RampUpDistance", RampUpDistance);
-        engine.telemetry.addData("RampDownDistance", RampDownDistance);
-
-        Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
-        Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
-        Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
-
-    }
-}

TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java

@@ -1,90 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class RotationState extends CyberarmState {
-    private final boolean stateDisabled;
-    PhoenixBot1 robot;
-    public RotationState(PhoenixBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
-        this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
-        drivePowerVariable = drivePower;
-        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
-
-
-    }
-
-    private double drivePower;
-    private float targetRotation;
-    float RobotRotation;
-    private double RotationTarget;
-    private double RotationDirectionMinimum;
-    private String debugStatus = "?";
-    private double drivePowerVariable;
-
-
-    @Override
-    public void exec() {
-        if (stateDisabled){
-            setHasFinished(true);
-            return;
-            } // end of if
-
-        RobotRotation = robot.imu.getAngularOrientation().firstAngle;
-
-        if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
-            drivePowerVariable = 0.4 * drivePower;
-            if (Math.abs(drivePowerVariable) < 0.4) {
-                if (drivePowerVariable < 0){
-                    drivePowerVariable = -0.4;
-                } else {
-                    drivePowerVariable = 0.4;
-                }
-            }
-            debugStatus = "Rotate Slow";
-        } // end of if
-        else {
-            drivePowerVariable = drivePower * 0.75;
-            debugStatus = "Rotate Fast";
-        }
-
-        if (RobotRotation >= targetRotation + 1){
-            drivePowerVariable = Math.abs(drivePowerVariable);
-        } else {
-            drivePowerVariable = -1 * Math.abs(drivePowerVariable);
-        }
-
-        if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) {
-            robot.backLeftDrive.setPower(drivePowerVariable);
-            robot.backRightDrive.setPower(-drivePowerVariable);
-            robot.frontLeftDrive.setPower(drivePowerVariable);
-            robot.frontRightDrive.setPower(-drivePowerVariable);
-        } else
-        {
-            robot.backLeftDrive.setPower(0);
-            robot.backRightDrive.setPower(0);
-            robot.frontLeftDrive.setPower(0);
-            robot.frontRightDrive.setPower(0);
-            setHasFinished(true);
-        }
-
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("DEBUG Status", debugStatus);
-
-        engine.telemetry.addLine();
-
-        engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
-        engine.telemetry.addData("Robot Target Rotation", targetRotation);
-        engine.telemetry.addData("Drive Power", drivePowerVariable);
-        engine.telemetry.addData("front right power", robot.frontRightDrive.getPower());
-        engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower());
-        engine.telemetry.addData("back left power", robot.backLeftDrive.getPower());
-        engine.telemetry.addData("back right power", robot.backRightDrive.getPower());
-
-    }
-}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftSideAutonomousEngine.java

@@ -1,22 +1,21 @@
-package org.timecrafters.Autonomous.Engines;
+package org.timecrafters.Phoenix.Autonomous.Engines;
 
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.ConeIdentification;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverState;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
+import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
+import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverState;
+import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
+import org.timecrafters.Phoenix.Autonomous.States.RotationState;
+import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
+import org.timecrafters.Phoenix.Autonomous.States.TopArm;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
-@Autonomous (name = "Left Side")
+//@Autonomous (name = "Left Side")
 @Disabled
 
 public class LeftSideAutonomousEngine extends CyberarmEngine {

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftStateAutoEngine.java

@@ -0,0 +1,32 @@
+package org.timecrafters.Phoenix.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+@Autonomous(name = "Left Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
+public class LeftStateAutoEngine extends CyberarmEngine {
+    PhoenixBot1 robot;
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
+    }
+
+    @Override
+    public void setup() {
+        robot = new PhoenixBot1(this);
+        robot.imu.resetYaw();
+
+        setupFromConfig(
+                robot.configuration,
+                "org.timecrafters.Autonomous.States",
+                robot,
+                PhoenixBot1.class,
+                "Left State Auto");
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftTwoConeAutonomousEngine.java

@@ -1,23 +1,21 @@
-package org.timecrafters.Autonomous.Engines;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+package org.timecrafters.Phoenix.Autonomous.Engines;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.ConeIdentification;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
-import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
-import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
+import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
+import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverStateWithOdometer;
+import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentAngleState;
+import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
+import org.timecrafters.Phoenix.Autonomous.States.RotationState;
+import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
+import org.timecrafters.Phoenix.Autonomous.States.TopArm;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
-@Autonomous (name = "left 2 cone auto")
+//@Autonomous (name = "left 2 cone auto")
 
 public class LeftTwoConeAutonomousEngine extends CyberarmEngine {
     PhoenixBot1 robot;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightSideAutonomousEngine.java

@@ -1,21 +1,21 @@
-package org.timecrafters.Autonomous.Engines;
+package org.timecrafters.Phoenix.Autonomous.Engines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverState;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverState;
+import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
+import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
+import org.timecrafters.Phoenix.Autonomous.States.RotationState;
+import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
+import org.timecrafters.Phoenix.Autonomous.States.TopArm;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
-@Autonomous (name = "Right Side")
+@Autonomous (name = "Right ")
 @Disabled
 
 public class RightSideAutonomousEngine extends CyberarmEngine {

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightStateAutoEngine.java

@@ -0,0 +1,128 @@
+package org.timecrafters.Phoenix.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+@Autonomous(name = "Right Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
+public class RightStateAutoEngine extends CyberarmEngine {
+    PhoenixBot1 robot;
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
+    }
+
+    @Override
+    public void setup() {
+        robot = new PhoenixBot1(this);
+        robot.imu.resetYaw();
+
+        setupFromConfig(
+                robot.configuration,
+                "org.timecrafters.Autonomous.States",
+                robot,
+                PhoenixBot1.class,
+                "Right State Auto");
+
+////        addState(new TopArmv2(robot, "Right State Auto", "06-0"));
+//
+////        // forward to low junction ..........................................................................(I have PreLoaded Cone)
+////        addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
+//
+//        // rotate left towards low junction (angle = 45, direction = CCW) (I have PreLoaded Cone)
+//        addState(new RotationState(robot, "Right State Auto", "02-1"));
+//
+//        // driving forward / backwards to adjust (I have PreLoaded Cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
+//
+//        // counteract distance driven .........................................................................(I'm going for 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
+//
+//        // rotate towards opposing alliance (angle = 0, direction = CW) (I'm going for 2nd cone)
+//        addState(new RotationState(robot, "Right State Auto", "05-0"));
+//
+//        // drive to tall junction (to adjust to cone stack)  (I'm going for 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));
+//
+//        // rotate at stack (angle = -90, direction = CW) (I'm going for 2nd cone)
+//        addState(new RotationState(robot, "Right State Auto", "07-0"));
+//
+//        // drive at stack (I'm going for 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "08-0"));
+//
+//
+//        // drive away from stack slightly....................................................................... (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "09-0"));
+//
+//        //drive away to low (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "10-0"));
+//
+//        // rotate at low junction (angle = -135, direction = CW) (I have a 2nd cone)
+//        addState(new RotationState(robot, "Right State Auto", "11-0"));
+//
+//        // driving forward / backwards to adjust (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "12-0"));
+//
+//        // counteract distance driven .......................................................................(I'm going for 3rd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "13-0"));
+//
+//        // rotate at stack (angle = -90, direction = CCW) (I'm going for 3rd cone)
+//        addState(new RotationState(robot, "Right State Auto", "14-0"));
+//
+//        // drive at stack (I'm going for 3rd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "15-0"));
+//
+//        // drive away from stack slightly................................................................... (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "16-0"));
+//
+//        //drive away to mid (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "17-0"));
+//
+//        // rotate at mid junction (angle = -135, direction = CW) (I have a 3rd and final cone)
+//        addState(new RotationState(robot, "Right State Auto", "18-0"));
+//
+//        // driving forward / backwards to adjust (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "19-0"));
+//
+//        // counteract distance driven.............................................................................. (I'm parking)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "20-0"));
+//
+//        // rotate at opposing alliance (angle = 0, direction = CCW) (I'm parking)
+//        addState(new RotationState(robot, "Right State Auto", "21-0"));
+//
+//        // Drive back one tile (I'm parking)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "22-0"));
+//
+//        // rotate towards stack side (-90 CW) (I'm parking)
+//        addState(new RotationState(robot, "Right State Auto", "23-0"));
+//
+//        // Choose Parking Spot (I'm parking)
+//        addState(new PathDecision(robot, "RightSideAutonomous", "24-0"));
+//
+//        // case 1 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-1"));
+//
+//        // case 2 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-2"));
+//
+//        // case 3 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-3"));
+//
+//        // rotate towards opposing alliance (angle = 0, direction = CCW) (I'm parking)
+//        addState(new RotationState(robot, "RightSideAutonomous", "25-0"));
+
+
+
+
+
+
+
+
+
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightTwoConeAutonomousEngine.java

@@ -1,23 +1,21 @@
-package org.timecrafters.Autonomous.Engines;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+package org.timecrafters.Phoenix.Autonomous.Engines;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.ConeIdentification;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
-import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
-import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
+import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
+import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverStateWithOdometer;
+import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentAngleState;
+import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
+import org.timecrafters.Phoenix.Autonomous.States.RotationState;
+import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
+import org.timecrafters.Phoenix.Autonomous.States.TopArm;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
-@Autonomous (name = "Right 2 cone auto")
+//@Autonomous (name = "Right 2 cone auto")
 
 public class RightTwoConeAutonomousEngine extends CyberarmEngine {
     PhoenixBot1 robot;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/BottomArm.java

@@ -1,7 +1,7 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class BottomArm extends CyberarmState {
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/CollectorDistanceState.java

@@ -1,10 +1,10 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class CollectorDistanceState extends CyberarmState {
 
@@ -12,7 +12,6 @@ public class CollectorDistanceState extends CyberarmState {
     private int traveledDistance;
     private int RampUpDistance;
     private int RampDownDistance;
-    private double drivePower;
     private double targetDrivePower;
     private double lastMeasuredTime;
     private double currentDistance;
@@ -24,6 +23,11 @@ public class CollectorDistanceState extends CyberarmState {
     private float collectTime;
     private double inRangeTime;
     private boolean stateDisabled;
+    private double distanceLimit;
+    private long maximumLookTime;
+    private long startOfSequencerTime;
+    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public final double COUNTS_PER_REVOLUTION = 8192;
 
 
     public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -33,6 +37,8 @@ public class CollectorDistanceState extends CyberarmState {
         this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
         this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
         this.collectTime = robot.configuration.variable(groupName, actionName, "collectTime").value();
+        this.distanceLimit = robot.configuration.variable(groupName, actionName, "distanceLimit").value();
+        this.maximumLookTime = robot.configuration.variable(groupName, actionName, "maximumLookTime").value();
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
 
 
@@ -40,25 +46,21 @@ public class CollectorDistanceState extends CyberarmState {
 
     @Override
     public void telemetry() {
+        engine.telemetry.addData("Time left", System.currentTimeMillis() - startOfSequencerTime);
+
         engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
         engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
         engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
         engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
-        engine.telemetry.addLine();
 
         engine.telemetry.addData("traveledDistance", traveledDistance);
         engine.telemetry.addData("RampUpDistance", RampUpDistance);
         engine.telemetry.addData("RampDownDistance", RampDownDistance);
 
-        engine.telemetry.addLine();
-
-        engine.telemetry.addData("drivePower", drivePower);
         engine.telemetry.addData("targetDrivePower", targetDrivePower);
 
         engine.telemetry.addLine();
-        engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
-        engine.telemetry.addData("Current Distance", currentDistance);
+        engine.telemetry.addData("Current Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
         engine.telemetry.addData("last Distance", LastDistanceRead);
         engine.telemetry.addLine();
 
@@ -75,25 +77,21 @@ public class CollectorDistanceState extends CyberarmState {
         robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
         robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
 
         robot.collectorLeft.setPower(1);
         robot.collectorRight.setPower(1);
 
         lastMeasuredTime = System.currentTimeMillis();
+        startOfSequencerTime = System.currentTimeMillis();
         LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM);
 
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
 
 
     }
@@ -102,13 +100,14 @@ public class CollectorDistanceState extends CyberarmState {
 
     @Override
     public void exec() {
-        if (stateDisabled){
+        if (stateDisabled || System.currentTimeMillis() - startOfSequencerTime > maximumLookTime) {
+            robot.collectorLeft.setPower(0);
+            robot.collectorRight.setPower(0);
             robot.frontRightDrive.setPower(0);
             robot.frontLeftDrive.setPower(0);
             robot.backRightDrive.setPower(0);
             robot.backLeftDrive.setPower(0);
             setHasFinished(true);
-            return;
         }
 
         if (System.currentTimeMillis() - lastMeasuredTime > 150) {
@@ -136,65 +135,44 @@ public class CollectorDistanceState extends CyberarmState {
                 robot.backLeftDrive.setPower(0);
 
                 setHasFinished(true);
-
-                return;
             }
         }
 
-        if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 70) {
+        if (robot.collectorDistance.getDistance(DistanceUnit.MM) > distanceLimit) {
+
             double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
 
-            if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
-                // ramping up
-                drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.15;
-            } else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
-                // ramping down
-                drivePower = ((delta / RampDownDistance) + 0.15);
-            } else {
-                // middle ground
-                drivePower = targetDrivePower;
-            }
+            robot.backLeftDrive.setPower(targetDrivePower);
+            robot.backRightDrive.setPower(targetDrivePower);
+            robot.frontLeftDrive.setPower(targetDrivePower);
+            robot.frontRightDrive.setPower(targetDrivePower);
 
-            if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
-                // This is limiting drive power to the targeted drive power
-                drivePower = targetDrivePower;
-            }
-
-            if (targetDrivePower < 0 && drivePower != targetDrivePower) {
-                drivePower = drivePower * -1;
-            }
-
-            if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
-                robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(drivePower);
-                robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(drivePower);
-            }
-    } else {
-        robot.frontRightDrive.setPower(0);
-        robot.frontLeftDrive.setPower(0);
-        robot.backRightDrive.setPower(0);
-        robot.backLeftDrive.setPower(0);
-
-        if (!inRange){
-            inRange = true;
-            inRangeTime = runTime();
         } else {
 
-            if (runTime() - inRangeTime >= collectTime){
-                robot.collectorRight.setPower(0);
-                robot.collectorLeft.setPower(0);
-                robot.frontRightDrive.setPower(0);
-                robot.frontLeftDrive.setPower(0);
-                robot.backRightDrive.setPower(0);
-                robot.backLeftDrive.setPower(0);
-                setHasFinished(true);
+            robot.frontRightDrive.setPower(0);
+            robot.frontLeftDrive.setPower(0);
+            robot.backRightDrive.setPower(0);
+            robot.backLeftDrive.setPower(0);
+
+            if (!inRange){
+                inRange = true;
+                inRangeTime = runTime();
+            } else {
+
+                if (runTime() - inRangeTime >= collectTime){
+                    robot.collectorRight.setPower(0);
+                    robot.collectorLeft.setPower(0);
+                    robot.frontRightDrive.setPower(0);
+                    robot.frontLeftDrive.setPower(0);
+                    robot.backRightDrive.setPower(0);
+                    robot.backLeftDrive.setPower(0);
+                    setHasFinished(true);
+                }
             }
-        }
+
+                }
 
             }
 
         }
 
-    }
-

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/CollectorState.java

@@ -1,8 +1,8 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class CollectorState extends CyberarmState {
 
@@ -52,8 +52,8 @@ public class CollectorState extends CyberarmState {
             }
 
         } else {
-//            robot.collectorLeft.setPower(0);
-//            robot.collectorRight.setPower(0);
+            robot.collectorLeft.setPower(0);
+            robot.collectorRight.setPower(0);
             setHasFinished(true);
         }
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/ConeIdentification.java

@@ -1,8 +1,8 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 import java.util.List;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverParkPlaceState.java

@@ -0,0 +1,318 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class DriverParkPlaceState extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private String intendedPlacement;
+    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public final double COUNTS_PER_REVOLUTION = 8192;
+    private double maximumTolerance;
+    private float direction;
+    private boolean targetAchieved = false;
+    public double startOfRampUpRight;
+    public double startOfRampDownRight;
+    public double startOfRampUpLeft;
+    public double startOfRampDownLeft;
+    public double endOfRampUpRight;
+    public double endOfRampDownRight;
+    public double endOfRampUpLeft;
+    public double endOfRampDownLeft;
+    public int driveStage;
+    public float currentAngle;
+    public double currentHorizontalEncoder;
+
+
+
+
+    public DriverParkPlaceState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
+        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
+        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
+        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
+        this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
+        this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+    }
+    private double drivePower, targetDrivePower;
+    private int traveledDistance;
+
+    @Override
+    public void start() {
+        robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+
+        if (targetDrivePower > 0) {
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
+
+        } else {
+
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
+
+        }
+
+        driveStage = 0;
+    }
+
+    @Override
+    public void exec() {
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+        String placement = engine.blackboardGetString("parkPlace");
+        if (placement == null || !placement.equals(intendedPlacement)) {
+
+            setHasFinished(true);
+
+        } else {
+
+                double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
+                double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
+
+                // Driving Forward
+                if (targetDrivePower > 0 && driveStage == 0) {
+
+                    // ramping up
+                    if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) ||
+                            (LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) {
+
+                        drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    }
+
+                    // Driving Normal
+                    else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) ||
+                            (LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) {
+
+                        drivePower = targetDrivePower;
+
+                    }
+
+                    // Ramping down going forward
+                    else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) ||
+                            (LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) {
+                        drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (driveStage == 0){
+                        driveStage = 1;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
+                }
+
+                // Driving Backwards .................................................................................................................................Backwards
+                if (targetDrivePower < 0 && driveStage == 0) {
+
+                    // ramping up
+                    if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) ||
+                            (LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) {
+
+                        drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    }
+
+                    // Driving Normal
+                    else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) ||
+                            (LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) {
+
+                        drivePower = targetDrivePower;
+
+                    }
+
+                    // Ramping down going backward
+                    else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) ||
+                            (LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) {
+
+                        drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+                    } else if (driveStage == 0){
+                        driveStage = 1;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                    }
+
+                    // end of ramp down
+                }
+
+                // Forwards distance adjust...............................................................................................................................STAGE 1
+                if (driveStage == 1 && targetDrivePower > 0) {
+
+                    if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                            RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                        drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                            RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                        drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else {
+                        driveStage = 2;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
+                }
+
+                // backwards distance adjust
+                if (driveStage == 1 && targetDrivePower < 0) {
+
+                    if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                            RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                        drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                            RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                        drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else {
+                        driveStage = 2;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
+                }
+
+                if (driveStage == 0 || driveStage == 1) {
+                    robot.frontRightDrive.setPower(drivePower);
+                    robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
+                    robot.backRightDrive.setPower(drivePower);
+                    robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
+                }
+                // Heading adjustment
+                if (driveStage == 2 || driveStage == 4) {
+
+                    currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+                    if (currentAngle - direction > robot.ROTATION_TOLERANCE) {
+
+                        robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
+                        robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
+                        robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER  );
+
+                    }
+                    else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) {
+
+                        robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                        robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                        robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                        robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+
+                    } else {
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                        driveStage ++;
+                    }
+                }
+
+                // ...........................................................................................................................................Strafe Adjustment
+                if ( driveStage == 3 ){
+
+                    currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
+                    if (currentHorizontalEncoder > 200){
+
+                        robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+
+                    }
+                    else if (currentHorizontalEncoder < -200){
+
+                        robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+
+                    } else {
+
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                        driveStage = 4;
+
+                    }
+
+                if (driveStage == 5) {
+                    setHasFinished(true);
+                }
+            } // placement equals if statement
+//            setHasFinished(true);
+        } // end of placement doesn't equal null
+
+    } // end of exec
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Position", intendedPlacement);
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
+
+        engine.telemetry.addData("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+    } // end of telemetry
+} // end of class

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverState.java

@@ -1,9 +1,9 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class DriverState extends CyberarmState {
     private final boolean stateDisabled;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverStateWithOdometer.java

@@ -0,0 +1,330 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class DriverStateWithOdometer extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private double RampUpDistance;
+    private double RampDownDistance;
+    private double maximumTolerance;
+    private float direction;
+    private boolean targetAchieved = false;
+    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public final double COUNTS_PER_REVOLUTION = 8192;
+    public double startOfRampUpRight;
+    public double startOfRampDownRight;
+    public double startOfRampUpLeft;
+    public double startOfRampDownLeft;
+    public double endOfRampUpRight;
+    public double endOfRampDownRight;
+    public double endOfRampUpLeft;
+    public double endOfRampDownLeft;
+    public int driveStage;
+    public float currentAngle;
+    public double currentHorizontalEncoder;
+
+    public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
+        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
+        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
+        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
+        this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
+        this.direction = robot.configuration.variable(groupName, actionName, "direction").value();
+
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+    }
+
+    private double drivePower, targetDrivePower, traveledDistance;
+
+    @Override
+    public void start() {
+
+        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+
+
+        if (targetDrivePower > 0) {
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
+
+        } else {
+
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
+
+        }
+
+        driveStage = 0;
+
+    }
+
+    @Override
+    public void exec() {
+
+
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+
+        double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
+        double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
+
+        // Driving Forward
+        if (targetDrivePower > 0 && driveStage == 0) {
+
+            // ramping up
+            if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) ||
+                    (LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) {
+
+                drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                        (Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+            }
+
+            // Driving Normal
+            else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) ||
+                    (LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) {
+
+                drivePower = targetDrivePower;
+
+            }
+
+            // Ramping down going forward
+            else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) ||
+                    (LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) {
+                drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                        (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else if (driveStage == 0){
+                driveStage = 1;
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+            }
+        }
+
+        // Driving Backwards .................................................................................................................................Backwards
+        if (targetDrivePower < 0 && driveStage == 0) {
+
+            // ramping up
+            if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) ||
+                    (LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) {
+
+                drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                        (Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+
+            }
+
+            // Driving Normal
+            else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) ||
+                    (LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) {
+
+                drivePower = targetDrivePower;
+
+            }
+
+            // Ramping down going backward
+            else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) ||
+                    (LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) {
+
+                drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                        (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+            } else if (driveStage == 0){
+                driveStage = 1;
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+            }
+
+            // end of ramp down
+        }
+
+        // Forwards distance adjust...............................................................................................................................STAGE 1
+        if (driveStage == 1 && targetDrivePower > 0) {
+
+            if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                    RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                    RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else {
+                driveStage = 2;
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+            }
+        }
+
+        // backwards distance adjust
+        if (driveStage == 1 && targetDrivePower < 0) {
+
+            if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                    RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                    RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else {
+                driveStage = 2;
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+            }
+        }
+
+        if (driveStage == 0 || driveStage == 1) {
+            robot.frontRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
+            robot.backRightDrive.setPower(drivePower);
+            robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
+        }
+        // Heading adjustment
+        if (driveStage == 2 || driveStage == 4) {
+
+            currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+            if (currentAngle - direction > robot.ROTATION_TOLERANCE) {
+
+                robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
+                robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05);
+                robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05  );
+                robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
+
+            }
+            else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) {
+
+                robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
+                robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
+                robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
+                robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05);
+
+            } else {
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+                driveStage ++;
+            }
+        }
+
+        // ...........................................................................................................................................Strafe Adjustment
+        if ( driveStage == 3 ){
+
+            currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
+            if (currentHorizontalEncoder > 200){
+
+                robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+
+            }
+            else if (currentHorizontalEncoder < -200){
+
+                robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+
+            } else {
+
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+                driveStage = 4;
+
+            }
+        }
+
+        if (driveStage == 5) {
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void telemetry () {
+        engine.telemetry.addData("Stage", driveStage);
+
+        engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
+        engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
+        engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
+        engine.telemetry.addData("startOfRampUpRight", startOfRampUpRight);
+        engine.telemetry.addData("endOfRampUpRight", endOfRampUpRight);
+        engine.telemetry.addData("startOfRampDownRight", startOfRampDownRight);
+        engine.telemetry.addData("endOfRampDownRight", endOfRampDownRight);
+        engine.telemetry.addData("startOfRampUpLeft", startOfRampUpLeft);
+        engine.telemetry.addData("endOfRampUpLeft", endOfRampUpLeft);
+        engine.telemetry.addData("startOfRampDownLeft", startOfRampDownLeft);
+        engine.telemetry.addData("endOfRampDownLeft", endOfRampDownLeft);
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
+        engine.telemetry.addData("maximumTolerance", maximumTolerance);
+        engine.telemetry.addData("imu 1 angle", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        engine.telemetry.addData("Target Achieved", targetAchieved);
+
+
+        engine.telemetry.addData("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/JunctionAllignmentAngleState.java

@@ -1,10 +1,11 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class JunctionAllignmentAngleState extends CyberarmState {
     private final boolean stateDisabled;
@@ -88,7 +89,7 @@ public class JunctionAllignmentAngleState extends CyberarmState {
     @Override
     public void exec() {
 
-        currentAngle = robot.imu.getAngularOrientation().firstAngle;
+        currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 
 
         if (stateDisabled){

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/JunctionAllignmentDistanceState.java

@@ -1,10 +1,8 @@
-package org.timecrafters.Autonomous.States;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class JunctionAllignmentDistanceState extends CyberarmState {
     private final boolean stateDisabled;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/PathDecision.java

@@ -1,7 +1,7 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class PathDecision extends CyberarmState {
     PhoenixBot1 robot;
@@ -17,6 +17,17 @@ public class PathDecision extends CyberarmState {
     @Override
     public void exec() {
         String placement = engine.blackboardGetString("parkPlace");
+
+        if (placement.equals("1")) {
+
+            addState(new DriverStateWithOdometer(robot, "" + groupName + " Parking", "Park 1"));
+        }
+
+        else if (placement.equals("3")){
+            addState(new DriverStateWithOdometer(robot, "" + groupName + " Parking", "Park 3"));
+
+        }
+
         setHasFinished(true);
         }
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/RotationState.java

@@ -0,0 +1,144 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class RotationState extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+
+    public RotationState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
+        this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
+        this.ClockWiseRotation = robot.configuration.variable(groupName, actionName, "ClockWiseRotation").value();
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+
+
+    }
+
+    private double drivePower;
+    private float targetRotation;
+    float CurrentRotation;
+    private String debugStatus = "?";
+    private double drivePowerVariable;
+    private double leftCompensator;
+    private double RightCompensator;
+    private boolean ClockWiseRotation;
+    private int RotationStage;
+    private double rotationDirection;
+    private long lastStepTime = 0;
+
+    @Override
+    public void start() {
+
+        leftCompensator = robot.OdometerEncoderLeft.getCurrentPosition();
+        RightCompensator = robot.OdometerEncoderRight.getCurrentPosition();
+
+        RotationStage = 0;
+    }
+
+    @Override
+    public void exec() {
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        } //
+
+
+        CurrentRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+        if (RotationStage == 0) {
+
+            drivePowerVariable = ((Math.abs(CurrentRotation - targetRotation) / 90) * (drivePower - robot.ROTATION_MINIMUM_POWER)) + robot.ROTATION_MINIMUM_POWER;
+
+            if (ClockWiseRotation) {
+                rotationDirection = 1;
+            } else {
+                rotationDirection = -1;
+            }
+
+
+            robot.backLeftDrive.setPower(drivePowerVariable * rotationDirection);
+            robot.backRightDrive.setPower(-drivePowerVariable * rotationDirection);
+            robot.frontLeftDrive.setPower(drivePowerVariable * rotationDirection);
+            robot.frontRightDrive.setPower(-drivePowerVariable * rotationDirection);
+
+            if (Math.abs(Math.abs(CurrentRotation) - Math.abs(targetRotation)) <= robot.ROTATION_TOLERANCE &&
+                    (RotationStage == 0) &&
+                    (CurrentRotation - targetRotation <= robot.ROTATION_TOLERANCE)) {
+                RotationStage = 1;
+                lastStepTime = System.currentTimeMillis();
+            }
+        }
+
+        if (RotationStage == 1) {
+            robot.backLeftDrive.setPower(0);
+            robot.backRightDrive.setPower(0);
+            robot.frontLeftDrive.setPower(0);
+            robot.frontRightDrive.setPower(0);
+            if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) {
+                RotationStage = 2;
+            }
+        }
+
+        if (RotationStage == 2) {
+
+            if (CurrentRotation - targetRotation > robot.ROTATION_TOLERANCE) {
+                // CW
+
+                robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+
+                lastStepTime = System.currentTimeMillis();
+
+            } else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) {
+                // CCW
+
+                robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+
+                lastStepTime = System.currentTimeMillis();
+
+            } else {
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+                if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) {
+                    RotationStage = 3;
+                }
+            }
+        }
+
+        if (RotationStage == 3) {
+            RotationStage ++;
+            setHasFinished(true);
+            }
+        }
+
+
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("DEBUG Status", debugStatus);
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("Robot IMU Rotation", CurrentRotation);
+        engine.telemetry.addData("Robot Target Rotation", targetRotation);
+        engine.telemetry.addData("Drive Power", drivePowerVariable);
+        engine.telemetry.addData("front right power", robot.frontRightDrive.getPower());
+        engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower());
+        engine.telemetry.addData("back left power", robot.backLeftDrive.getPower());
+        engine.telemetry.addData("back right power", robot.backRightDrive.getPower());
+        engine.telemetry.addData("RotationStage", RotationStage);
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/ServoCameraRotate.java

@@ -1,7 +1,7 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class ServoCameraRotate extends CyberarmState {
     private final boolean stateDisabled;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArm.java

@@ -1,7 +1,7 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class TopArm extends CyberarmState {
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArmResetState.java

@@ -0,0 +1,48 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class TopArmResetState extends CyberarmState {
+    private final PhoenixBot1 robot;
+    private double targetPower;
+    private int targetPosition;
+    private int tolerance;
+    private long lastMeasuredTime;
+    private long pausingTime;
+
+
+    public TopArmResetState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
+        this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value();
+        this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
+        this.pausingTime = robot.configuration.variable(groupName, actionName, "pausingTime").value();
+    }
+
+    @Override
+    public void start() {
+        robot.ArmMotor.setTargetPosition(targetPosition);
+        robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ArmMotor.setPower(targetPower);
+        lastMeasuredTime = System.currentTimeMillis();
+
+    }
+
+    @Override
+    public void exec() {
+
+        if (System.currentTimeMillis() - lastMeasuredTime > pausingTime) {
+
+            robot.ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            robot.ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            robot.ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            robot.ArmMotor.setTargetPosition(0);
+            robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            robot.ArmMotor.setPower(0.2);
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArmv2.java

@@ -0,0 +1,37 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class TopArmv2 extends CyberarmState {
+    private final PhoenixBot1 robot;
+    private double targetPower;
+    private int targetPosition;
+    private int tolerance;
+
+
+    public TopArmv2(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
+        this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value();
+        this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
+    }
+
+    @Override
+    public void start() {
+        robot.ArmMotor.setTargetPosition(targetPosition);
+        robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ArmMotor.setPower(targetPower);
+
+    }
+
+    @Override
+    public void exec() {
+
+        if (robot.ArmMotor.getTargetPosition() == targetPosition) {
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/PhoenixBot1.java

@@ -1,11 +1,14 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix;
 
 import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
@@ -16,12 +19,25 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
 import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
 import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+import org.timecrafters.minibots.cyberarm.chiron.states.autonomous.Arm;
 
 public class PhoenixBot1 {
 
     private static final float mmPerInch        = 25.4f;
-    public static final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public static double WHEEL_CIRCUMFERENCE;
     public static final int COUNTS_PER_REVOLUTION = 8192;
+    public static double leftCompensatorGlobal;
+    public static double RightCompensatorGlobal;
+    public double VEER_COMPENSATION_DBL; // some place around 1, .99 is 1% power reduction
+    public double DRIVETRAIN_MINIMUM_POWER;
+    public double ROTATION_MINIMUM_POWER;
+    public double STRAFE_MINIMUM_POWER;
+    public double DRIVE_TOLERANCE;
+    public double ROTATION_TOLERANCE;
+    public long PAUSE_ON_ROTATION;
+    public double DISTANCE_MULTIPLIER;
+    public double CAMERA_INITiAL_POS;
 
 //    private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
     private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
@@ -46,11 +62,13 @@ public class PhoenixBot1 {
 
     public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance;
 
-        public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal, ArmMotor, armMotorEncoder;
+        public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal;
+
+        public DcMotorEx ArmMotor;
 
         public CRServo collectorLeft, collectorRight;
 
-         public BNO055IMU imu;
+         public IMU imu;
 
          public TimeCraftersConfiguration configuration;
 
@@ -68,11 +86,28 @@ public class PhoenixBot1 {
         public PhoenixBot1(CyberarmEngine engine) {
             this.engine = engine;
 
+            configuration = new TimeCraftersConfiguration();
+
+            initConstants();
+
             initVuforia();
             initTfod();
             setupRobot();
         }
 
+        public void initConstants(){
+            VEER_COMPENSATION_DBL = configuration.variable("Robot", "Tuning", "VEER_COMPENSATION_DBL").value();
+            DRIVETRAIN_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "DRIVETRAIN_MINIMUM_POWER").value();
+            ROTATION_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "ROTATION_MINIMUM_POWER").value();
+            STRAFE_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "STRAFE_MINIMUM_POWER").value();
+            DRIVE_TOLERANCE = configuration.variable("Robot", "Tuning", "DRIVE_TOLERANCE").value();
+            ROTATION_TOLERANCE = configuration.variable("Robot", "Tuning", "ROTATION_TOLERANCE").value();
+            PAUSE_ON_ROTATION = configuration.variable("Robot", "Tuning", "PAUSE_ON_ROTATION").value();
+            WHEEL_CIRCUMFERENCE = configuration.variable("Robot", "Tuning", "WHEEL_CIRCUMFERENCE").value();
+            DISTANCE_MULTIPLIER = configuration.variable("Robot", "Tuning", "DISTANCE_MULTIPLIER").value();
+            CAMERA_INITiAL_POS = configuration.variable("Robot", "Tuning", "CAMERA_INITiAL_POS").value();
+        }
+
     private void initVuforia(){
         /*
          * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
@@ -108,17 +143,20 @@ public class PhoenixBot1 {
             leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
             rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance");
 
-            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+            IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+                    RevHubOrientationOnRobot.UsbFacingDirection.UP
+            ));
 
-            parameters.mode = BNO055IMU.SensorMode.IMU;
-            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
-            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
-            parameters.loggingEnabled = false;
+//            parameters.mode = BNO055IMU.SensorMode.IMU;
+//            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+//            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+//            parameters.loggingEnabled = false;
 
-            this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu");
+            this.imu = engine.hardwareMap.get(IMU.class, "imu");
             imu.initialize(parameters);
 
-            imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
+//            imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
 
             configuration = new TimeCraftersConfiguration("Phoenix");
 //            AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit");
@@ -147,8 +185,7 @@ public class PhoenixBot1 {
             LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight");
 //            HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft");
 //            HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight");
-            ArmMotor = engine.hardwareMap.dcMotor.get("ArmMotor");
-            armMotorEncoder = engine.hardwareMap.dcMotor.get("Arm Motor Encoder");
+            ArmMotor = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor");
 
             //motors direction and encoders
 
@@ -162,6 +199,7 @@ public class PhoenixBot1 {
             frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
             frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
+            backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
             backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
             backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
             backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -199,10 +237,9 @@ public class PhoenixBot1 {
 //            HighRiserRight.setDirection(Servo.Direction.FORWARD);
             LowRiserLeft.setDirection(Servo.Direction.FORWARD);
             LowRiserRight.setDirection(Servo.Direction.REVERSE);
-            armMotorEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            armMotorEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
             ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
             ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
             ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
             CameraServo.setDirection(Servo.Direction.FORWARD);
@@ -212,8 +249,17 @@ public class PhoenixBot1 {
 //            HighRiserLeft.setPosition(0.40);
 //            HighRiserRight.setPosition(0.40);
 
-            CameraServo.setPosition(0.775);
+            CameraServo.setPosition(CAMERA_INITiAL_POS);
 
+
+
+        }
+
+        public int AngleToTicks(double angle) {
+            double d = (60 * 28) / 360.0;
+
+            // Casting to float so that the int version of Math.round is used.
+            return Math.round((float)d * (float)angle);
         }
 
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/CameraTestEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/DriveDebugEngine.java

@@ -1,7 +1,7 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class DriveDebugEngine extends CyberarmEngine {
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/DynamicSetupEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/PhoenixTeleOP.java

@@ -0,0 +1,23 @@
+package org.timecrafters.Phoenix.TeleOp.engine;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Phoenix.PhoenixBot1;
+import org.timecrafters.Phoenix.TeleOp.states.TeleOPArmDriver;
+import org.timecrafters.Phoenix.TeleOp.states.TeleOPTankDriver;
+
+@TeleOp (name = "APhoenixTeleOP")
+
+public class PhoenixTeleOP extends CyberarmEngine {
+
+    PhoenixBot1 robot;
+
+    @Override
+    public void setup() {
+
+        robot = new PhoenixBot1(this);
+        addState(new TeleOPArmDriver(robot));
+        addParallelStateToLastState(new TeleOPTankDriver(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/PhoenixWingEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.CRServo;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SodiEngine.java

@@ -1,10 +1,10 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.LaserState;
+import org.timecrafters.Phoenix.TeleOp.states.LaserState;
 
 @Disabled
 @TeleOp(name = "Wheel")

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SpeedrunEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -6,9 +6,9 @@ import com.qualcomm.robotcore.hardware.Gamepad;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.cyberarm.engine.V2.GamepadChecker;
+import org.timecrafters.Phoenix.TeleOp.states.TeleOPSpeedrunState;
+import org.timecrafters.Phoenix.PhoenixBot1;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-import org.timecrafters.TeleOp.states.TeleOPSpeedrunState;
 
 @Disabled
 @TeleOp (name = "Speedrun Engine")

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SterringDriveEngine.java

@@ -1,12 +1,11 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.LaserState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-import org.timecrafters.TeleOp.states.SteeringDriveExperiment;
+import org.timecrafters.Phoenix.TeleOp.states.SteeringDriveExperiment;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 @Disabled
 @TeleOp(name = "Steering Drive Test")

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/CameraTestCommon.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.firstinspires.ftc.robotcore.external.tfod.Recognition;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/CameraTestState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/DynamicSetupState.java

@@ -1,7 +1,7 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.engine.DynamicSetupEngine;
+import org.timecrafters.Phoenix.TeleOp.engine.DynamicSetupEngine;
 
 public class DynamicSetupState extends CyberarmState {
     private long delay;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/LaserServoHeight.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.robotcore.hardware.Servo;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/LaserState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixTeleOPState.java

@@ -1,16 +1,16 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import android.annotation.SuppressLint;
-import android.widget.Switch;
 
-import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.cyberarm.engine.V2.GamepadChecker;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class PhoenixTeleOPState extends CyberarmState {
 
@@ -52,7 +52,7 @@ public class PhoenixTeleOPState extends CyberarmState {
     public void telemetry() {
         engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
         engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
-        engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
+        engine.telemetry.addData("IMU", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
         engine.telemetry.addData("Drive Power", drivePower);
         engine.telemetry.addData("Delta Rotation", DeltaRotation);
         engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM));
@@ -131,7 +131,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.dpad_right) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = 90;
             CalculateDeltaRotation();
             if (RobotRotation > -90 && RobotRotation < 89) {//CCW
@@ -158,7 +158,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.dpad_left) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = -90;
             CalculateDeltaRotation();
             if (RobotRotation > -89 && RobotRotation <= 90) {//CW
@@ -185,7 +185,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.y) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = 180;
             CalculateDeltaRotation();
             if (RobotRotation < 0 && RobotRotation > -179) {
@@ -210,7 +210,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.a && !engine.gamepad1.start) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = 0;
             CalculateDeltaRotation();
             if (RobotRotation < -1) {
@@ -237,7 +237,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.x) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = -45;
             CalculateDeltaRotation();
             if (RobotRotation < -46 || RobotRotation > 135) {//CCW
@@ -264,7 +264,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.b) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = 45;
             CalculateDeltaRotation();
             if (RobotRotation > -135 && RobotRotation < 44) {//CCW
@@ -470,24 +470,10 @@ public class PhoenixTeleOPState extends CyberarmState {
     @Override
     public void buttonUp(Gamepad gamepad, String button) {
         if (engine.gamepad1 == gamepad && button.equals("start")) {
-            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
 
-            parameters.mode = BNO055IMU.SensorMode.IMU;
-            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
-            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
-            parameters.loggingEnabled = false;
-
-            robot.imu.initialize(parameters);
+            robot.imu.resetYaw();
         }
     }
-//    public double downSensor() {
-//        double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5;
-//        Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
-//        return Distance;
+
 
     }

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixTeleOPv2.java

@@ -1,6 +1,7 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class PhoenixTeleOPv2 extends CyberarmState {
     private double drivePower = 1;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixWingState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SodiLEDState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SodiState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SteeringDriveExperiment.java

@@ -1,6 +1,7 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 
 public class SteeringDriveExperiment extends CyberarmState {

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPArmDriver.java

@@ -0,0 +1,234 @@
+package org.timecrafters.Phoenix.TeleOp.states;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.cyberarm.engine.V2.GamepadChecker;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class TeleOPArmDriver extends CyberarmState {
+    private final PhoenixBot1 robot;
+    private long lastStepTime = 0, Spirit;
+    private int CyclingArmUpAndDown = 0;
+    private GamepadChecker gamepad2Checker;
+    private int Opportunity, JunctionHeight, Ingenuity;
+    private double drivePower, armPower;
+    private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05;
+    private double servoCollect = 0.45; //Low servos, A button
+    //    private double servoCollectHigh = 0.40; //High servos, A button
+    private double servoLow = 0.45; //Low servos, X button
+    //    private double servoLowHigh = 0.75; //High servos, X button
+    private double servoMed = 0.45; //Low servos, B button
+    //    private double servoMedHigh = 0.9; //High servos, B button
+    private double servoHigh = 0.8; //Low servos, Y button
+    //    private double servoHighHigh = 0.9; //High servos, Y button
+    private double ArmNeededPower;
+    private int armMotorCollect = -100;
+    private int armMotorLow = 280;
+    private int armMotorMed = 430;
+    private int armMotorHigh = 510;
+    private int ArmMotorStepSize = 2;
+    private int TargetPosition = 0, OverrideTarget = -64;
+
+    public TeleOPArmDriver(PhoenixBot1 robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Arm Driver:");
+        engine.telemetry.addData("Arm Motor Power", robot.ArmMotor.getPower());
+        engine.telemetry.addData("Arm Motor Encoder Position", robot.ArmMotor.getCurrentPosition());
+        engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
+        engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
+        engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition());
+        engine.telemetry.addData("Target Junction Height", JunctionHeight);
+    }
+
+    @Override
+    public void init() {
+        robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
+        robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
+        robot.LowRiserLeft.setPosition(0.45);
+        robot.LowRiserRight.setPosition(0.45);
+        robot.ArmMotor.setTargetPosition(0);
+        robot.ArmMotor.setPower(0.5);
+        robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ArmMotor.setTargetPositionTolerance(5);
+        armPower = 0.4;
+
+        JunctionHeight = 0;
+    }
+
+    @Override
+    public void exec() {
+
+        double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - TargetPosition) / 400.0 + 0.1;
+
+
+//        ArmNeededPower =  Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
+//        robot.ArmMotor.setTargetPosition(TargetPosition);
+//        robot.ArmMotor.setPower(armPower);
+
+        if (engine.gamepad2.y) {
+            JunctionHeight = 4;
+            TargetPosition = armMotorHigh;
+        }
+        if (engine.gamepad2.b) {
+            JunctionHeight = 3;
+            TargetPosition = armMotorMed;
+        }
+        if (engine.gamepad2.x) {
+            JunctionHeight = 2;
+            TargetPosition = armMotorLow;
+        }
+        if (engine.gamepad2.a) {
+            JunctionHeight = 1;
+            TargetPosition = armMotorCollect;
+        }
+
+        if (JunctionHeight == 4) {
+            if (robot.LowRiserLeft.getPosition() <= servoHigh && robot.ArmMotor.getTargetPosition() != armMotorHigh) {
+                robot.ArmMotor.setPower(0);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(armMotorHigh);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+            if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoHigh) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
+                }
+            } else if (robot.ArmMotor.getTargetPosition() == armMotorHigh && robot.LowRiserLeft.getPosition() == servoHigh) {
+                JunctionHeight = 0;
+            }
+        }
+
+        if (JunctionHeight == 3) {
+            if (robot.LowRiserLeft.getPosition() <= servoMed && robot.ArmMotor.getTargetPosition() != armMotorMed) {
+                robot.ArmMotor.setPower(0);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(armMotorMed);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+            if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoMed) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
+                }
+            } else if (robot.LowRiserLeft.getPosition() > servoMed) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
+                }
+            } else if (robot.ArmMotor.getTargetPosition() == armMotorMed && robot.LowRiserLeft.getPosition() == servoMed) {
+                JunctionHeight = 0;
+            }
+        }
+
+        if (JunctionHeight == 2) {
+            if (robot.LowRiserLeft.getPosition() <= servoLow && robot.ArmMotor.getTargetPosition() != armMotorLow) {
+                robot.ArmMotor.setPower(0);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(armMotorLow);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+            if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoLow) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
+                }
+            } else if (robot.LowRiserLeft.getPosition() > servoLow) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
+                }
+            } else if (robot.ArmMotor.getTargetPosition() == armMotorLow && robot.LowRiserLeft.getPosition() == servoLow) {
+                JunctionHeight = 0;
+            }
+        }
+
+        if (JunctionHeight == 1) {
+            if (robot.LowRiserLeft.getPosition() <= servoCollect && robot.ArmMotor.getTargetPosition() != armMotorCollect) {
+                robot.ArmMotor.setPower(0);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(armMotorCollect);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+            if (robot.LowRiserLeft.getPosition() > servoCollect) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
+                }
+            } else if (robot.ArmMotor.getTargetPosition() == armMotorCollect && robot.LowRiserLeft.getPosition() == servoCollect) {
+                JunctionHeight = 0;
+            }
+        }
+
+        if (engine.gamepad2.left_bumper) {
+            if (robot.ArmMotor.getTargetPosition() != OverrideTarget) {
+                OverrideTarget = robot.ArmMotor.getCurrentPosition() - 80;
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(OverrideTarget);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+        }
+
+        if (engine.gamepad2.right_bumper) {
+            if (robot.ArmMotor.getTargetPosition() != OverrideTarget) {
+                OverrideTarget = robot.ArmMotor.getCurrentPosition() + 80;
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(OverrideTarget);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+        }
+
+        if (engine.gamepad2.dpad_left) {
+            robot.collectorRight.setPower(-1);
+            robot.collectorLeft.setPower(-1);
+        } else if (engine.gamepad2.dpad_right) {
+            robot.collectorLeft.setPower(1);
+            robot.collectorRight.setPower(1);
+        } else {
+            robot.collectorLeft.setPower(0);
+            robot.collectorRight.setPower(0);
+        }
+
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        if (engine.gamepad2 == gamepad) {
+            if (button.equals("right_bumper")) {
+                OverrideTarget = -64;
+            } else if (button.equals("left_bumper")) {
+                OverrideTarget = -64;
+            }
+        }
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (engine.gamepad2 == gamepad) {
+            if (button.equals("right_bumper")) {
+
+            } else if (button.equals("left_bumper")) {
+
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPSpeedrunState.java

@@ -1,8 +1,9 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
 public class TeleOPSpeedrunState extends CyberarmState {

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPTankDriver.java

@@ -0,0 +1,144 @@
+package org.timecrafters.Phoenix.TeleOp.states;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.cyberarm.engine.V2.GamepadChecker;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class TeleOPTankDriver extends CyberarmState {
+
+    private final PhoenixBot1 robot;
+    private long lastStepTime = 0;
+    private double drivePower = 0.3;
+    private double RobotRotation;
+    private double RotationTarget, DeltaRotation;
+    private double MinimalPower = 0.2;
+    private int DeltaOdometerR, Spirit;
+    private GamepadChecker gamepad1Checker;
+
+    public TeleOPTankDriver(PhoenixBot1 robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Tank Driver");
+        engine.telemetry.addData("IMU", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        engine.telemetry.addData("Drive Power", drivePower);
+        engine.telemetry.addData("Delta Rotation", DeltaRotation);
+    }
+
+    @Override
+    public void init() {
+
+    }
+
+    @Override
+    public void exec() {
+
+        double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative //ORLY?
+        double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
+        double rx = engine.gamepad1.right_stick_x;
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+        double frontLeftPower = (y + x + rx) / denominator;
+        double backLeftPower = (y - x + rx) / denominator;
+        double frontRightPower = (y - x - rx) / denominator;
+        double backRightPower = (y + x - rx) / denominator;
+
+
+        double heading = -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+        double rotX = x * Math.cos(heading) - y * Math.sin(heading);
+        double rotY = x * Math.sin(heading) + y * Math.cos(heading);
+
+        frontLeftPower = (rotY + rotX + rx) / denominator;
+        backLeftPower = (rotY - rotX + rx) / denominator;
+        frontRightPower = (rotY - rotX - rx) / denominator;
+        backRightPower = (rotY + rotX - rx) / denominator;
+
+        robot.backLeftDrive.setPower(backLeftPower * drivePower);
+        robot.backRightDrive.setPower(backRightPower * drivePower);
+        robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
+        robot.frontRightDrive.setPower(frontRightPower * drivePower);
+
+
+
+//        if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
+//            drivePower = engine.gamepad1.left_stick_y;
+//            robot.backLeftDrive.setPower(drivePower);
+//            robot.backRightDrive.setPower(drivePower);
+//            robot.frontLeftDrive.setPower(drivePower);
+//            robot.frontRightDrive.setPower(drivePower);
+//        }
+//
+//        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) {
+//            drivePower = engine.gamepad1.left_stick_x;
+//            robot.backLeftDrive.setPower(drivePower);
+//            robot.backRightDrive.setPower(-drivePower);
+//            robot.frontLeftDrive.setPower(-drivePower);
+//            robot.frontRightDrive.setPower(drivePower);
+//        }
+//
+//        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
+//            robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
+//            robot.backLeftDrive.setPower(backLeftPower * drivePower);
+//            robot.frontRightDrive.setPower(frontRightPower * drivePower);
+//            robot.backRightDrive.setPower(backRightPower * drivePower);
+//        }
+//
+//        if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
+//            drivePower = engine.gamepad1.right_stick_x;
+//            robot.backLeftDrive.setPower(-drivePower);
+//            robot.backRightDrive.setPower(drivePower);
+//            robot.frontLeftDrive.setPower(-drivePower);
+//            robot.frontRightDrive.setPower(drivePower);
+//        }
+//
+//        if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) {
+//            drivePower = 0;
+//            robot.backLeftDrive.setPower(drivePower);
+//            robot.backRightDrive.setPower(drivePower);
+//            robot.frontLeftDrive.setPower(drivePower);
+//            robot.frontRightDrive.setPower(drivePower);
+//        }
+
+
+
+    }
+
+    public void CalculateDeltaRotation() {
+        if (RotationTarget >= 0 && RobotRotation >= 0) {
+            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
+        } else if (RotationTarget <= 0 && RobotRotation <= 0) {
+            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
+        } else if (RotationTarget >= 0 && RobotRotation <= 0) {
+            DeltaRotation = Math.abs(RotationTarget + RobotRotation);
+        } else if (RotationTarget <= 0 && RobotRotation >= 0) {
+            DeltaRotation = Math.abs(RobotRotation + RotationTarget);
+        }
+    }
+
+    public void getDeltaOdometerR() {
+        Spirit = robot.OdometerEncoderRight.getCurrentPosition();
+        if (System.currentTimeMillis() - lastStepTime >= 1000) {
+            lastStepTime = System.currentTimeMillis();
+            DeltaOdometerR = robot.OdometerEncoderRight.getCurrentPosition() - Spirit;
+        }
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        if (engine.gamepad1 == gamepad && button.equals("start")) {
+
+            robot.imu.resetYaw();
+        }
+        if (engine.gamepad1 == gamepad && button.equals("y")) {
+            drivePower = 1;
+        }
+        if (engine.gamepad1 == gamepad && button.equals("a")) {
+            drivePower = 0.5;
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixTeleOP.java

@@ -1,21 +0,0 @@
-package org.timecrafters.TeleOp.engine;
-
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-import org.timecrafters.TeleOp.states.PhoenixTeleOPState;
-
-@TeleOp (name = "APhoenixTeleOP")
-
-public class PhoenixTeleOP extends CyberarmEngine {
-
-    PhoenixBot1 robot;
-
-    @Override
-    public void setup() {
-
-        robot = new PhoenixBot1(this);
-        addState(new PhoenixTeleOPState(robot));
-    }
-}

TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java

@@ -1,220 +0,0 @@
-package org.timecrafters.TeleOp.states;
-
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.cyberarm.engine.V2.GamepadChecker;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-
-public class TeleOPArmDriver extends CyberarmState {
-    private final PhoenixBot1 robot;
-    private long lastStepTime = 0;
-    private int CyclingArmUpAndDown = 0;
-    private GamepadChecker gamepad2Checker;
-    private int Opportunity, Endeavour, Peanut;
-    private double drivePower, armPower;
-    private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05;
-    private double servoCollectLow = 0.40; //Low servos, A button
-//    private double servoCollectHigh = 0.40; //High servos, A button
-    private double servoLowLow = 0.5; //Low servos, X button
-//    private double servoLowHigh = 0.75; //High servos, X button
-    private double servoMedLow = 0.5; //Low servos, B button
-//    private double servoMedHigh = 0.9; //High servos, B button
-    private double servoHighLow = 0.8; //Low servos, Y button
-//    private double servoHighHigh = 0.9; //High servos, Y button
-    private int armMotorCollect = 0;
-    private int armMotorLow = 100;
-    private int armMotorMed = 1000;
-    private int armMotorHigh = 1600;
-
-    public TeleOPArmDriver(PhoenixBot1 robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addLine("Arm Driver:");
-        engine.telemetry.addData("Arm Motor Power", robot.ArmMotor.getPower());
-        engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
-        engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
-    }
-
-    @Override
-    public void init() {
-        robot.ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
-        robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
-        robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
-        robot.LowRiserLeft.setPosition(0.45);
-        robot.LowRiserRight.setPosition(0.45);
-        robot.ArmMotor.setPower(0);
-        robot.armMotorEncoder.setPower(0);
-        Opportunity = 0;
-        Endeavour = 0;
-
-
-
-        gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
-    }
-
-@Override
-public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO MM HEIGHTS!!
-
-        if (engine.gamepad2.y) {
-            Endeavour = 4;
-        }
-        if (engine.gamepad2.b) {
-            Endeavour = 3;
-        }
-        if (engine.gamepad2.x) {
-            Endeavour = 2;
-        }
-        if (engine.gamepad2.a) {
-            Endeavour = 1;
-        }
-
-        if (Endeavour == 4) {
-            if (robot.armMotorEncoder.getCurrentPosition() < armMotorHigh - 5)/* <-- high level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setPower(0.5);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() < servoHighLow - 5)/* <-- low level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
-                }
-            }
-            if (robot.armMotorEncoder.getCurrentPosition() >= armMotorHigh &&
-                    robot.LowRiserLeft.getPosition() >= servoHighLow) {
-                Endeavour = 0;
-            }
-        }
-
-        if (Endeavour == 3) {
-            if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() < servoMedLow - 5)/* <-- low level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 &&
-                    robot.armMotorEncoder.getCurrentPosition() > armMotorMed + 5)/* <-- high level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setPower(-0.5);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 &&
-                    robot.armMotorEncoder.getCurrentPosition() < armMotorMed - 5)/* <-- high level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setPower(0.5);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
-                    robot.LowRiserLeft.getPosition() <= servoMedLow &&
-                    robot.armMotorEncoder.getCurrentPosition() > armMotorMed - 5) {
-                Endeavour = 0;
-            }
-        }
-
-        if (Endeavour == 2) {
-            if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() < servoLowLow - 5)/* <-- low level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
-                    robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
-                    robot.armMotorEncoder.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setPower(-0.5);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
-                    robot.armMotorEncoder.getCurrentPosition() < armMotorLow - 5)/* <-- high level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setPower(0.5);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
-                    robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
-                    robot.armMotorEncoder.getCurrentPosition() > armMotorLow - 5) {
-                Endeavour = 0;
-            }
-        }
-
-        if (Endeavour == 1) {
-            if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 5)/* <-- low level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
-                }
-            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
-                    robot.armMotorEncoder.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setPower(-0.5);
-                }
-            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 &&
-                    robot.armMotorEncoder.getCurrentPosition() <= armMotorCollect) {
-                Endeavour = 0;
-            }
-        }
-
-        if (engine.gamepad2.dpad_left && Peanut != 1) {
-            Peanut = 1;
-        }
-
-        if (engine.gamepad2.dpad_right && Peanut != 2) {
-            Peanut = 2;
-        }
-
-        if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) {
-            robot.collectorLeft.setPower(0);
-            robot.collectorRight.setPower(0);
-        }
-
-        if (Peanut == 1) {
-            robot.collectorRight.setPower(1);
-            robot.collectorLeft.setPower(-1);
-        }
-
-        if (Peanut == 2) {
-            robot.collectorLeft.setPower(1);
-            robot.collectorRight.setPower(-1);
-        }
-
-        if (engine.gamepad2.right_trigger > 0.1) {
-            armPower = engine.gamepad2.right_trigger;
-            robot.ArmMotor.setPower(armPower);
-        } else if (engine.gamepad2.left_trigger > 0.1) {
-            armPower = -(engine.gamepad2.left_trigger);
-            robot.ArmMotor.setPower(armPower);
-        }
-
-    }
-}

TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java

@@ -1,131 +0,0 @@
-package org.timecrafters.TeleOp.states;
-
-import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.cyberarm.engine.V2.GamepadChecker;
-
-public class TeleOPTankDriver extends CyberarmState {
-
-    private final PhoenixBot1 robot;
-    private long lastStepTime = 0;
-    private double drivePower = 0.3;
-    private double RobotRotation;
-    private double currentDriveCommand;
-    private double RotationTarget, DeltaRotation;
-    private double MinimalPower = 0.2;
-    private int DeltaOdometerR, Endeavour, Spirit;
-    private boolean FreeSpirit;
-    private GamepadChecker gamepad1Checker;
-
-    public TeleOPTankDriver(PhoenixBot1 robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addLine("Tank Driver");
-        engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
-        engine.telemetry.addData("Drive Power", drivePower);
-        engine.telemetry.addData("Delta Rotation", DeltaRotation);
-    }
-
-    @Override
-    public void init() {
-        gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
-        FreeSpirit = false;
-    }
-
-    @Override
-    public void exec() {
-
-        if (drivePower > 0.2) {
-            if (System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR < 4096) {
-                lastStepTime = System.currentTimeMillis();
-                FreeSpirit = true;
-            }
-        }
-
-        if (FreeSpirit && System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR < 4096) {
-            getCurrentDriveCommand();
-            drivePower = -currentDriveCommand;
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-        }
-
-        if (FreeSpirit && System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR >= 4096) {
-            FreeSpirit = false;
-        }
-
-        if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && !FreeSpirit) {
-            drivePower = engine.gamepad1.left_stick_y;
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-        }
-
-        if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
-            drivePower = engine.gamepad1.right_stick_x;
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(-drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(-drivePower);
-        }
-
-
-    }
-
-    public void CalculateDeltaRotation() {
-        if (RotationTarget >= 0 && RobotRotation >= 0) {
-            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
-        } else if (RotationTarget <= 0 && RobotRotation <= 0) {
-            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
-        } else if (RotationTarget >= 0 && RobotRotation <= 0) {
-            DeltaRotation = Math.abs(RotationTarget + RobotRotation);
-        } else if (RotationTarget <= 0 && RobotRotation >= 0) {
-            DeltaRotation = Math.abs(RobotRotation + RotationTarget);
-        }
-    }
-
-    public void getDeltaOdometerR() {
-        Spirit = robot.OdometerEncoderRight.getCurrentPosition();
-        if (System.currentTimeMillis() - lastStepTime >= 1000) {
-            lastStepTime = System.currentTimeMillis();
-            DeltaOdometerR = robot.OdometerEncoderRight.getCurrentPosition() - Spirit;
-        }
-    }
-
-    public void getCurrentDriveCommand() {
-        if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
-            currentDriveCommand = engine.gamepad1.left_stick_y;
-        } else if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) {
-            currentDriveCommand = engine.gamepad1.left_stick_x;
-        } else if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
-            currentDriveCommand = engine.gamepad1.right_stick_x;
-        } else if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
-            currentDriveCommand = engine.gamepad1.right_stick_y;
-        } else if ((Math.abs(engine.gamepad1.left_stick_y)) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_y) > 0.1 && !FreeSpirit) {
-            currentDriveCommand = 0;
-        }
-
-    }
-
-    @Override
-    public void buttonUp(Gamepad gamepad, String button) {
-        if (engine.gamepad1 == gamepad && button.equals("start")) {
-            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
-
-            parameters.mode = BNO055IMU.SensorMode.IMU;
-            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
-            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
-            parameters.loggingEnabled = false;
-
-            robot.imu.initialize(parameters);
-        }
-    }
-
-}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java

@@ -43,7 +43,7 @@ public class Robot {
     public final ColorSensor indicatorA, indicatorB;
     public LynxModule expansionHub;
 
-    public final double imuAngleOffset;
+    public final double imuAngleOffset, initialFacing;
     public boolean wristManuallyControlled = false, armManuallyControlled = false;
     public boolean automaticAntiTipActive = false;
     public boolean hardwareFault = false;
@@ -191,6 +191,9 @@ public class Robot {
 
         imu.initialize(parameters);
 
+        // Preserve our "initial" facing, since we transform it from zero.
+        initialFacing = facing();
+
         // BulkRead from Hubs
         for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
             hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
@@ -209,6 +212,10 @@ public class Robot {
         vuforia = initVuforia();
         tfod = initTfod();
 
+        // Drive Encoder Error Setup
+        engine.blackboardSet("left_drive_error", 0);
+        engine.blackboardSet("right_drive_error", 0);
+
         // INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes
         this.fieldLocalizer.setRobot(this);
         this.fieldLocalizer.standardSetup();
@@ -563,6 +570,11 @@ public class Robot {
         return (fmod < 0 ? fmod + 360.0 : fmod +  0.0) - 180;
     }
 
+    public double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
+    }
+
     public Status getStatus() { return status; }
 
     public double getRadius() { return radius; }
@@ -716,6 +728,10 @@ public class Robot {
         arm.setPower(tuningConfig("arm_automatic_power").value());
     }
 
+    public double initialFacing() {
+        return initialFacing;
+    }
+
     public double facing() {
         double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java

@@ -8,8 +8,9 @@ public class Arm extends CyberarmState {
     private final String groupName, actionName;
 
     private final double targetVelocity, timeInMS;
-    private final int tolerance, targetPosition;
-    private final boolean stateDisabled;
+    private final int tolerance, halfTolerance, position, manualPosition;
+    private final boolean positionManually, stateDisabled;
+    private final String positionLookupLabel;
 
     public Arm(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -20,16 +21,27 @@ public class Arm extends CyberarmState {
                 robot.getConfiguration().variable(groupName, actionName, "targetVelocityInDegreesPerSecond").value());
         tolerance = robot.angleToTicks(
                 robot.getConfiguration().variable(groupName, actionName, "toleranceInDegrees").value());
-        targetPosition = robot.angleToTicks(
-                robot.getConfiguration().variable(groupName, actionName, "targetAngle").value());
         timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
 
+        positionLookupLabel = robot.getConfiguration().variable(groupName, actionName, "positionLookupLabel").value();
+        positionManually = robot.getConfiguration().variable(groupName, actionName, "positionManually").value();
+        manualPosition = robot.angleToTicks(
+                robot.getConfiguration().variable(groupName, actionName, "manualTargetAngle").value());
+
         stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+
+        halfTolerance = Math.round(tolerance / 2.0f);
+
+        if (positionManually) {
+            position = manualPosition;
+        } else {
+            position = robot.angleToTicks(robot.tuningConfig(positionLookupLabel).value());
+        }
     }
 
     @Override
     public void start() {
-        robot.arm.setTargetPosition(targetPosition);
+        robot.arm.setTargetPosition(position);
         robot.arm.setTargetPositionTolerance(tolerance);
         robot.arm.setVelocity(targetVelocity);
     }
@@ -49,5 +61,10 @@ public class Arm extends CyberarmState {
 
             return;
         }
+
+        int currentPosition = robot.arm.getCurrentPosition();
+        if (robot.isBetween(currentPosition, currentPosition - halfTolerance, currentPosition + halfTolerance)) {
+            setHasFinished(true);
+        }
     }
 }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java

@@ -1,5 +1,7 @@
 package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
 
+import com.qualcomm.robotcore.util.Range;
+
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
@@ -8,14 +10,19 @@ public class Move extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
 
-    private final double targetDistance, tolerance, facing, targetVelocity, minimumVelocity, timeInMS, easeInDistance, easeOutDistance;
+    private final double tolerance, facing, targetVelocity, minimumVelocity, timeInMS, easeInDistance, easeOutDistance;
+    private final int targetDistance;
 
     private final double maxVelocity;
     private double speed;
 
-    private int distanceAlreadyTravelled;
+    private int leftDistanceAlreadyTravelled, rightDistanceAlreadyTravelled, leftTravelledDistance, rightTravelledDistance;
     private final boolean stateDisabled;
 
+    private double velocity;
+    private double ratio;
+    private boolean stopped = false, correcting = false, correctingLeft = false;
+
     public Move(Robot robot, String groupName, String actionName) {
         this.robot = robot;
         this.groupName = groupName;
@@ -39,7 +46,8 @@ public class Move extends CyberarmState {
     @Override
     public void start() {
         // TODO: Use a dead wheel for this
-        distanceAlreadyTravelled = robot.frontRightDrive.getCurrentPosition();
+        leftDistanceAlreadyTravelled = -robot.frontRightDrive.getCurrentPosition();
+        rightDistanceAlreadyTravelled = -robot.backLeftDrive.getCurrentPosition();
     }
 
     @Override
@@ -58,34 +66,47 @@ public class Move extends CyberarmState {
             return;
         }
 
-        int travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled;
+        leftTravelledDistance = -robot.frontRightDrive.getCurrentPosition() - leftDistanceAlreadyTravelled;
+        rightTravelledDistance = -robot.backLeftDrive.getCurrentPosition() - rightDistanceAlreadyTravelled;
 
-        if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) {
-            stop();
-
-            setHasFinished(true);
-
-            return;
+        if (robot.isBetween(leftTravelledDistance, targetDistance - tolerance, targetDistance + tolerance) ||
+                robot.isBetween(rightTravelledDistance, targetDistance - tolerance, targetDistance + tolerance)) {
+            correctOrientation();
+        } else {
+            moveDirectional();
         }
-
-        moveDirectional(travelledDistance);
     }
 
-    private void moveDirectional(double travelledDistance) {
-        double velocity = targetVelocity;
-        double easeVelocity;
-        double ratio = 1.0;
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Move");
+        engine.telemetry.addData("Ratio", ratio);
+        engine.telemetry.addData("Minimum Velocity", minimumVelocity);
+        engine.telemetry.addData("Target Velocity", targetVelocity);
+        engine.telemetry.addData("Velocity", velocity);
+        engine.telemetry.addData("Target Distance", targetDistance);
+        engine.telemetry.addData("Travelled Distance", leftTravelledDistance);
+        engine.telemetry.addData("Distance Already Travelled", leftDistanceAlreadyTravelled);
+        engine.telemetry.addData("Travel Forward", targetDistance > leftTravelledDistance);
+        engine.telemetry.addData("Left Drive Error", engine.blackboardGetInt("left_drive_error"));
+        engine.telemetry.addData("Right Drive Error", engine.blackboardGetInt("right_drive_error"));
+    }
+
+    private void moveDirectional() {
+        double travelledDistance = leftTravelledDistance;
+        ratio = 1.0;
 
         if (Math.abs(travelledDistance) < easeInDistance) {
-            ratio = travelledDistance / easeInDistance;
-        } else if (Math.abs(travelledDistance) > targetDistance - easeOutDistance) {
-            ratio = (targetDistance - Math.abs(travelledDistance)) / easeOutDistance;
+            ratio = Math.abs(travelledDistance) / easeInDistance;
+        } else if (Math.abs(travelledDistance) > Math.abs(targetDistance) - easeOutDistance) {
+            ratio = (Math.abs(targetDistance) - Math.abs(travelledDistance)) / easeOutDistance;
+        } else {
+            ratio = 1.0;
         }
 
-        easeVelocity = targetVelocity * ratio;
+        ratio = Range.clip(ratio, 0.0, 1.0);
 
-        if (easeVelocity < minimumVelocity) { easeVelocity = minimumVelocity; }
-        if (easeVelocity < targetVelocity) { velocity = easeVelocity; }
+        velocity = robot.lerp(minimumVelocity, targetVelocity, ratio);
 
         if (targetDistance > travelledDistance) {
             robot.frontRightDrive.setVelocity(-velocity);
@@ -142,6 +163,42 @@ public class Move extends CyberarmState {
         robot.backRightDrive.setVelocity(backRightPower * targetVelocity);
     }
 
+    private void correctOrientation() {
+        if (!stopped && Math.abs(robot.backLeftDrive.getVelocity()) > 0 && Math.abs(robot.frontRightDrive.getVelocity()) > 0) {
+            stop();
+        } else {
+            stopped = true;
+
+            if (!correcting) {
+                correcting = true;
+
+                if (leftTravelledDistance > rightTravelledDistance) {
+                    correctingLeft = true;
+                } else if (leftTravelledDistance < rightTravelledDistance) {
+                    correctingLeft = false;
+                }
+            }
+
+            if (correctingLeft) {
+                robot.frontRightDrive.setVelocity(robot.unitToTicks(DistanceUnit.INCH, 1));
+
+                if (leftTravelledDistance <= rightTravelledDistance) {
+                    stop();
+
+                    setHasFinished(true);
+                }
+            } else {
+                robot.backLeftDrive.setVelocity(robot.unitToTicks(DistanceUnit.INCH, 1));
+
+                if (leftTravelledDistance >= rightTravelledDistance) {
+                    stop();
+
+                    setHasFinished(true);
+                }
+            }
+        }
+    }
+
     @Override
     public void stop() {
         robot.backLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
@@ -149,5 +206,8 @@ public class Move extends CyberarmState {
 
         robot.frontLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0);
         robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0);
+
+        engine.blackboardSet("left_drive_error", (int)(targetDistance - leftTravelledDistance));
+        engine.blackboardSet("right_drive_error", (int)(targetDistance - rightTravelledDistance));
     }
 }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java

@@ -32,7 +32,7 @@ public class Rotate extends CyberarmState {
 
         stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
 
-        facing = (robot.facing() + targetFacing + 360.0) % 360.0;
+        facing = (robot.initialFacing() + targetFacing + 360.0) % 360.0;
 
         velocity = targetVelocity;
     }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java

@@ -14,7 +14,7 @@ public class SignalProcessor extends CyberarmState {
     private final int fallbackPosition;
     private final boolean stateDisabled;
 
-    private List<Recognition> updatedRecognitions;
+    private List<Recognition> updatedRecognitions = null;
 
     public SignalProcessor(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -54,7 +54,15 @@ public class SignalProcessor extends CyberarmState {
             // the last time that call was made.
             List<Recognition> recognitions = robot.getTfod().getUpdatedRecognitions();
 
+            boolean secondPass = false;
             if (recognitions != null) {
+                /* Since recognitions will be null unless there has been a change and updatedRecognitions
+                   will be null unless we have had a first pass of recognitions we can safely assume that
+                   we are on our second pass. */
+                if (updatedRecognitions != null) {
+                    secondPass = true;
+                }
+
                 updatedRecognitions = recognitions;
             }
 
@@ -76,6 +84,12 @@ public class SignalProcessor extends CyberarmState {
                     }
                 }
             }
+
+            /* Attempt to speed up this state by assuming that the second pass of recognitions is
+               good enough and finish the state */
+            if (secondPass) {
+                setHasFinished(true);
+            }
         }
     }
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wait.java

@@ -0,0 +1,39 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+
+public class Wait extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+
+    private final double timeInMS;
+    private final boolean stateDisabled;
+
+    public Wait(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
+
+        stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+    }
+
+    @Override
+    public void exec() {
+        if (stateDisabled) {
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java

@@ -164,6 +164,12 @@ public class ArmDriverControl extends CyberarmState {
         // Swap controlling gamepad
         if (gamepad == engine.gamepad2 && button.equals("guide")) {
             controller = controller == engine.gamepad1 ? engine.gamepad2 : engine.gamepad1;
+
+            if (controller == engine.gamepad1) {
+                engine.telemetry.speak("Pilot Only");
+            } else {
+                engine.telemetry.speak("Co-Pilot Enabled");
+            }
         }
 
         if (gamepad != controller) {

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java

@@ -10,6 +10,7 @@ import org.timecrafters.minibots.cyberarm.chiron.Robot;
 public class DrivetrainDriverControl extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
+    private final double robotCentricRotation;
 
     private Gamepad controller;
 
@@ -23,6 +24,8 @@ public class DrivetrainDriverControl extends CyberarmState {
         this.actionName = actionName;
 
         this.controller = engine.gamepad1;
+
+        this.robotCentricRotation = robot.tuningConfig("robot_centric_rotation").value();
     }
 
     @Override
@@ -50,6 +53,13 @@ public class DrivetrainDriverControl extends CyberarmState {
         double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
         double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x);
 
+        if (!fieldCentricControl) {
+            Vector2d v = new Vector2d(x, y).rotated(robotCentricRotation);
+
+            x = v.getX();
+            y = v.getY();
+        }
+
         // Improve control?
         if (y < 0) {
             y = -Math.sqrt(-y);
@@ -160,6 +170,10 @@ public class DrivetrainDriverControl extends CyberarmState {
         if (button.equals("right_stick_button")) {
             robotSlowMode = !robotSlowMode;
         }
+
+        if (button.equals("left_stick_button") && robot.hardwareFault) {
+            robot.imu.resetYaw();
+        }
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java

@@ -0,0 +1,86 @@
+package org.timecrafters.minibots.cyberarm.phoenix;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class PositionalServoController {
+    final private Servo servo;
+    final private double targetDegreesPerSecond, servoDegreesPerSecond, servoRangeInDegrees;
+
+    private double lastEstimatedPosition, estimatedPosition, workingPosition, targetPosition;
+    private long lastUpdatedAt;
+    private boolean travelling = false;
+
+    public PositionalServoController(Servo servo, double targetDegreesPerSecond, double servoDegreesPerSecond, double servoRangeInDegrees) {
+        this.servo = servo;
+        this.targetDegreesPerSecond = targetDegreesPerSecond;
+        this.servoDegreesPerSecond = servoDegreesPerSecond;
+        this.servoRangeInDegrees = servoRangeInDegrees;
+
+        this.workingPosition = servo.getPosition();
+        this.estimatedPosition = workingPosition;
+        this.lastEstimatedPosition = estimatedPosition;
+        this.lastUpdatedAt = System.currentTimeMillis();
+    }
+
+    public Servo getServo() {
+        return servo;
+    }
+
+    public void update() {
+        double error = targetPosition - estimatedPosition;
+        double delta = (System.currentTimeMillis() - lastUpdatedAt) / 1000.0;
+        double estimatedTravel = servoDegreesPerSecond * delta;
+        double targetTravel = targetDegreesPerSecond * delta;
+
+        if (targetTravel < estimatedTravel) {
+            estimatedTravel = targetTravel;
+        }
+
+        if (travelling) {
+            this.lastEstimatedPosition = estimatedPosition;
+
+            if (error < 0.0) {
+                this.estimatedPosition -= estimatedTravel;
+            } else {
+                this.estimatedPosition += estimatedTravel;
+            }
+        }
+
+        if (error < 0.0 - estimatedTravel) {
+            workingPosition -= estimatedTravel;
+            travelling = true;
+        } else if (error > 0.0 + estimatedTravel) {
+            workingPosition += estimatedTravel;
+            travelling = true;
+        } else {
+            workingPosition = targetPosition;
+            travelling = false;
+        }
+
+        servo.setPosition(workingPosition);
+        this.lastUpdatedAt = System.currentTimeMillis();
+    }
+
+    public void setTargetPosition(double targetPosition) {
+        this.targetPosition = targetPosition;
+    }
+
+    public double getEstimatedPosition() {
+        return estimatedPosition;
+    }
+
+    public double getWorkingPosition() { return workingPosition; }
+
+    public double getTargetPosition() { return targetPosition; }
+
+    public double getEstimatedAngle() {
+        return estimatedPosition * servoRangeInDegrees;
+    }
+
+    public double getError() { return targetPosition - estimatedPosition; }
+
+    private double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java

@@ -0,0 +1,724 @@
+package org.timecrafters.minibots.cyberarm.phoenix;
+
+import android.annotation.SuppressLint;
+import android.util.Log;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.Blinker;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.firstinspires.ftc.robotcore.external.ClassFactory;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+
+import java.util.ArrayList;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.CopyOnWriteArrayList;
+import java.util.concurrent.TimeUnit;
+
+public class Robot {
+    private static final String TAG = "Phoenix | Robot";
+    public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
+    public final PositionalServoController leftRiserServoController, rightRiserServoController;
+    public final Servo cameraServo;
+    public final CRServo collectorLeftServo, collectorRightServo;
+//    public final ServoImplEx gripper;
+    public final IMU imu;
+    public LynxModule expansionHub;
+
+    public final double imuAngleOffset, initialFacing;
+    public boolean armManuallyControlled = false;
+    public boolean automaticAntiTipActive = false;
+    public boolean hardwareFault = false;
+    public String hardwareFaultMessage = "";
+
+    private Status status = Status.OKAY, lastStatus = Status.OKAY;
+    private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
+    private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
+    private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
+    private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
+    private ArmPosition armTargetPosition;
+
+    public enum ArmPosition {
+        COLLECT,
+        GROUND,
+        LOW,
+        MEDIUM,
+        HIGH
+
+    }
+
+    public enum Status {
+        OKAY,
+        MONITORING,
+        WARNING,
+        DANGER
+    }
+
+    private final CyberarmEngine engine;
+    private TimeCraftersConfiguration configuration;
+    private final double radius, diameter;
+
+    private final double wheelRadius, wheelGearRatio, armGearRatio;
+    private final int wheelTicksPerRevolution, armTicksPerRevolution;
+
+    private final VuforiaLocalizer vuforia;
+    private final TFObjectDetector tfod;
+
+    private boolean LEDStatusToggle = false;
+    private double lastLEDStatusAnimationTime = 0;
+
+    public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) {
+        this.engine = engine;
+        this.configuration = configuration;
+
+        radius = tuningConfig("field_localizer_robot_radius").value();
+        diameter = radius * 2;
+
+        imuAngleOffset = hardwareConfig("imu_angle_offset").value();
+
+        wheelRadius = tuningConfig("wheel_radius").value();
+        wheelGearRatio = tuningConfig("wheel_gear_ratio").value();
+        wheelTicksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
+
+        armGearRatio = tuningConfig("arm_gear_ratio").value();
+        armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
+
+        // FIXME: Rename motors in configuration
+        // Define hardware
+        backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Left");     // MOTOR PORT: 2 - CONTROL HUB
+        frontRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Right"); // MOTOR PORT: 1 - CONTROL HUB
+
+        frontLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Left");   // MOTOR PORT: 3 - CONTROL HUB
+        backRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Right");   // MOTOR PORT: 0 - CONTROL HUB
+
+        // FIXME: Rename lift_drive to arm in hardware config
+        arm = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor");          // MOTOR PORT: 2 - EXPANSION HUB
+
+//        gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper");       // SERVO PORT: ?
+
+        imu = engine.hardwareMap.get(IMU.class, "imu");
+
+
+        // Configure hardware
+        //   MOTORS
+        //      DIRECTION
+        frontLeftDrive.setDirection(hardwareConfig("front_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+        frontRightDrive.setDirection(hardwareConfig("front_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        backLeftDrive.setDirection(hardwareConfig("back_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+        backRightDrive.setDirection(hardwareConfig("back_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        arm.setDirection(hardwareConfig("arm_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        //      RUNMODE
+        frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        arm.setTargetPosition(0);
+        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        armTargetPosition = ArmPosition.COLLECT;
+
+        //      ZERO POWER BEHAVIOR
+        frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        //      MOTOR POWER
+        arm.setPower(tuningConfig("arm_automatic_power").value());
+
+        //   SERVOS
+        Servo leftRiser = engine.hardwareMap.servo.get("LowRiserLeft");
+        leftRiser.setDirection(Servo.Direction.FORWARD);
+        Servo rightRiser = engine.hardwareMap.servo.get("LowRiserRight");
+        rightRiser.setDirection(Servo.Direction.REVERSE);
+
+        //      NOTES: 428.57142858° per second = no load speed of 60° per 0.14s at 6V, see: https://docs.revrobotics.com/duo-build/actuators/servos/smart-robot-servo#mechanical-specifications
+        leftRiserServoController = new PositionalServoController(
+                leftRiser,
+                5.0,
+                428.57142858,
+                270.0
+        ); // SERVER PORT: ? - ? HUB
+
+        rightRiserServoController = new PositionalServoController(
+                rightRiser,
+                5.0,
+                428.57142858,
+                270.0
+        ); // SERVER PORT: ? - ? HUB
+
+        cameraServo = engine.hardwareMap.servo.get("Camera Servo"); // SERVER PORT: ? - ? HUB
+
+        collectorLeftServo = engine.hardwareMap.crservo.get("Collector Left");   // SERVER PORT: ? - ? HUB
+        collectorRightServo = engine.hardwareMap.crservo.get("Collector Right"); // SERVER PORT: ? - ? HUB
+
+        //      SERVO DIRECTIONS
+        cameraServo.setDirection(Servo.Direction.REVERSE);
+        collectorLeftServo.setDirection(DcMotorSimple.Direction.REVERSE);
+        collectorRightServo.setDirection(DcMotorSimple.Direction.FORWARD);
+
+        //  SENSORS
+        //      IMU
+        IMU.Parameters parameters = new IMU.Parameters(
+            new RevHubOrientationOnRobot(
+                RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+                RevHubOrientationOnRobot.UsbFacingDirection.UP
+            )
+        );
+
+        imu.initialize(parameters);
+
+        // Preserve our "initial" facing, since we transform it from zero.
+        initialFacing = facing();
+
+        // BulkRead from Hubs
+        for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+
+            if (!hub.isParent() && expansionHub == null) {
+                expansionHub = hub;
+            }
+        }
+
+        // Set LED pattern
+        if (expansionHub != null) {
+            expansionHub.setPattern(ledPatternOkay());
+        }
+
+        // Webcam
+        vuforia = initVuforia();
+        tfod = initTfod();
+
+        // Drive Encoder Error Setup
+        engine.blackboardSet("left_drive_error", 0);
+        engine.blackboardSet("right_drive_error", 0);
+    }
+
+    private VuforiaLocalizer initVuforia() {
+        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
+
+        parameters.vuforiaLicenseKey = hardwareConfig("vuforia_license_key").value();
+        parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
+
+         return ClassFactory.getInstance().createVuforia(parameters);
+    }
+
+    private TFObjectDetector initTfod() {
+        int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
+        tfodParameters.minResultConfidence = 0.75f;
+        tfodParameters.isModelTensorFlow2 = true;
+        tfodParameters.inputSize = 300;
+        TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
+
+        tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel");
+
+        return tfod;
+    }
+
+    public void reloadConfig() {
+        String name = configuration.getConfig().getName();
+        configuration = new TimeCraftersConfiguration(name);
+    }
+
+    public void standardTelemetry() {
+        engine.telemetry.addLine();
+
+        // STATUS
+        engine.telemetry.addLine("DATA");
+        engine.telemetry.addData("      Robot Status", status);
+        engine.telemetry.addData("      Hardware Fault", hardwareFault);
+        engine.telemetry.addData("      Hardware Fault Message", hardwareFaultMessage);
+        engine.telemetry.addLine();
+
+        // Motor Powers
+        engine.telemetry.addLine("Motor Powers");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getPower());
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getPower());
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getPower());
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getPower());
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getPower());
+
+        engine.telemetry.addLine();
+
+        // Motor Positions
+        engine.telemetry.addLine("Motor Positions");
+        engine.telemetry.addData("      Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getCurrentPosition()));
+        engine.telemetry.addData("      Front Right Drive", "%d (%8.2f in)", frontRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getCurrentPosition()));
+
+        engine.telemetry.addData("      Back Left Drive", "%d (%8.2f in)", backLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getCurrentPosition()));
+        engine.telemetry.addData("      Back Right Drive", "%d (%8.2f in)", backRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getCurrentPosition()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%d (%8.2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition()));
+
+        engine.telemetry.addLine();
+
+        // Motor Target Positions
+        engine.telemetry.addLine("Motor Target Positions");
+        engine.telemetry.addData("      Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getTargetPosition()));
+        engine.telemetry.addData("      Front Right Drive", "%d (%8.2f in)", frontRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getTargetPosition()));
+
+        engine.telemetry.addData("      Back Left Drive", "%d (%8.2f in)", backLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getTargetPosition()));
+        engine.telemetry.addData("      Back Right Drive", "%d (%8.2f in)", backRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getTargetPosition()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
+
+        engine.telemetry.addLine();
+
+        // Motor Velocity
+        engine.telemetry.addLine("Motor Velocity");
+        engine.telemetry.addData("      Front Left Drive", "%8.2f (%8.2f in)", frontLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontLeftDrive.getVelocity()));
+        engine.telemetry.addData("      Front Right Drive", "%8.2f (%8.2f in)", frontRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontRightDrive.getVelocity()));
+
+        engine.telemetry.addData("      Back Left Drive", "%8.2f (%8.2f in)", backLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backLeftDrive.getVelocity()));
+        engine.telemetry.addData("      Back Right Drive", "%8.2f (%8.2f in)", backRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backRightDrive.getVelocity()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%8.2f (%8.2f degrees)", arm.getVelocity(), ticksToAngle((int)arm.getVelocity()));
+
+        engine.telemetry.addLine();
+
+        // Motor Currents
+        engine.telemetry.addLine("Motor Currents (AMPS)");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getCurrent(CurrentUnit.AMPS));
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getCurrent(CurrentUnit.AMPS));
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addLine();
+
+        // Motor Directions
+        engine.telemetry.addLine("Motor Directions");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getDirection());
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getDirection());
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getDirection());
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getDirection());
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getDirection());
+
+        engine.telemetry.addLine();
+
+        // Servo Positions
+        engine.telemetry.addLine("Servo Positions");
+        engine.telemetry.addData("      Left Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", leftRiserServoController.getEstimatedPosition(), leftRiserServoController.getError(), leftRiserServoController.getTargetPosition(), leftRiserServoController.getServo().getPosition());
+        engine.telemetry.addData("      Right Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", rightRiserServoController.getEstimatedPosition(), rightRiserServoController.getError(), rightRiserServoController.getTargetPosition(), rightRiserServoController.getServo().getPosition());
+        engine.telemetry.addData("      Camera (Blind)", cameraServo.getPosition());
+
+        engine.telemetry.addLine();
+
+        // Continuous Servo Powers
+        engine.telemetry.addLine("Servo Powers");
+        engine.telemetry.addData("      Collector Left", collectorLeftServo.getPower());
+        engine.telemetry.addData("      Collector Right", collectorRightServo.getPower());
+
+        engine.telemetry.addLine();
+
+        // Servo Directions
+        engine.telemetry.addLine("Servo Directions");
+        engine.telemetry.addData("      Left Riser", leftRiserServoController.getServo().getDirection());
+        engine.telemetry.addData("      Right Riser", rightRiserServoController.getServo().getDirection());
+        engine.telemetry.addData("      Camera", cameraServo.getDirection());
+        engine.telemetry.addData("      Collector Left", collectorLeftServo.getDirection());
+        engine.telemetry.addData("      Collector Right", collectorRightServo.getDirection());
+
+        engine.telemetry.addLine();
+
+        // Sensors / IMU
+        engine.telemetry.addLine("IMU");
+        engine.telemetry.addData("      Facing (Degrees)", facing());
+        engine.telemetry.addData("      Heading (Radians)", heading());
+        engine.telemetry.addData("      Turn Rate", turnRate());
+        engine.telemetry.addData("      Angle Offset (Degrees)", imuAngleOffset);
+
+        engine.telemetry.addLine();
+    }
+
+    private ArrayList<Blinker.Step> ledPatternStandby() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0x008000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x005000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x002000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x001000, 250, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternOkay() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0x00aa00, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x00aa44, 500, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternMonitoring() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xaaaaaa, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x000000, 250, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternWarning() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xffff00, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0xff8800, 500, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternDanger() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xff0000, 250, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x000000, 100, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    public void reportStatus(Status status) {
+        reportedStatuses.add(status);
+    }
+
+    public void update() {
+        for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
+            hub.clearBulkCache();
+        }
+
+        double voltage = getVoltage();
+        if (voltage < 9.75) {
+            reportStatus(Status.DANGER);
+            hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)";
+
+            hardwareFault = true;
+        }
+
+        status = Status.OKAY;
+        for (Status s : reportedStatuses) {
+            if (s.ordinal() > status.ordinal()) {
+                status = s;
+            }
+        }
+
+        reportedStatuses.clear();
+
+        if (status != lastStatus) {
+            lastStatus = status;
+
+            if (expansionHub != null) {
+                if (lastStatus == Status.OKAY) { expansionHub.setPattern(ledPatternOkay()); }
+                if (lastStatus == Status.MONITORING) { expansionHub.setPattern(ledPatternMonitoring()); }
+                if (lastStatus == Status.WARNING) { expansionHub.setPattern(ledPatternWarning()); }
+                if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
+            }
+        }
+
+        manageArmAndRiserServos();
+    }
+
+    public void stop() {
+        if (expansionHub != null) {
+            expansionHub.setPattern(ledPatternStandby());
+        }
+    }
+
+    public void armPosition(ArmPosition position) {
+        if (hardwareFault) {
+            return;
+        }
+
+        armTargetPosition = position;
+
+        reportStatus(Status.WARNING);
+    }
+
+    private void manageArmAndRiserServos() {
+        boolean lowerServos = true;
+
+        switch (armTargetPosition) {
+            case COLLECT:
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
+                }
+                break;
+
+//            case GROUND:
+//                if (areRiserServosInLoweredPosition()) {
+//                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
+//                }
+//                break;
+
+            case GROUND: // DISABLE GROUND JUNCTION SUPPORT FOR NOW
+            case LOW:
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
+                }
+                break;
+
+            case MEDIUM:
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
+                }
+                break;
+
+            case HIGH:
+                double angleHigh = tuningConfig("arm_position_angle_high").value();
+                arm.setTargetPosition(angleToTicks(angleHigh));
+
+                if (arm.getCurrentPosition() >= angleToTicks(angleHigh - 10.0)) {
+                    lowerServos = false;
+                }
+                break;
+
+            default:
+                throw new RuntimeException("Unexpected arm position!");
+        }
+
+        if (lowerServos) {
+            leftRiserServoController.setTargetPosition(0.45);
+            rightRiserServoController.setTargetPosition(0.45);
+        } else {
+            leftRiserServoController.setTargetPosition(0.8);
+            rightRiserServoController.setTargetPosition(0.8);
+        }
+
+        leftRiserServoController.update();
+        rightRiserServoController.update();
+    }
+
+    private boolean areRiserServosInLoweredPosition() {
+        return leftRiserServoController.getEstimatedPosition() < 0.5 && rightRiserServoController.getEstimatedPosition() < 0.5;
+    }
+
+    // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
+    public double angleDiff(double from, double to) {
+        double value = (to - from + 180);
+
+        double fmod = (value - 0.0) % (360.0 - 0.0);
+
+        return (fmod < 0 ? fmod + 360.0 : fmod +  0.0) - 180;
+    }
+
+    public double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
+    }
+
+    public Status getStatus() { return status; }
+
+    public double getRadius() { return radius; }
+
+    public double getDiameter() { return diameter; }
+
+    public double getVoltage() {
+        return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
+    }
+
+    public TFObjectDetector getTfod() { return tfod; }
+
+    public VuforiaLocalizer getVuforia() { return vuforia; }
+
+    public TimeCraftersConfiguration getConfiguration() { return configuration; }
+
+    // For: Drive Wheels
+    public int unitToTicks(DistanceUnit unit, double distance) {
+        double fI = (wheelGearRatio * wheelTicksPerRevolution) / (wheelRadius * 2 * Math.PI * (wheelGearRatio * wheelTicksPerRevolution) / (wheelGearRatio * wheelTicksPerRevolution));
+
+        double inches = unit.toInches(unit.fromUnit(unit, distance));
+
+        double ticks = fI * inches;
+
+        return (int)ticks;
+    }
+
+    // For: Drive Wheels
+    public double ticksToUnit(DistanceUnit unit, int ticks) {
+        // Convert to inches, then to unit.
+        double inches = wheelRadius * 2 * Math.PI * ticks / (wheelGearRatio * wheelTicksPerRevolution);
+
+        return unit.fromUnit(DistanceUnit.INCH, inches);
+    }
+
+    // For: Arm
+    public int angleToTicks(double angle) {
+        double d = (armGearRatio * armTicksPerRevolution) / 360.0;
+
+        // Casting to float so that the int version of Math.round is used.
+        return Math.round((float)d * (float)angle);
+    }
+
+    // For: Arm
+    public double ticksToAngle(int ticks) {
+        double oneDegree = 360.0 / (armGearRatio * armTicksPerRevolution);
+
+        return oneDegree * ticks;
+    }
+
+    public Variable hardwareConfig(String variableName) {
+        Action hardwareConfiguration = configuration.action("Robot", "Hardware");
+
+        for (Variable v : hardwareConfiguration.getVariables()) {
+            if (variableName.trim().equals(v.name)) {
+                return v;
+            }
+        }
+
+        throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Hardware");
+    }
+
+    public Variable tuningConfig(String variableName) {
+        Action action = configuration.action("Robot", "Tuning");
+
+        for (Variable v : action.getVariables()) {
+            if (variableName.trim().equals(v.name)) {
+                return v;
+            }
+        }
+
+        throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Tuning");
+    }
+
+    @SuppressLint("NewApi")
+    public void controlMotorPIDF(DcMotorEx motor, String motorName, double targetVelocity, double feedForward) {
+        Action action = configuration.action("Robot", "Tuning_PIDF_" + motorName);
+        double proportional = 0, integral = 0, derivative = 0;
+
+        for (Variable v : action.getVariables()) {
+            switch (v.name.trim()) {
+                case "proportional":
+                    proportional = v.value();
+                    break;
+                case "integral":
+                    integral = v.value();
+                    break;
+                case "derivative":
+                    derivative = v.value();
+                    break;
+            }
+        }
+
+        double interval = (engine.getRuntime() - motorVelocityLastTiming.getOrDefault(motorName, 0.0));
+
+        double distance = motor.getTargetPosition() - motor.getCurrentPosition();
+
+        if (Math.abs(distance) < Math.abs(targetVelocity)) {
+            if ((targetVelocity < 0 && distance > 0) || (targetVelocity > 0 && distance < 0)) {
+                targetVelocity = -distance;
+            } else {
+                targetVelocity = distance;
+            }
+        }
+
+        double error = targetVelocity - motor.getVelocity();
+        double deltaError = error - motorVelocityError.getOrDefault(motorName, 0.0);
+
+        double kIntegral = error * interval;
+        double kDerivative = deltaError / interval;
+
+        double kp = proportional * error;
+        double ki = integral * kIntegral;
+        double kd = derivative * kDerivative;
+
+        motorVelocityError.put(motorName, error);
+        motorVelocityLastTiming.put(motorName, engine.getRuntime());
+
+        double newTargetVelocity = kp + ki + kd + targetVelocity;
+
+        motor.setVelocity(newTargetVelocity);
+
+        Log.d(TAG, "Interval: " + interval + "ms, Error: " + error + " ticks, deltaError: " + deltaError + " ticks, distance: " +
+                distance + " ticks, kIntegral: " + kIntegral + ", kDerivative: " + kDerivative + ", proportional: " + proportional +
+                ", integral: " + integral + ", derivative: " + derivative + ", kp: " + kp + ", ki: " + ki + ", kd: " + kd +
+                ", targetVelocity: " + targetVelocity + " ticks, new Target Velocity: " + newTargetVelocity + " ticks, + motorVelocity: " + motor.getVelocity() + " ticks.");
+    }
+
+    @SuppressLint("NewApi")
+    public void controlArmMotor(double targetVelocity) {
+//        double time = System.currentTimeMillis();
+//        double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity);
+//        double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
+//        double deltaTime = (time - lastTiming) * 0.001;
+//
+//        double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
+//        double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
+//
+//        double error = adjustedTargetVelocity - arm.getVelocity();
+//        double kp = 0.9;
+//
+//        newTargetVelocity += error * kp * deltaTime;
+//
+//        motorTargetVelocity.put("Arm", newTargetVelocity);
+//        motorVelocityLastTiming.put("Arm", time);
+
+//        arm.setVelocity(newTargetVelocity);
+
+        arm.setPower(tuningConfig("arm_automatic_power").value());
+    }
+
+    public double initialFacing() {
+        return initialFacing;
+    }
+
+    public double facing() {
+        double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+        return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
+    }
+
+    public double heading() {
+        return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
+//        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+    }
+
+    public double turnRate() {
+        return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
+    }
+
+    public boolean isBetween(double value, double min, double max) {
+        return value >= min && value <= max;
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java

@@ -0,0 +1,47 @@
+package org.timecrafters.minibots.cyberarm.phoenix.engines;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.minibots.cyberarm.phoenix.Robot;
+
+@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Mentor Phoenix | TeleOp", group = "Mentor Phoenix")
+public class TeleOp extends CyberarmEngine {
+    private Robot robot;
+    private TimeCraftersConfiguration configuration;
+    final private String configurationName = "MentorPhoenix", actionsGroupName = "TeleOpStates";
+
+    @Override
+    public void setup() {
+        configuration = new TimeCraftersConfiguration(configurationName);
+
+        robot = new Robot(
+                this,
+                configuration
+        );
+
+        robot.imu.resetYaw();
+
+        setupFromConfig(
+                robot.getConfiguration(),
+                "org.timecrafters.minibots.cyberarm.phoenix.states.teleop",
+                robot,
+                Robot.class,
+                actionsGroupName);
+    }
+
+    @Override
+    public void loop() {
+        robot.update();
+
+        super.loop();
+
+        robot.standardTelemetry();
+    }
+
+    @Override
+    public void stop() {
+        robot.stop();
+
+        super.stop();
+    }
+}