diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2d87fc1..f3dfd97 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 416d090..04a55bc 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/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() { - boolean robotManual = m_robotContainer.manual; - - if (robotManual) { - m_robotContainer.configureManualButtonBindings(); - } else { - m_robotContainer.configureAutomaticButtonBindings(); - } - } + public void teleopPeriodic() {} @Override public void testInit() { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8880940..c9a3ea2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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) - .whenPressed(() -> m_robotIntake.runExtender(true)); + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry)); // Y > Retract Intake - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(() -> m_robotIntake.runExtender(false));*/ + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .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) - .whenPressed(new RunCommand(() -> setManual(true))) - .whenReleased(new RunCommand(() -> setManual(false))); + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) + .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) + .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); - // new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) - // .whileHeld(new RunCommand(() -> System.out.println("MiddleSwitch"))); + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + .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() { - return m_buttonFox; + public ButtonBox getButtonBox() { + return m_buttonBox; } public void setManual(boolean set) { diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 8466940..5983d26 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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(); + } + } diff --git a/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java b/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java new file mode 100644 index 0000000..3a61d55 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index f0749a5..108784c 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/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); diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index d486374..a74ad70 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/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; diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 4c086b2..5d6f001 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/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