diff --git a/build.gradle b/build.gradle
index 3ad153a..e04580c 100644
--- a/build.gradle
+++ b/build.gradle
@@ -4,7 +4,6 @@ plugins {
     id "cpp"
     id "google-test-test-suite"
     id "edu.wpi.first.GradleRIO" version "2022.4.1"
-    id "org.ysb33r.doxygen" version "0.7.0"
 }
 
 // For using remote development artifacts
@@ -205,29 +204,4 @@ task testRelease {
 task intellisense(type: Exec) {
     dependsOn 'generateCompileCommands'
     commandLine 'python3', './buildscripts/copy_compile_commands.py'
-}
-
-doxygen {
-    executables {
-        doxygen version : '1.9.2',
-        baseURI : 'https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen'
-    }
-
-    exclude 'frc/*'
-    exclude 'photonlib/*'
-
-    extension_mapping 'inc=C++'
-    generate_html true
-    generate_latex false
-    generate_treeview true
-    html_extra_stylesheet 'docs/theme.css'
-    html_timestamp true
-    quiet true
-    project_name 'Robot-2022'
-    use_mathjax true
-    warn_as_error 'FAIL_ON_WARNINGS'
-    warn_if_undocumented true
-
-    source 'src/main/include'
-    outputDir 'build/docs/'
-}
+}
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index e016598..cfab10f 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -69,12 +69,6 @@ Robot::Robot() : frc::TimesliceRobot{2_ms, Constants::kControllerPeriod} {
     // dashboard plots
     SetNetworkTablesFlushEnabled(true);
 
-    // AUTONOMOUS MODES
-    m_autonChooser.AddAutonomous("Drive Backwards", [=] { AutoBackwards(); });
-    m_autonChooser.AddAutonomous("Shoot One", [=] { AutoShootOne(); });
-    m_autonChooser.AddAutonomous("Shoot Two", [=] { AutoShootTwo(); });
-    m_autonChooser.AddAutonomous("Shoot Three", [=] { AutoShootThree(); });
-    m_autonChooser.AddAutonomous("Shoot Four", [=] { AutoShootFour(); });
 
     // TIMESLICE ALLOCATION TABLE
     //
@@ -113,10 +107,6 @@ Robot::Robot() : frc::TimesliceRobot{2_ms, Constants::kControllerPeriod} {
     if constexpr (!IsSimulation()) {
         frc::CameraServer::StartAutomaticCapture().SetResolution(160, 120);
     }
-
-    vision.SubscribeToVisionData(drivetrain.visionQueue);
-    vision.SubscribeToVisionData(backFlywheel.visionQueue);
-    vision.SubscribeToVisionData(frontFlywheel.visionQueue);
 }
 
 Robot::~Robot() {}
@@ -197,16 +187,16 @@ void Robot::TeleopPeriodic() {
         }
     } else {
         if (driveStick2.GetRawButtonPressed(1)) {
-            Shoot(FrontFlywheelConstants::kShootHighTarmac,
-                  BackFlywheelConstants::kShootHighTarmac, true);
+            Shoot(FrontFlywheelConstants::kShootHighFender,
+                  BackFlywheelConstants::kShootHighFender);
         }
         if (driveStick1.GetRawButtonPressed(1)) {
             Shoot(FrontFlywheelConstants::kShootHighTarmac,
                   BackFlywheelConstants::kShootHighTarmac);
         }
         if (driveStick1.GetRawButtonPressed(4)) {
-            Shoot(FrontFlywheelConstants::kShootHighTarmac,
-                  BackFlywheelConstants::kShootHighTarmac, false, true);
+            Shoot(FrontFlywheelConstants::kShootLow,
+                  BackFlywheelConstants::kShootLow);
         }
     }
 
@@ -275,8 +265,7 @@ void Robot::ExpectAutonomousEndConds() {
 bool Robot::IsShooting() const { return m_state != ShootingState::kIdle; }
 
 void Robot::Shoot(units::radians_per_second_t frontSpeed,
-                  units::radians_per_second_t backSpeed, bool visionAim,
-                  bool shootWithRange) {
+                  units::radians_per_second_t backSpeed) {
     if (m_state == ShootingState::kIdle) {
         frontFlywheel.SetGoal(frontSpeed);
         backFlywheel.SetGoal(backSpeed);
@@ -284,15 +273,8 @@ void Robot::Shoot(units::radians_per_second_t frontSpeed,
         m_shootTimer.Reset();
         m_shootTimer.Stop();
 
-        m_visionAim = visionAim;
-        m_shootWithRange = shootWithRange;
-        if (!m_visionAim && m_shootWithRange) {
-            m_state = ShootingState::kVisionSpinUp;
-        } else if (m_visionAim && !m_shootWithRange) {
-            m_state = ShootingState::kVisionAim;
-        } else {
-            m_state = ShootingState::kSpinUp;
-        }
+
+        m_state = ShootingState::kSpinUp;
     }
 }
 
@@ -303,22 +285,6 @@ void Robot::RunShooterSM() {
     switch (m_state) {
         case ShootingState::kIdle:
             break;
-        case ShootingState::kVisionAim:
-            drivetrain.AimWithVision();
-            m_state = ShootingState::kVisionSpinUp;
-            break;
-        case ShootingState::kVisionSpinUp:
-            if constexpr (IsSimulation()) {
-                m_state = ShootingState::kSpinUp;
-            } else {
-                if (vision.HaveTargets() ||
-                    (m_shootWithRange && ReadyToShoot())) {
-                    frontFlywheel.SetGoalFromRange(true);
-                    backFlywheel.SetGoalFromRange(true);
-                    m_state = ShootingState::kSpinUp;
-                }
-            }
-            break;
         case ShootingState::kSpinUp:
             if (frontFlywheel.IsReady() && backFlywheel.IsReady()) {
                 if ((m_visionAim && drivetrain.AtVisionTarget()) ||
diff --git a/src/main/cpp/autonomous/AutoBackwards.cpp b/src/main/cpp/autonomous/AutoBackwards.cpp
deleted file mode 100644
index 2411c28..0000000
--- a/src/main/cpp/autonomous/AutoBackwards.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-// Copyright (c) FRC Team 3512. All Rights Reserved.
-
-#include "Robot.hpp"
-
-namespace frc3512 {
-
-void Robot::AutoBackwards() {
-    // Initial Pose - Right in line with the Loading Zone
-    const frc::Pose2d kInitialPose{8.44_m, 5.25_m, 0_rad};
-    // End Pose - Drive forward slightly
-    const frc::Pose2d kEndPose{8.44_m - 2.5 * Drivetrain::kLength, 5.25_m,
-                               0_rad};
-
-    auto config = DrivetrainController::MakeTrajectoryConfig();
-    config.SetReversed(true);
-
-    drivetrain.Reset(kInitialPose);
-    drivetrain.AddTrajectory(kInitialPose, {}, kEndPose, config);
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtGoal(); })) {
-        return;
-    }
-}
-
-}  // namespace frc3512
diff --git a/src/main/cpp/autonomous/AutoShootFour.cpp b/src/main/cpp/autonomous/AutoShootFour.cpp
deleted file mode 100644
index d1d8015..0000000
--- a/src/main/cpp/autonomous/AutoShootFour.cpp
+++ /dev/null
@@ -1,125 +0,0 @@
-// Copyright (c) FRC Team 3512. All Rights Reserved.
-
-#include "Robot.hpp"
-
-namespace frc3512 {
-
-void Robot::AutoShootFour() {
-    // Initial Pose - Right in front of alliance colored ball. The robot starts
-    // pre-loaded with one ball.
-    const frc::Pose2d kInitialPose{8.598_m, 6.081_m,
-                                   units::radian_t{wpi::numbers::pi / 2}};
-
-    // Intake Pose - Just in front of the initial position. Right on top of the
-    // colored ball.
-    frc::Pose2d kIntakePose{8.598_m, 7.231_m,
-                            units::radian_t{wpi::numbers::pi / 2}};
-
-    // Third Ball - Right on top of the third ball.
-    frc::Pose2d kThirdBall{11.068_m, 6.518_m, units::radian_t{-5.524}};
-
-    /// Fourth Ball - Right in front of the human player station on top of the
-    /// ball.
-    const frc::Pose2d kFourthBall{14.040_m, 6.568_m, units::radian_t{0.406644}};
-
-    // End Pose - The last position, between the third and fourth ball. Farther
-    // than the third ball from the goal so the robot doesn't have to travel as
-    // far.
-    const frc::Pose2d kEndPose{12.568_m, 6.177_m,
-                               units::radian_t{(3.0 * wpi::numbers::pi) / 2.0}};
-
-    drivetrain.Reset(kInitialPose);
-
-    intake.Deploy();
-    intake.Start(Intake::IntakeDirection::kIntake);
-
-    autonTimer.Start();
-
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    drivetrain.AddTrajectory(kInitialPose, {}, kIntakePose);
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtGoal(); })) {
-        return;
-    }
-
-    autonTimer.Start();
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    intake.Stop();
-    intake.Stow();
-
-    drivetrain.SetHeadingGoal(units::radian_t{(3 * wpi::numbers::pi) / 2});
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtHeading(); })) {
-        return;
-    }
-
-    Shoot(FrontFlywheelConstants::kShootHighTarmac,
-          FrontFlywheelConstants::kShootHighTarmac, true);
-    SetReadyToShoot(true);
-
-    if (!m_autonChooser.Suspend([=] { return !IsShooting(); })) {
-        return;
-    }
-
-    kIntakePose = UpdateAutoPoseRotation(kIntakePose, drivetrain.GetAngle());
-
-    intake.Deploy();
-    intake.Start(Intake::IntakeDirection::kIntake);
-
-    autonTimer.Start();
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    drivetrain.AddTrajectory({kIntakePose, kThirdBall, kFourthBall});
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtGoal(); })) {
-        return;
-    }
-
-    autonTimer.Start();
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-    intake.Stop();
-    intake.Stow();
-
-    {
-        auto config = DrivetrainController::MakeTrajectoryConfig();
-        config.SetReversed(true);
-
-        drivetrain.AddTrajectory(kFourthBall, {}, kEndPose, config);
-
-        if (!m_autonChooser.Suspend([=] { return drivetrain.AtGoal(); })) {
-            return;
-        }
-    }
-
-    Shoot(FrontFlywheelConstants::kShootHighTarmac,
-          BackFlywheelConstants::kShootHighTarmac, true);
-    SetReadyToShoot(true);
-
-    if (!m_autonChooser.Suspend([=] { return !IsShooting(); })) {
-        return;
-    }
-}
-
-}  // namespace frc3512
diff --git a/src/main/cpp/autonomous/AutoShootOne.cpp b/src/main/cpp/autonomous/AutoShootOne.cpp
deleted file mode 100644
index 472c273..0000000
--- a/src/main/cpp/autonomous/AutoShootOne.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-// Copyright (c) FRC Team 3512. All Rights Reserved.
-
-#include "Robot.hpp"
-
-namespace frc3512 {
-void Robot::AutoShootOne() {
-    /// Initial Pose - against the hub. The robots starts pre-loaded with one
-    /// ball.
-    frc::Pose2d kInitialPose{2.5_m, 2_m, 0_rad};
-    /// Backup Pose - drive back off the tarmac
-    frc::Pose2d kBackupPose{1.0_m, 2_m, 0_rad};
-
-    drivetrain.Reset(kInitialPose);
-
-    auto config = Drivetrain::MakeTrajectoryConfig();
-    config.SetReversed(true);
-
-    drivetrain.AddTrajectory(kInitialPose, {}, kBackupPose, config);
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtGoal(); })) {
-        return;
-    }
-
-    // shoots one preloaded ball.
-    Shoot(FrontFlywheelConstants::kShootHighFender,
-          BackFlywheelConstants::kShootHighFender, true);
-    // shoots as soon as flywheels are up to speed.
-    SetReadyToShoot(true);
-}
-}  // namespace frc3512
diff --git a/src/main/cpp/autonomous/AutoShootThree.cpp b/src/main/cpp/autonomous/AutoShootThree.cpp
deleted file mode 100644
index 85bfb3a..0000000
--- a/src/main/cpp/autonomous/AutoShootThree.cpp
+++ /dev/null
@@ -1,111 +0,0 @@
-// Copyright (c) FRC Team 3512. All Rights Reserved.
-
-#include "Robot.hpp"
-
-namespace frc3512 {
-
-void Robot::AutoShootThree() {
-    // Initial Pose - Right in front of alliance colored ball. The robot starts
-    // pre-loaded with one ball.
-    const frc::Pose2d kInitialPose{8.598_m, 6.081_m,
-                                   units::radian_t{wpi::numbers::pi / 2}};
-
-    // Intake Pose - Just in front of the initial position. Right on top of the
-    // colored ball.
-    frc::Pose2d kIntakePose{8.598_m, 7.231_m,
-                            units::radian_t{wpi::numbers::pi / 2}};
-
-    // Shoot Pose - Right on top of the third ball.
-    const frc::Pose2d kEndPose{11.068_m, 6.177_m, units::radian_t{0}};
-
-    drivetrain.Reset(kInitialPose);
-
-    intake.Deploy();
-    intake.Start(Intake::IntakeDirection::kIntake);
-
-    autonTimer.Start();
-
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    drivetrain.AddTrajectory(kInitialPose, {}, kIntakePose);
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtGoal(); })) {
-        return;
-    }
-
-    autonTimer.Start();
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(1.0_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    intake.Stop();
-    intake.Stow();
-
-    drivetrain.SetHeadingGoal(units::radian_t{(3 * wpi::numbers::pi) / 2});
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtHeading(); })) {
-        return;
-    }
-
-    Shoot(FrontFlywheelConstants::kShootHighTarmac,
-          FrontFlywheelConstants::kShootHighTarmac, true);
-    SetReadyToShoot(true);
-
-    if (!m_autonChooser.Suspend([=] { return !IsShooting(); })) {
-        return;
-    }
-
-    kIntakePose = UpdateAutoPoseRotation(kIntakePose, drivetrain.GetAngle());
-
-    intake.Deploy();
-    intake.Start(Intake::IntakeDirection::kIntake);
-
-    autonTimer.Start();
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    drivetrain.AddTrajectory(kIntakePose, {}, kEndPose);
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtGoal(); })) {
-        return;
-    }
-
-    autonTimer.Start();
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    intake.Stow();
-    intake.Stop();
-
-    drivetrain.SetHeadingGoal(units::radian_t{(2.5 * wpi::numbers::pi) / 2.0});
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtHeading(); })) {
-        return;
-    }
-
-    Shoot(FrontFlywheelConstants::kShootHighTarmac,
-          BackFlywheelConstants::kShootHighTarmac, true);
-    SetReadyToShoot(true);
-
-    if (!m_autonChooser.Suspend([=] { return !IsShooting(); })) {
-        return;
-    }
-}
-
-}  // namespace frc3512
diff --git a/src/main/cpp/autonomous/AutoShootTwo.cpp b/src/main/cpp/autonomous/AutoShootTwo.cpp
deleted file mode 100644
index bc622d7..0000000
--- a/src/main/cpp/autonomous/AutoShootTwo.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-// Copyright (c) FRC Team 3512. All Rights Reserved.
-
-#include "Robot.hpp"
-
-namespace frc3512 {
-
-void Robot::AutoShootTwo() {
-    // Initial Pose - Right in front of alliance colored ball. The robot starts
-    // pre-loaded with one ball.
-    const frc::Pose2d kInitialPose{8.598_m, 6.081_m,
-                                   units::radian_t{wpi::numbers::pi / 2}};
-
-    // Intake Pose - Just in front of the initial position. Right on top of the
-    // colored ball.
-    frc::Pose2d kIntakePose{8.598_m, 7.431_m,
-                            units::radian_t{wpi::numbers::pi / 2}};
-
-    drivetrain.Reset(kInitialPose);
-
-    intake.Deploy();
-    intake.Start(Intake::IntakeDirection::kIntake);
-
-    autonTimer.Start();
-
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    drivetrain.AddTrajectory(kInitialPose, {}, kIntakePose);
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtGoal(); })) {
-        return;
-    }
-
-    autonTimer.Start();
-    if (!m_autonChooser.Suspend([=] { return autonTimer.HasElapsed(0.5_s); })) {
-        return;
-    }
-
-    autonTimer.Reset();
-    autonTimer.Stop();
-
-    intake.Stop();
-    intake.Stow();
-
-    drivetrain.SetHeadingGoal(units::radian_t{(3 * wpi::numbers::pi) / 2});
-
-    if (!m_autonChooser.Suspend([=] { return drivetrain.AtHeading(); })) {
-        return;
-    }
-
-    Shoot(FrontFlywheelConstants::kShootHighTarmac,
-          FrontFlywheelConstants::kShootHighTarmac, true);
-    SetReadyToShoot(true);
-
-    if (!m_autonChooser.Suspend([=] { return !IsShooting(); })) {
-        return;
-    }
-}
-
-}  // namespace frc3512
diff --git a/src/main/cpp/subsystems/BackFlywheel.cpp b/src/main/cpp/subsystems/BackFlywheel.cpp
index cfd7040..a1e05fb 100644
--- a/src/main/cpp/subsystems/BackFlywheel.cpp
+++ b/src/main/cpp/subsystems/BackFlywheel.cpp
@@ -133,11 +133,6 @@ void BackFlywheel::ControllerPeriodic() {
     m_u = m_controller.Calculate(m_observer.Xhat());
     SetVoltage(units::volt_t{m_u(Input::kVoltage)});
 
-    while (visionQueue.size() > 0) {
-        auto measurement = visionQueue.pop_front();
-        m_range = measurement.range;
-    }
-
     Log(m_controller.GetReferences(), m_observer.Xhat(), m_u, y);
 
     if constexpr (frc::RobotBase::IsSimulation()) {
diff --git a/src/main/cpp/subsystems/Drivetrain.cpp b/src/main/cpp/subsystems/Drivetrain.cpp
index e8a340a..88e4b8f 100644
--- a/src/main/cpp/subsystems/Drivetrain.cpp
+++ b/src/main/cpp/subsystems/Drivetrain.cpp
@@ -146,8 +146,6 @@ void Drivetrain::Reset(const frc::Pose2d& initialPose) {
 }
 
 void Drivetrain::ControllerPeriodic() {
-    using Input = DrivetrainController::Input;
-
     UpdateDt();
 
     m_velocityObserver.Predict(m_u, GetDt());
@@ -178,60 +176,6 @@ void Drivetrain::ControllerPeriodic() {
 
     m_velocityObserver.Correct(m_controller.GetInputs(), velPosY);
 
-    Eigen::Vector<double, 7> controllerState = GetStates();
-
-    while (visionQueue.size() > 0) {
-        auto measurement = visionQueue.pop_front();
-
-        m_controller.SetVisionYaw(measurement.yaw);
-        m_controller.SetVisionRange(measurement.range);
-    }
-
-    if (m_controller.HaveTrajectory()) {
-        m_u = m_controller.Calculate(GetStates());
-
-        if (!AtGoal()) {
-            m_leftGrbx.SetVoltage(units::volt_t{m_u(Input::kLeftVoltage)});
-            m_rightGrbx.SetVoltage(units::volt_t{m_u(Input::kRightVoltage)});
-        } else {
-            m_leftGrbx.SetVoltage(0_V);
-            m_rightGrbx.SetVoltage(0_V);
-        }
-    } else if (HasHeadingGoal()) {
-        // Update previous u stored in the controller. We don't care what the
-        // return value is.
-        static_cast<void>(m_controller.Calculate(GetStates()));
-
-        if (!AtHeading()) {
-            auto turningOutput = m_turningPID.Calculate(units::radian_t{
-                controllerState(DrivetrainController::State::kHeading)});
-            m_leftGrbx.SetVoltage(units::volt_t{-turningOutput} +
-                                  m_turningFeedforward.Calculate(
-                                      m_turningPID.GetGoal().velocity));
-            m_rightGrbx.SetVoltage(units::volt_t{turningOutput} +
-                                   m_turningFeedforward.Calculate(
-                                       m_turningPID.GetGoal().velocity));
-        } else {
-            m_hasNewHeading = false;
-            m_leftGrbx.SetVoltage(0_V);
-            m_rightGrbx.SetVoltage(0_V);
-        }
-    } else {
-        // Update previous u stored in the controller. We don't care what the
-        // return value is.
-        static_cast<void>(m_controller.Calculate(GetStates()));
-
-        // Run observer predict with inputs from teleop
-        m_u = Eigen::Vector<double, 2>{
-            std::clamp(m_leftGrbx.Get(), -1.0, 1.0) *
-                frc::RobotController::GetInputVoltage(),
-            std::clamp(m_rightGrbx.Get(), -1.0, 1.0) *
-                frc::RobotController::GetInputVoltage()};
-
-        m_turningPID.Calculate(units::radian_t{
-            controllerState(DrivetrainController::State::kHeading)});
-    }
-
     if (IsVisionAiming()) {
         double rotation =
             -m_visionController.Calculate(GetVisionYaw().value(), 0.0);
diff --git a/src/main/cpp/subsystems/FrontFlywheel.cpp b/src/main/cpp/subsystems/FrontFlywheel.cpp
index e3c8092..8023018 100644
--- a/src/main/cpp/subsystems/FrontFlywheel.cpp
+++ b/src/main/cpp/subsystems/FrontFlywheel.cpp
@@ -134,11 +134,6 @@ void FrontFlywheel::ControllerPeriodic() {
     m_u = m_controller.Calculate(m_observer.Xhat());
     SetVoltage(units::volt_t{m_u(Input::kVoltage)});
 
-    while (visionQueue.size() > 0) {
-        auto measurement = visionQueue.pop_front();
-        m_range = measurement.range;
-    }
-
     Log(m_controller.GetReferences(), m_observer.Xhat(), m_u, y);
 
     if constexpr (frc::RobotBase::IsSimulation()) {
diff --git a/src/main/cpp/subsystems/Vision.cpp b/src/main/cpp/subsystems/Vision.cpp
deleted file mode 100644
index 1da1130..0000000
--- a/src/main/cpp/subsystems/Vision.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-// Copyright (c) FRC Team 3512. All Rights Reserved.
-
-#include "subsystems/Vision.hpp"
-
-#include <frc/DriverStation.h>
-#include <frc/Joystick.h>
-#include <frc/RobotBase.h>
-#include <frc/Timer.h>
-#include <photonlib/PhotonUtils.h>
-
-#include "HWConfig.hpp"
-#include "TargetModel.hpp"
-
-using namespace frc3512;
-
-void Vision::TurnLEDOn() { m_rpiCam.SetLEDMode(photonlib::LEDMode::kOn); }
-
-void Vision::TurnLEDOff() { m_rpiCam.SetLEDMode(photonlib::LEDMode::kOff); }
-
-bool Vision::IsLEDOn() const {
-    return m_rpiCam.GetLEDMode() == photonlib::LEDMode::kOn;
-}
-
-void Vision::SubscribeToVisionData(
-    wpi::static_circular_buffer<GlobalMeasurements, 8>& queue) {
-    m_subsystemQueues.push_back(&queue);
-}
-
-void Vision::UnsubscribeFromVisionData(
-    wpi::static_circular_buffer<GlobalMeasurements, 8>& queue) {
-    m_subsystemQueues.erase(
-        std::remove(m_subsystemQueues.begin(), m_subsystemQueues.end(), &queue),
-        m_subsystemQueues.end());
-}
-
-void Vision::UpdateVisionMeasurementsSim(const frc::Pose2d& drivetrainPose) {
-    m_simVision.ProcessFrame(drivetrainPose);
-}
-
-bool Vision::HaveTargets() const { return m_haveTargets; }
-
-void Vision::SwitchDriverMode(bool driveSwitch) {
-    m_rpiCam.SetDriverMode(driveSwitch);
-}
-
-int Vision::GetPipelineIndex() { return m_rpiCam.GetPipelineIndex(); }
-
-void Vision::SetPipeline(int pipeline) { m_rpiCam.SetPipelineIndex(pipeline); }
-
-void Vision::RobotPeriodic() {
-    if constexpr (!frc::RobotBase::IsSimulation()) {
-        static frc::Joystick appendageStick1{HWConfig::kAppendageStick1Port};
-
-        const auto& result = m_rpiCam.GetLatestResult();
-
-        if (result.GetTargets().size() == 0 ||
-            frc::DriverStation::IsDisabled()) {
-            m_haveTargets = false;
-            return;
-        }
-
-        m_haveTargets = true;
-
-        auto timestamp = frc::Timer::GetFPGATimestamp() - result.GetLatency();
-
-        m_timestampEntry.SetDouble(units::time::second_t{timestamp}.value());
-
-        photonlib::PhotonTrackedTarget target = result.GetBestTarget();
-
-        if (frc::DriverStation::IsTeleop()) {
-            if (appendageStick1.GetRawButtonPressed(4)) {
-                TurnLEDOn();
-            } else if (appendageStick1.GetRawButtonPressed(6)) {
-                TurnLEDOff();
-            }
-        }
-
-        // Converts solvePnP() data from the NetworkTables to a global
-        // drivetrain pose measurement
-        auto cameraInTarget = target.GetCameraRelativePose();
-        auto cameraInGlobal =
-            TargetModel::kTargetPoseInGlobal.TransformBy(cameraInTarget);
-        std::array<double, 3> pose{cameraInGlobal.X().value(),
-                                   cameraInGlobal.Y().value(),
-                                   cameraInGlobal.Rotation().Radians().value()};
-        m_poseEntry.SetDoubleArray(pose);
-
-        m_pitch = units::degree_t{target.GetPitch()};
-        if (target.GetYaw() < 0.0) {
-            m_yaw = units::degree_t{target.GetYaw()} - kYawOffset;
-        } else if (target.GetYaw() > 0.0) {
-            m_yaw = units::degree_t{target.GetYaw()} + kYawOffset;
-        } else {
-            m_yaw = units::degree_t{target.GetYaw()};
-        }
-
-        m_yawEntry.SetDouble(units::radian_t{m_yaw}.value());
-
-        m_range = photonlib::PhotonUtils::CalculateDistanceToTarget(
-            kCameraHeight, 2.606_m, kCameraPitch, units::degree_t{m_pitch});
-
-        m_rangeEntry.SetDouble(units::meter_t{m_range}.value());
-
-        for (auto& queue : m_subsystemQueues) {
-            queue->push_back({m_yaw, m_pitch, m_range});
-        }
-    }
-}
diff --git a/src/main/include/Robot.hpp b/src/main/include/Robot.hpp
index b10bac1..0aff7d9 100644
--- a/src/main/include/Robot.hpp
+++ b/src/main/include/Robot.hpp
@@ -19,7 +19,6 @@
 #include "subsystems/FrontFlywheel.hpp"
 #include "subsystems/Intake.hpp"
 #include "subsystems/SubsystemBase.hpp"
-#include "subsystems/Vision.hpp"
 
 #if RUNNING_FRC_TESTS
 #include <gtest/gtest.h>
@@ -54,9 +53,7 @@ class Robot : public frc::TimesliceRobot {
      */
     enum class ShootingState {
         kIdle,
-        kVisionSpinUp,
         kSpinUp,
-        kVisionAim,
         kStartConveyor,
         kEndShoot
     };
@@ -78,9 +75,6 @@ class Robot : public frc::TimesliceRobot {
     /// Climber subsystem.
     Climber climber;
 
-    /// Vision subsystem
-    Vision vision;
-
     /// Timer used to add delays into autonomous
     frc::Timer autonTimer;
 
@@ -160,31 +154,6 @@ class Robot : public frc::TimesliceRobot {
      */
     void AutoNoOp();
 
-    /**
-     * Robot shoots one ball from the agianst the hub and drives off the tarmac.
-     */
-    void AutoShootOne();
-
-    /**
-     * Drive backwards autonomous.
-     */
-    void AutoBackwards();
-
-    /**
-     * Shoot two autonomous.
-     */
-    void AutoShootTwo();
-
-    /**
-     * Shoot three autonomous.
-     */
-    void AutoShootThree();
-
-    /**
-     * Shoot four autonomous.
-     */
-    void AutoShootFour();
-
     /**
      * Returns a pose with the same x and y coordinates, but an updated heading.
      * A utility function for autonomous positions used when the robot turns in
@@ -232,8 +201,7 @@ class Robot : public frc::TimesliceRobot {
      * speed with vision.
      */
     void Shoot(units::radians_per_second_t frontSpeed,
-               units::radians_per_second_t backSpeed, bool visionAim = false,
-               bool shootWithRange = false);
+               units::radians_per_second_t backSpeed);
 
     /**
      * Runs the shooter state machine.
diff --git a/src/main/include/subsystems/BackFlywheel.hpp b/src/main/include/subsystems/BackFlywheel.hpp
index 75ebaaa..e8f8642 100644
--- a/src/main/include/subsystems/BackFlywheel.hpp
+++ b/src/main/include/subsystems/BackFlywheel.hpp
@@ -27,7 +27,6 @@
 #include "NetworkTableUtil.hpp"
 #include "controllers/FlywheelController.hpp"
 #include "subsystems/ControlledSubsystemBase.hpp"
-#include "subsystems/Vision.hpp"
 
 namespace frc3512 {
 
@@ -49,12 +48,6 @@ class BackFlywheel : public ControlledSubsystemBase<1, 1, 1> {
     /// offset for all shooting speeds.
     static constexpr auto kSpeedOffset = 0.9;
 
-    /**
-     * Producer-consumer queue for yaw, pitch, and range measurements from
-     * Vision subsystem.
-     */
-    wpi::static_circular_buffer<Vision::GlobalMeasurements, 8> visionQueue;
-
     /**
      * Returns angular displacement of the rear flywheel
      *
diff --git a/src/main/include/subsystems/Drivetrain.hpp b/src/main/include/subsystems/Drivetrain.hpp
index 3d43410..10b4aa1 100644
--- a/src/main/include/subsystems/Drivetrain.hpp
+++ b/src/main/include/subsystems/Drivetrain.hpp
@@ -31,7 +31,6 @@
 #include <frc/trajectory/TrajectoryConfig.h>
 #include <frc/trajectory/TrapezoidProfile.h>
 #include <networktables/NetworkTableEntry.h>
-#include <photonlib/PhotonCamera.h>
 #include <rev/CANSparkMax.h>
 #include <units/acceleration.h>
 #include <units/angle.h>
@@ -44,7 +43,6 @@
 #include "Constants.hpp"
 #include "HWConfig.hpp"
 #include "NetworkTableUtil.hpp"
-#include "Vision.hpp"
 #include "controllers/DrivetrainController.hpp"
 #include "subsystems/ControlledSubsystemBase.hpp"
 
@@ -66,12 +64,6 @@ class Drivetrain : public ControlledSubsystemBase<7, 2, 5> {
      */
     static constexpr units::meter_t kMiddleOfRobotToIntake = 0.656_m;
 
-    /**
-     * Producer-consumer queue for global pose measurements from Vision
-     * subsystem.
-     */
-    wpi::static_circular_buffer<Vision::GlobalMeasurements, 8> visionQueue;
-
     Drivetrain();
 
     Drivetrain(const Drivetrain&) = delete;
@@ -400,8 +392,6 @@ class Drivetrain : public ControlledSubsystemBase<7, 2, 5> {
     DrivetrainController m_controller;
     Eigen::Vector<double, 2> m_u = Eigen::Vector<double, 2>::Zero();
 
-    frc3512::Vision vision;
-
     frc::Timer m_visionTimer;
     bool m_aimWithVision = false;
     bool m_atVisionTarget = false;
diff --git a/src/main/include/subsystems/FrontFlywheel.hpp b/src/main/include/subsystems/FrontFlywheel.hpp
index a81525a..24b58c9 100644
--- a/src/main/include/subsystems/FrontFlywheel.hpp
+++ b/src/main/include/subsystems/FrontFlywheel.hpp
@@ -27,7 +27,6 @@
 #include "NetworkTableUtil.hpp"
 #include "controllers/FlywheelController.hpp"
 #include "subsystems/ControlledSubsystemBase.hpp"
-#include "subsystems/Vision.hpp"
 
 namespace frc3512 {
 
@@ -49,12 +48,6 @@ class FrontFlywheel : public ControlledSubsystemBase<1, 1, 1> {
     /// offset applied to shooting speeds.
     static constexpr double kSpeedOffset = 0.95;
 
-    /**
-     * Producer-consumer queue for yaw, pitch, and range measurements from
-     * Vision subsystem.
-     */
-    wpi::static_circular_buffer<Vision::GlobalMeasurements, 8> visionQueue;
-
     /**
      * Deploys the solenoid for the shooter.
      */
diff --git a/src/main/include/subsystems/Vision.hpp b/src/main/include/subsystems/Vision.hpp
deleted file mode 100644
index 8b9a7f3..0000000
--- a/src/main/include/subsystems/Vision.hpp
+++ /dev/null
@@ -1,168 +0,0 @@
-// Copyright (c) FRC Team 3512. All Rights Reserved.
-
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <frc/geometry/Pose2d.h>
-#include <frc/geometry/Transform2d.h>
-#include <frc/geometry/Translation2d.h>
-#include <networktables/NetworkTableEntry.h>
-#include <networktables/NetworkTableInstance.h>
-#include <photonlib/PhotonCamera.h>
-#include <photonlib/SimVisionSystem.h>
-#include <units/angle.h>
-#include <units/length.h>
-#include <units/time.h>
-#include <wpi/static_circular_buffer.h>
-
-#include "Constants.hpp"
-#include "NetworkTableUtil.hpp"
-#include "TargetModel.hpp"
-#include "subsystems/SubsystemBase.hpp"
-
-namespace frc3512 {
-
-/**
- * Vision subsystem
- */
-class Vision : public SubsystemBase {
-public:
-    /// Vision Targeting Camera name in Networktables
-    static constexpr char kVisionCamName[] = "mmal_service_16.1";
-
-    /// Driver Camera name in Networktables
-    static constexpr char kDriverCamName[] = "USB_CAM";
-
-    /// Pipeline index for upper hub vision tracking
-    static constexpr int kTargetPipeline = 0;
-
-    /// Pipeline index for blue cargo vision tracking
-    static constexpr int kBlueBallPipeline = 1;
-
-    /// Pipeline index for red cargo vision tracking
-    static constexpr int kRedBallPipeline = 2;
-
-    /// Target Height
-    static constexpr units::meter_t kTargetHeight = 2.6_m;
-
-    /// Camera Height
-    static constexpr units::meter_t kCameraHeight = 0.76835_m;
-
-    /// Camera Pitch
-    static constexpr units::degree_t kCameraPitch = 39.6_deg;
-
-    /// Pi camera V1 diagonal field of view
-    static constexpr units::degree_t kCameraDiagonalFOV = 74.8_deg;
-
-    /// The yaw offset of the vision camera.
-    static constexpr units::degree_t kYawOffset = 3.5_deg;
-
-    /**
-     * Container for global measurements.
-     *
-     * These are sent to the drivetrian and flywheel subsystems via a
-     * producer-consumer queue.
-     */
-    struct GlobalMeasurements {
-        /// Yaw reported by photonvision
-        units::radian_t yaw;
-        /// Pitch reported by photonvision
-        units::radian_t pitch;
-        /// Range from robot to target
-        units::meter_t range;
-    };
-
-    /// Turn Camera LEDs on
-    void TurnLEDOn();
-
-    /// Turn Camera LEDs off
-    void TurnLEDOff();
-
-    /**
-     * Returns whether or not the LEDs are on
-     */
-    bool IsLEDOn() const;
-
-    /**
-     * Subscribe queue to vision data
-     *
-     * @param queue the queue that is subscribing to recieve vision data
-     */
-    void SubscribeToVisionData(
-        wpi::static_circular_buffer<GlobalMeasurements, 8>& queue);
-
-    /**
-     * Unsubscribe queue from the vision data
-     *
-     * @param queue the quue that is unsubscribing form the vision data
-     */
-    void UnsubscribeFromVisionData(
-        wpi::static_circular_buffer<GlobalMeasurements, 8>& queue);
-
-    /**
-     * Updates vision sim data with new pose and camera transformation
-     *
-     * @param drivetrainPose Drivetrain pose to see if target is in range
-     */
-    void UpdateVisionMeasurementsSim(const frc::Pose2d& drivetrainPose);
-
-    /**
-     * Returns whether or not the camera can see a target
-     */
-    bool HaveTargets() const;
-
-    /**
-     * Switches the camere to driver mode
-     */
-    void SwitchDriverMode(bool driveSwitch);
-
-    /**
-     * Returns the value of the current pipeline index
-     */
-    int GetPipelineIndex();
-
-    /**
-     * Sets the pipeline of the vision system
-     */
-    void SetPipeline(int pipeline);
-
-    void RobotPeriodic() override;
-
-private:
-    photonlib::PhotonCamera m_rpiCam{kVisionCamName};
-    photonlib::PhotonCamera m_usbCam{kDriverCamName};
-
-    bool m_haveTargets = false;
-
-    std::vector<wpi::static_circular_buffer<GlobalMeasurements, 8>*>
-        m_subsystemQueues;
-
-    units::radian_t m_yaw;
-    units::radian_t m_pitch;
-    units::meter_t m_range;
-
-    std::string m_allianceColor = "Blue";
-
-    nt::NetworkTableEntry m_poseEntry = NetworkTableUtil::MakeDoubleArrayEntry(
-        "/Diagnostics/Vision/Drivetrain pose");
-    nt::NetworkTableEntry m_yawEntry =
-        NetworkTableUtil::MakeDoubleEntry("/Diagnostics/Vision/Yaw");
-    nt::NetworkTableEntry m_rangeEntry =
-        NetworkTableUtil::MakeDoubleEntry("/Diagnostics/Vision/Range Estimate");
-    nt::NetworkTableEntry m_timestampEntry =
-        NetworkTableUtil::MakeDoubleEntry("/Diagnostics/Vision/Timestamp");
-
-    // Simulation variables
-    photonlib::SimVisionSystem m_simVision{kVisionCamName,
-                                           kCameraDiagonalFOV,
-                                           kCameraPitch,
-                                           frc::Transform2d{},
-                                           kCameraHeight,
-                                           20_m,
-                                           960,
-                                           720,
-                                           10};
-};
-}  // namespace frc3512
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
deleted file mode 100644
index cad59ad..0000000
--- a/vendordeps/photonlib.json
+++ /dev/null
@@ -1,41 +0,0 @@
-{
-    "fileName": "photonlib.json",
-    "name": "photonlib",
-    "version": "v2022.2.0",
-    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
-    "mavenUrls": [
-        "https://maven.photonvision.org/repository/internal",
-        "https://maven.photonvision.org/repository/snapshots"
-    ],
-    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
-    "jniDependencies": [],
-    "cppDependencies": [
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "PhotonLib-cpp",
-            "version": "v2022.2.0",
-            "libName": "Photon",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxathena",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "PhotonLib-java",
-            "version": "v2022.2.0"
-        },
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "PhotonTargeting-java",
-            "version": "v2022.2.0"
-        }
-    ]
-}