From 574dfe3500eb209bebbc709e4b9cc88a81993445 Mon Sep 17 00:00:00 2001
From: Zachary Orr <516458+ZachOrr@users.noreply.github.com>
Date: Thu, 17 Feb 2022 23:54:35 -0500
Subject: [PATCH] Backout some changes

---
 .../swervelib/ctre/CtreUtils.java                  |  2 +-
 .../Falcon500SteerControllerFactoryBuilder.java    |  2 +-
 src/main/java/frc/robot/RobotContainer.java        | 14 ++++----------
 3 files changed, 6 insertions(+), 12 deletions(-)

diff --git a/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java b/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java
index c79530b7..7c032d3e 100644
--- a/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java
+++ b/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java
@@ -9,7 +9,7 @@ private CtreUtils() {}
 
     public static void checkCtreError(ErrorCode errorCode, String message) {
         if (errorCode != ErrorCode.OK) {
-            // DriverStation.reportError(String.format("%s: %s", message, errorCode.toString()), false);
+            DriverStation.reportError(String.format("%s: %s", message, errorCode.toString()), false);
         }
     }
 }
diff --git a/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java b/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java
index ec9df95a..e980e681 100644
--- a/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java
+++ b/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java
@@ -151,7 +151,7 @@ public ControllerImplementation create(Falcon500SteerConfiguration<T> steerConfi
     }
 
     private static class ControllerImplementation implements SteerController {
-        private static final int ENCODER_RESET_ITERATIONS = 125;
+        private static final int ENCODER_RESET_ITERATIONS = 500;
         private static final double ENCODER_RESET_MAX_ANGULAR_VELOCITY = Math.toRadians(0.5);
 
         private final TalonFX motor;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e28d8dd7..5026f54e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -7,8 +7,6 @@
 import com.ctre.phoenix.sensors.PigeonIMU;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -18,7 +16,6 @@
 import frc.robot.commands.auto.DoNothingCommand;
 import frc.robot.commands.delivery.DeliveryOverrideCommand;
 import frc.robot.commands.swerve.SwerveDriveCommand;
-import frc.robot.commands.teleop.DistanceToTargetCommand;
 import frc.robot.commands.teleop.FlareCommand;
 import frc.robot.nerdyfiles.oi.NerdyOperatorStation;
 import frc.robot.commands.shooter.RunKicker;
@@ -31,23 +28,20 @@ public class RobotContainer {
   private final NerdyOperatorStation operatorStation = new NerdyOperatorStation(2);
 
   private final PigeonIMU pigeon = new PigeonIMU(0);
-  // private final PixyCam pixyCam = new PixyCam();
+  private final PixyCam pixyCam = new PixyCam();
 
   // private final Climber climber = new Climber();
-  // private final Intake intake = new Intake();
+  private final Intake intake = new Intake();
   private final Shooter shooter = new Shooter();
   private final Kicker kicker = new Kicker();
-  // private final Vision vision = new Vision();
+  private final Vision vision = new Vision();
   private final AutoDrive autoDrive = new AutoDrive();
-  // private final Delivery delivery = new Delivery();
+  private final Delivery delivery = new Delivery();
   private final Drivetrain drivetrain = new Drivetrain(pigeon);
   private final Heading heading = new Heading(drivetrain::getGyroscopeRotation, drivetrain::getChassisSpeeds);
 
   private final SendableChooser<Command> autonChooser = new SendableChooser<>();
 
-  // private final ColorSensorREV c1 = new ColorSensorREV(I2C.Port.kOnboard);
-  private final ColorSensorTCS c2 = new ColorSensorTCS(I2C.Port.kMXP);
-
   public RobotContainer() {
     drivetrain.setDefaultCommand(new SwerveDriveCommand(driverController, autoDrive, heading, drivetrain));
     heading.setDefaultCommand(new HeadingToTargetCommand(() -> drivetrain.getPose().getTranslation(), heading));