Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

src/main/java/frc4388/robot/Constants.java

@@ -164,7 +164,7 @@ public final class Constants {
   public static final class OIConstants {
     public static final int XBOX_DRIVER_ID = 0;
     public static final int XBOX_OPERATOR_ID = 1;
-    public static final int BUTTON_FOX_ID = 2;
+    public static final int BUTTON_BOX_ID = 2;
     public static final double LEFT_AXIS_DEADBAND = 0.1;
     public static final double RIGHT_AXIS_DEADBAND = 0.6;
     public static final boolean SKEW_STICKS = true;

src/main/java/frc4388/robot/Robot.java

@@ -269,15 +269,7 @@ public class Robot extends TimedRobot {
    * This function is called periodically during operator control.
    */
   @Override
-  public void teleopPeriodic() {
+  public void teleopPeriodic() {}
-    boolean robotManual = m_robotContainer.manual;
-    
-    if (robotManual) {
-      m_robotContainer.configureManualButtonBindings();
-    } else {
-      m_robotContainer.configureAutomaticButtonBindings();
-    }
-  }
 
   @Override
   public void testInit() {

src/main/java/frc4388/robot/RobotContainer.java

@@ -29,7 +29,6 @@ import java.util.regex.Matcher;
 import java.util.regex.Pattern;
 import java.util.stream.Collectors;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.diffplug.common.base.Errors;
 import com.pathplanner.lib.PathPlanner;
@@ -58,11 +57,11 @@ import edu.wpi.first.wpilibj2.command.NotifierCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
+import frc4388.robot.commands.RunMiddleSwitch;
 import frc4388.robot.commands.Shoot;
 import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.subsystems.BoomBoom;
@@ -116,7 +115,7 @@ public class RobotContainer {
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
-  private final ButtonBox m_buttonFox = new ButtonBox(OIConstants.BUTTON_FOX_ID);
+  private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
 
   /* Autonomous */
   private PathPlannerTrajectory loadedPathTrajectory = null;
@@ -226,19 +225,14 @@ public class RobotContainer {
     //     .whenPressed(() -> m_robotMap.leftBack.reset())
     //     .whenPressed(() -> m_robotMap.rightBack.reset()); 
 
-    
-    
-    
-    
-    
     /* Operator Buttons */
 
       // X > Extend Intake
-    /*new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-        .whenPressed(() -> m_robotIntake.runExtender(true));
+        .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry));
       // Y > Retract Intake
-    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+    // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
-        .whenPressed(() -> m_robotIntake.runExtender(false));*/
+    //     .whenPressed(() -> m_robotIntake.runExtender(false));
     
     // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft
     //     .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425)))
@@ -263,7 +257,7 @@ public class RobotContainer {
         .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
-        .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
+        .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
         .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
@@ -286,7 +280,6 @@ public class RobotContainer {
     //     .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
     //     .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
 
-
       // A > Shoot with Odo
     /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
         .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/
@@ -296,37 +289,23 @@ public class RobotContainer {
     //     .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
   
     /* Button Box Buttons */
-    new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftSwitch.value)
+    new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
-        .whenPressed(new RunCommand(() -> setManual(true)))
+        .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true)))
-        .whenReleased(new RunCommand(() -> setManual(false)));
+        .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false)));
     
-    // new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value)
+    new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
-    //     .whileHeld(new RunCommand(() -> System.out.println("MiddleSwitch")));
+        .whileHeld(new RunMiddleSwitch());
       
-    new JoystickButton(getButtonFox(), ButtonBox.Button.kRightSwitch.value)
+    new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
         .whileHeld(new RunCommand(() -> System.out.println("RightSwitch")));
 
-    new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftButton.value)
+    new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
         .whileHeld(new RunCommand(() -> System.out.println("LeftButton")));
 
-    new JoystickButton(getButtonFox(), ButtonBox.Button.kRightButton.value)
+    new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value)
         .whileHeld(new RunCommand(() -> System.out.println("RightButton")));
   }
 
-  public void configureManualButtonBindings() {
-
-    new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value)
-      .whileHeld(new RunCommand(() -> System.out.println("MANUAL")));
-
-  }
-
-  public void configureAutomaticButtonBindings() {
-    
-    new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value)
-    .whileHeld(new RunCommand(() -> System.out.println("AUTOMATIC")));
-    
-  }
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
@@ -363,8 +342,8 @@ public class RobotContainer {
     return m_operatorXbox;
   }
 
-  public ButtonBox getButtonFox() {
+  public ButtonBox getButtonBox() {
-    return m_buttonFox;
+    return m_buttonBox;
   }
 
   public void setManual(boolean set) {

src/main/java/frc4388/robot/RobotMap.java

@@ -37,6 +37,10 @@ public class RobotMap {
     // configureLEDMotorControllers();
     configureSwerveMotorControllers();
     configureShooterMotorControllers();
+    configureIntakeMotors();
+    configureExtenderMotors();
+    configureSerializerMotors();
+    configureStorageMotors();
   }
 
   /* LED Subsystem */
@@ -182,7 +186,7 @@ public class RobotMap {
     rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
         RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    }
+}
 
 //   // Shooter Config
 //   /* Boom Boom Subsystem */
@@ -258,15 +262,32 @@ public class RobotMap {
   public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
   public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
 
-  void configureIntakeMotors(){
+  void configureIntakeMotors() {
     intakeMotor.configFactoryDefault();
+    intakeMotor.setInverted(false);
     intakeMotor.setNeutralMode(NeutralMode.Coast);
+
     intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
     intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
   }
 
+  void configureExtenderMotors() {
+      extenderMotor.restoreFactoryDefaults();
+      extenderMotor.setInverted(true);
+      extenderMotor.setIdleMode(IdleMode.kBrake);
+  }
+
+    void configureSerializerMotors() {
+        serializerBelt.restoreFactoryDefaults();
+    }
+
   /* Storage Subsystem */
-  public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
+    public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
 //   public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
 //   public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
+
+    void configureStorageMotors() {
+            storageMotor.restoreFactoryDefaults();
+    }
+
 }

src/main/java/frc4388/robot/commands/RunMiddleSwitch.java

@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class RunMiddleSwitch extends CommandBase {
+  
+  private static boolean manual = false;
+  private boolean newManual = false;
+  private boolean changes = false;
+
+  /** Creates a new RunMiddleSwitch. */
+  public RunMiddleSwitch() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    changes = (newManual == manual);
+
+    if (manual) {
+      printManual();
+    } else {
+      printNormal();
+    }
+
+    newManual = manual;
+  }
+
+  public void printNormal(){
+    System.out.println("Normal");
+  }
+
+  public void printManual(){
+    System.out.println("Manual");
+  }
+
+  public static void setManual(boolean set)
+  {
+    manual = set;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return changes;
+  }
+}

src/main/java/frc4388/robot/subsystems/Extender.java

@@ -23,9 +23,6 @@ public class Extender extends SubsystemBase {
 
     m_extenderMotor = extenderMotor;
 
-    m_extenderMotor.restoreFactoryDefaults();
-    m_extenderMotor.setInverted(true);
-
     m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
     m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
     m_inLimit.enableLimitSwitch(true);

src/main/java/frc4388/robot/subsystems/Hood.java

@@ -10,6 +10,7 @@ import com.revrobotics.SparkMaxPIDController;
 import com.revrobotics.SparkMaxLimitSwitch;
 import com.revrobotics.CANSparkMax.ControlType;
 import com.revrobotics.CANSparkMax.SoftLimitDirection;
+import com.revrobotics.SparkMaxRelativeEncoder.Type;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;

src/main/java/frc4388/robot/subsystems/Intake.java

@@ -23,9 +23,6 @@ public class Intake extends SubsystemBase {
   public Intake(WPI_TalonFX intakeMotor, Serializer serializer) {
     m_intakeMotor = intakeMotor;
     m_serializer = serializer;
-
-    m_intakeMotor.setNeutralMode(NeutralMode.Brake);
-    m_intakeMotor.setInverted(false);
   }
 
   @Override