Merge pull request #143 from strykeforce/QuickAutonFix

Quick auton fix

Author: mwitcpalek

src/main/java/frc/robot/Robot.java

@@ -124,6 +124,7 @@ public void teleopInit() {
     m_robotContainer.zeroClimb();
     m_robotContainer.startAutoIntake();
     m_robotContainer.setDoVisionOdomReset(true);
+    m_robotContainer.autonIgnoreColorSensor(false);
     CommandScheduler.getInstance()
         .schedule(
             new EnableVisionCommand(

src/main/java/frc/robot/RobotContainer.java

@@ -47,7 +47,7 @@
 import frc.robot.commands.drive.XLockCommand;
 import frc.robot.commands.drive.ZeroGyroCommand;
 import frc.robot.commands.intake.IntakeOpenLoopCommand;
-import frc.robot.commands.magazine.AutonIgnoreColorSensorCommand;
+import frc.robot.commands.magazine.IgnoreColorSensorCommand;
 import frc.robot.commands.magazine.LowerMagazineOpenLoopCommand;
 import frc.robot.commands.magazine.ManualEjectCargoReverseCommand;
 import frc.robot.commands.magazine.PitClearCargoColor;
@@ -197,6 +197,10 @@ public void zeroClimb() {
     }
   }
 
+  public void autonIgnoreColorSensor(boolean ignore) {
+    magazineSubsystem.setAutonIgnoreColorSensor(ignore);
+  }
+
   private void configureTelemetry() {
     driveSubsystem.registerWith(telemetryService);
     shooterSubsystem.registerWith(telemetryService);
@@ -228,9 +232,9 @@ private void configureDriverButtonBindings() {
 
     // Ignore Color Sensor
     new JoystickButton(driveJoystick, Toggle.LEFT_TOGGLE.id)
-        .whenPressed(new AutonIgnoreColorSensorCommand(magazineSubsystem, true));
+        .whenPressed(new IgnoreColorSensorCommand(magazineSubsystem, true));
     new JoystickButton(driveJoystick, Toggle.LEFT_TOGGLE.id)
-        .whenReleased(new AutonIgnoreColorSensorCommand(magazineSubsystem, false));
+        .whenReleased(new IgnoreColorSensorCommand(magazineSubsystem, false));
 
     // Hood Open Loop
     new JoystickButton(driveJoystick, Trim.RIGHT_Y_POS.id)

src/main/java/frc/robot/subsystems/MagazineSubsystem.java

@@ -489,7 +489,7 @@ public void periodic() {
             logger.info("ReadTimer Elapsed");
             cargoColor = allianceCargoColor;
             storedCargoColors[1] = cargoColor;
-          } else {
+          } else if (hasReadElapsed) {
             logger.info("ReadTimer Elapsed");
             autonReadTimerElapsed = true;
           }