From 47c8e99c5f52579d2dba2066d41595b65aa1d259 Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Wed, 30 Apr 2025 00:01:40 -0600 Subject: [PATCH 1/2] code for letting kids drive --- .../java/org/blackknights/RobotContainer.java | 107 ++++++++++-------- 1 file changed, 58 insertions(+), 49 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 3ede56a..65c0be9 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -116,34 +116,34 @@ private void configureBindings() { swerveSubsystem, () -> primaryController.getLeftY() - * ConfigManager.getInstance().get("driver_max_speed", 3.5), + * ConfigManager.getInstance().get("demo_max_speed", 1.0), () -> primaryController.getLeftX() - * ConfigManager.getInstance().get("driver_max_speed", 3.5), + * ConfigManager.getInstance().get("demo_max_speed", 1.0), () -> -primaryController.getRightX() * Math.toRadians( ConfigManager.getInstance() - .get("driver_max_speed_rot", 360)), + .get("demo_max_speed_rot", 90)), true, true)); - primaryController - .leftBumper() - .whileTrue( - getPlaceCommand( - () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); +// primaryController +// .leftBumper() +// .whileTrue( +// getPlaceCommand( +// () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); primaryController .rightBumper() .whileTrue( - new SequentialCommandGroup( + new ParallelRaceGroup( new DriveCommands( swerveSubsystem, - primaryController::getLeftY, - primaryController::getLeftX, - () -> -primaryController.getRightX() * Math.PI, + () -> primaryController.getLeftY() / 2.0, + () -> primaryController.getLeftX() / 2.0, + () -> -primaryController.getRightX() * Math.PI/2, true, true), new ElevatorArmCommand( @@ -151,39 +151,26 @@ private void configureBindings() { armSubsystem, () -> ScoringConstants.ScoringHeights.INTAKE), new IntakeCommand( - intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)), - new RunCommand( - () -> - swerveSubsystem.drive( - ConfigManager.getInstance() - .get("back_mps", -1.0), - 0.0, - 0.0, - false, - false, - false), - swerveSubsystem) - .withTimeout( - ConfigManager.getInstance() - .get("back_time_sec", 0.2)))); + intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)) + ); elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem)); - primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); - - primaryController - .a() - .whileTrue( - new RunCommand( - () -> - elevatorSubsystem.setVoltage( - ConfigManager.getInstance() - .get("elevator_manual_zero", -2.0)), - elevatorSubsystem)); - - primaryController - .x() - .whileTrue(new InstantCommand(() -> elevatorSubsystem.resetEncoders())); +// primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); + +// primaryController +// .a() +// .whileTrue( +// new RunCommand( +// () -> +// elevatorSubsystem.setVoltage( +// ConfigManager.getInstance() +// .get("elevator_manual_zero", -2.0)), +// elevatorSubsystem)); +// +// primaryController +// .x() +// .whileTrue(new InstantCommand(() -> elevatorSubsystem.resetEncoders())); // primaryController // .povUp() @@ -195,9 +182,9 @@ private void configureBindings() { // .get("break_right_encoder_pos", // -10.0)))); - secondaryController - .rightStick() - .onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions)); +// secondaryController +// .rightStick() +// .onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions)); // SECONDARY CONTROLLER @@ -236,13 +223,35 @@ private void configureBindings() { armSubsystem, () -> ScoringConstants.ScoringHeights.L4)); +// secondaryController +// .leftBumper() +// .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); // +// +// secondaryController +// .rightBumper() +// .onTrue(new InstantCommand(() -> coralQueue.stepBackwards())); + secondaryController .leftBumper() - .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); // + .whileTrue( + getPlaceCommand( + () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); - secondaryController - .rightBumper() - .onTrue(new InstantCommand(() -> coralQueue.stepBackwards())); + secondaryController.rightBumper().onTrue(new DriveCommands( + swerveSubsystem, + () -> + primaryController.getLeftY() + * ConfigManager.getInstance().get("driver_max_speed", 3.5), + () -> + primaryController.getLeftX() + * ConfigManager.getInstance().get("driver_max_speed", 3.5), + () -> + -primaryController.getRightX() + * Math.toRadians( + ConfigManager.getInstance() + .get("driver_max_speed_rot", 360)), + true, + true)); secondaryController .rightTrigger(0.2) From c9678327a1190c807c3f212050cecdb6cf9bda2b Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Wed, 30 Apr 2025 10:29:51 -0600 Subject: [PATCH 2/2] not letting kids 500mph --- .../java/org/blackknights/RobotContainer.java | 131 +++++++++--------- 1 file changed, 68 insertions(+), 63 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 65c0be9..b912e30 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -128,49 +128,49 @@ private void configureBindings() { true, true)); -// primaryController -// .leftBumper() -// .whileTrue( -// getPlaceCommand( -// () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); + // primaryController + // .leftBumper() + // .whileTrue( + // getPlaceCommand( + // () -> coralQueue.getCurrentPosition(), () -> + // coralQueue.getNext())); primaryController .rightBumper() .whileTrue( - - new ParallelRaceGroup( - new DriveCommands( - swerveSubsystem, - () -> primaryController.getLeftY() / 2.0, - () -> primaryController.getLeftX() / 2.0, - () -> -primaryController.getRightX() * Math.PI/2, - true, - true), - new ElevatorArmCommand( - elevatorSubsystem, - armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE), - new IntakeCommand( - intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)) - ); + new ParallelRaceGroup( + new DriveCommands( + swerveSubsystem, + () -> primaryController.getLeftY() / 2.0, + () -> primaryController.getLeftX() / 2.0, + () -> -primaryController.getRightX() * Math.PI / 2, + true, + true), + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.INTAKE), + new IntakeCommand( + intakeSubsystem, IntakeCommand.IntakeMode.INTAKE))); elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem)); -// primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); - -// primaryController -// .a() -// .whileTrue( -// new RunCommand( -// () -> -// elevatorSubsystem.setVoltage( -// ConfigManager.getInstance() -// .get("elevator_manual_zero", -2.0)), -// elevatorSubsystem)); -// -// primaryController -// .x() -// .whileTrue(new InstantCommand(() -> elevatorSubsystem.resetEncoders())); + primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); + + // primaryController + // .a() + // .whileTrue( + // new RunCommand( + // () -> + // elevatorSubsystem.setVoltage( + // ConfigManager.getInstance() + // .get("elevator_manual_zero", + // -2.0)), + // elevatorSubsystem)); + // + // primaryController + // .x() + // .whileTrue(new InstantCommand(() -> elevatorSubsystem.resetEncoders())); // primaryController // .povUp() @@ -182,14 +182,14 @@ private void configureBindings() { // .get("break_right_encoder_pos", // -10.0)))); -// secondaryController -// .rightStick() -// .onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions)); + // secondaryController + // .rightStick() + // .onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions)); // SECONDARY CONTROLLER - climberSubsystem.setDefaultCommand( - new ClimberCommand(climberSubsystem, secondaryController)); + // climberSubsystem.setDefaultCommand( + // new ClimberCommand(climberSubsystem, secondaryController)); secondaryController .a() @@ -223,13 +223,13 @@ private void configureBindings() { armSubsystem, () -> ScoringConstants.ScoringHeights.L4)); -// secondaryController -// .leftBumper() -// .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); // -// -// secondaryController -// .rightBumper() -// .onTrue(new InstantCommand(() -> coralQueue.stepBackwards())); + // secondaryController + // .leftBumper() + // .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); // + // + // secondaryController + // .rightBumper() + // .onTrue(new InstantCommand(() -> coralQueue.stepBackwards())); secondaryController .leftBumper() @@ -237,21 +237,26 @@ private void configureBindings() { getPlaceCommand( () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); - secondaryController.rightBumper().onTrue(new DriveCommands( - swerveSubsystem, - () -> - primaryController.getLeftY() - * ConfigManager.getInstance().get("driver_max_speed", 3.5), - () -> - primaryController.getLeftX() - * ConfigManager.getInstance().get("driver_max_speed", 3.5), - () -> - -primaryController.getRightX() - * Math.toRadians( - ConfigManager.getInstance() - .get("driver_max_speed_rot", 360)), - true, - true)); + secondaryController + .rightBumper() + .onTrue( + new DriveCommands( + swerveSubsystem, + () -> + secondaryController.getLeftY() + * ConfigManager.getInstance() + .get("driver_max_speed", 3.5), + () -> + secondaryController.getLeftX() + * ConfigManager.getInstance() + .get("driver_max_speed", 3.5), + () -> + -secondaryController.getRightX() + * Math.toRadians( + ConfigManager.getInstance() + .get("driver_max_speed_rot", 360)), + true, + true)); secondaryController .rightTrigger(0.2)