diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 603bf0e..10acaaf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -38,6 +38,7 @@ import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; import com.pathplanner.lib.commands.PPSwerveControllerCommand; import org.ejml.dense.row.decomposition.hessenberg.TridiagonalDecompositionHouseholderOrig_DDRM; +import org.opencv.core.Point; import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.PIDController; @@ -69,6 +70,7 @@ import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ClimberCommands.RunClaw; +import frc4388.robot.commands.ClimberCommands.RunClimberPath; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; @@ -149,8 +151,13 @@ public class RobotContainer { .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); public static boolean softLimits = true; - public enum Mode {SHOOTER, CLIMBER}; - public Mode currentMode = Mode.SHOOTER; + + // mode switching + private enum ControlMode { SHOOTER, CLIMBER }; + private ControlMode currentControlMode = ControlMode.SHOOTER; + + private enum ClimberMode { MANUAL, AUTONOMOUS }; + private ClimberMode currentClimberMode = ClimberMode.MANUAL; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -159,28 +166,22 @@ public class RobotContainer { configureButtonBindings(); /* Default Commands */ - // moves climber in xy space with two-axis input from the operator controller - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7), - // m_robotClimber)); - - // IK command // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), // getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); - // Swerve Drive with Input + // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), - //getDriverController().getRightX(), getDriverController().getRightX(), getDriverController().getRightY(), true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); - // Intake with Triggers + + // Intake with Triggers m_robotIntake.setDefaultCommand( new RunCommand(() -> m_robotIntake.runWithTriggers( getOperatorController().getLeftTriggerAxis(), @@ -191,14 +192,12 @@ public class RobotContainer { // new ManageStorage(m_robotStorage, // m_robotBoomBoom, // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) - // ); - // Storage Management - /*m_robotStorage.setDefaultCommand( + + // Storage Management + m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), - m_robotStorage).withName("Storage manageStorage defaultCommand"));*/ + m_robotStorage).withName("Storage manageStorage defaultCommand")); + // Serializer Management m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), @@ -210,20 +209,22 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); m_robotHood.setDefaultCommand( new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); - + m_robotClimber.setDefaultCommand( new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getLeftX(), -getOperatorController().getRightY()); } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { + if (this.currentClimberMode.equals(ClimberMode.MANUAL)) { m_robotClimber.setMotors(-getOperatorController().getLeftX(), -getOperatorController().getRightY()); } + } }, m_robotClimber)); // m_robotTurret.setDefaultCommand( @@ -259,10 +260,6 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - - - - /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) @@ -274,9 +271,6 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); - - - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); @@ -297,7 +291,8 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - // // Left Bumper > Storage Out (note: neccessary?) + + // Left Bumper > Storage Out (note: necessary?) // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); @@ -316,7 +311,6 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); // .whileHeld% - /* Button Box Buttons */ new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) @@ -337,27 +331,19 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - - // .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) - // .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) - // .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber)))) - - // .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) - // .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))) - // .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( - // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); - - .whenPressed(new InstantCommand(() -> this.currentMode = Mode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentMode = Mode.SHOOTER)); - // .whenReleased(EnableClimber())); + .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); // control turret manual mode // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whenPressed(new InstantCommand(() -> this.currentClimberMode = ClimberMode.AUTONOMOUS)) + // TODO: actual climber autonomous command goes here (next line) + .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) + .whenReleased(new InstantCommand(() -> this.currentClimberMode = ClimberMode.MANUAL)); new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) diff --git a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java index 592d72d..84f66d1 100644 --- a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java @@ -23,7 +23,7 @@ public class RunClimberPath extends CommandBase { boolean endPath; /** Creates a new RunClimberPath. */ - public RunClimberPath(Point[] _path, Climber _climber, Claws _claws) { + public RunClimberPath(Climber _climber, Claws _claws, Point[] _path) { path = _path; endPath = false; diff --git a/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java b/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java deleted file mode 100644 index c610eda..0000000 --- a/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java +++ /dev/null @@ -1,50 +0,0 @@ -// 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; -import frc4388.robot.RobotContainer; -import frc4388.robot.subsystems.Climber; -import frc4388.robot.subsystems.Turret; - -public class RunTurretOrClimber extends CommandBase { - - Turret turret; - Climber climber; - - /** Creates a new RunTurretOrClimber. */ - public RunTurretOrClimber(Turret turret, Climber climber) { - // Use addRequirements() here to declare subsystem dependencies. - - this.turret = turret; - this.climber = climber; - - addRequirements(this.turret, this.climber); - } - - // 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() { - // if (RobotContainer.currentMode.equals(RobotContainer.Mode.TURRET)) { - // this.turret.runTurretWithInput(getOperatorController().getLeftX()) - // } else if (RobotContainer.currentMode.equals(RobotContainer.Mode.CLIMBER)) { - - // } - } - - // 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 false; - } -}