Backout some changes

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);
         }
     }
 }

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;

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));