From 7c3b61c9d6ea3de98a74fd0cedff5c9b34421ad0 Mon Sep 17 00:00:00 2001 From: Derek Date: Thu, 19 Oct 2023 16:28:51 -0700 Subject: [PATCH 01/63] Controls remapped Controls have been changed to match the controls from the FRC-2023 code --- src/main/java/frc/robot/RobotContainer.java | 139 ++++++++++-------- .../robot/commands/GroundPickupCommand.java | 2 +- 2 files changed, 81 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69c8fd7..5598bf2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,7 +34,6 @@ import frc.robot.autonomous.commands.N9_1ConePlus2CubeMobility; import frc.robot.autonomous.commands.N9_1ConePlusMobility; import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage; -import frc.robot.commands.AlignGamepieceCommand; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.DriveToPlaceCommand; @@ -46,7 +45,6 @@ import frc.robot.commands.HashMapCommand; import frc.robot.commands.IntakeModeCommand; import frc.robot.commands.RotateVectorDriveCommand; -import frc.robot.commands.RotateVelocityDriveCommand; import frc.robot.commands.ScoreCommand; import frc.robot.commands.ScoreCommand.ScoreStep; import frc.robot.commands.SetZeroModeCommand; @@ -68,7 +66,6 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeSelection; import frc.util.NodeSelectorUtility.NodeType; -import frc.util.NodeSelectorUtility.ScoreTypeIdentifier; import frc.util.SharedReference; import frc.util.Util; import frc.util.pathing.AlliancePose2d; @@ -215,11 +212,9 @@ private void configureButtonBindings() { will.back() .onTrue(new InstantCommand(drivebaseSubsystem::smartZeroGyroscope, drivebaseSubsystem)); - // pov(-1) is the case when no pov is pressed, so doing while false will bind this command to - // any pov angle - will.pov(-1).whileFalse(new DefenseModeCommand(drivebaseSubsystem)); + will.leftBumper().onTrue(new DefenseModeCommand(drivebaseSubsystem)); - will.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + will.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); jason.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); DoubleSupplier rotation = @@ -236,15 +231,14 @@ private void configureButtonBindings() { /** percent of fraction power */ (will.getHID().getAButton() ? .3 : .8); - new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0) - .whileTrue( - new RotateVelocityDriveCommand( - drivebaseSubsystem, - translationXSupplier, - translationYSupplier, - rotationVelocity, - will.rightBumper(), - will.leftBumper())); + // new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0) + // .whileTrue( + // new RotateVelocityDriveCommand( + // drivebaseSubsystem, + // translationXSupplier, + // translationYSupplier, + // rotationVelocity, + // will.rightBumper())); new Trigger( () -> @@ -261,39 +255,29 @@ private void configureButtonBindings() { // start driving to score will.b() - .onTrue( - new DriveToPlaceCommand( - drivebaseSubsystem, - manueverGenerator, - () -> currentNodeSelection.get().nodeStack().position().get(), - translationXSupplier, - translationYSupplier, - will.rightBumper(), - Optional.of(rgbSubsystem), - Optional.of(will.getHID())) - .andThen( - new AlignGamepieceCommand( - drivebaseSubsystem, - manueverGenerator, - () -> currentNodeSelection.get().nodeStack().position().get(), - translationXSupplier, - translationYSupplier, - will.rightBumper(), - Optional.of(rgbSubsystem), - Optional.of(will.getHID())))); - - will.y() .onTrue( new DriveToPlaceCommand( drivebaseSubsystem, manueverGenerator, - (new AlliancePose2d(15.3639 - 1.5, 7.3965, Rotation2d.fromDegrees(0)))::get, + () -> currentNodeSelection.get().nodeStack().position().get(), translationXSupplier, translationYSupplier, will.rightBumper(), Optional.of(rgbSubsystem), Optional.of(will.getHID()))); + // will.y() + // .onTrue( + // new DriveToPlaceCommand( + // drivebaseSubsystem, + // manueverGenerator, + // (new AlliancePose2d(15.3639 - 1.5, 7.3965, Rotation2d.fromDegrees(0)))::get, + // translationXSupplier, + // translationYSupplier, + // will.rightBumper(), + // Optional.of(rgbSubsystem), + // Optional.of(will.getHID()))); + will.x() .onTrue( new EngageCommand( @@ -305,37 +289,75 @@ private void configureButtonBindings() { .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); jasonLayer .off(jason.rightTrigger()) - .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.OUTTAKE)); - jasonLayer.off(jason.x()).onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE)); + jasonLayer + .off(jason.x()) + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) + .onTrue( + new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)); // intake presets // jasonLayer // .off(jason.a()) - // .onTrue(new ScoreCommand( elevatorSubsystem, Setpoints.GROUND_INTAKE)) + // .onTrue(new ScoreCommand(intakeSubsystem, elevatorSubsystem, Setpoints.GROUND_INTAKE)) // .whileTrue( - // new ForceOuttakeSubsystemModeCommand( - // IntakeMode.INTAKE)); + // new ForceintakeSubsystemModeCommand(intakeSubsystem, + // Modes.INTAKE)); - // shelf intake - jasonLayer - .off(jason.b()) - // FIXME: This error is here to kind of guide you... + will.povUp() .onTrue( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); + .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); - // ground pickup + // reset jasonLayer - .off(jason.a()) + .off(jason.y()) + .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); + jason.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); + + will.povLeft() + .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) + .onTrue(new SetZeroModeCommand(elevatorSubsystem)); + + will.povDown() .onTrue( new GroundPickupCommand( + intakeSubsystem, elevatorSubsystem, + () -> jason.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); + + jason + .leftBumper() + .onTrue( + new GroundPickupCommand( intakeSubsystem, - () -> - jason.getHID().getPOV() == 180 - ? IntakeSubsystem.Modes.HOLD - : IntakeSubsystem.Modes.INTAKE)); + elevatorSubsystem, + () -> jason.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); + + jason + .povUp() + .onTrue( + new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) + .alongWith( + new ElevatorPositionCommand( + elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))); + + // jason.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem)); + + jason + .back() + .whileTrue( + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE) + .alongWith( + new ElevatorPositionCommand( + elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))) + .onFalse( + new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); // scoring // jasonLayer @@ -366,17 +388,16 @@ private void configureButtonBindings() { var scoreCommandMap = new HashMap(); - for (ScoreTypeIdentifier scoreType : Constants.SCORE_STEP_MAP.keySet()) + for (var scoreType : Constants.SCORE_STEP_MAP.keySet()) scoreCommandMap.put( scoreType, new ScoreCommand( intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), - jason.leftBumper())); + will.povRight())); - jasonLayer - .on(jason.x()) + will.povRight() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 4639b10..f169246 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -17,8 +17,8 @@ public class GroundPickupCommand extends SequentialCommandGroup { /** Creates a new GroundPickupCommand. */ public GroundPickupCommand( - ElevatorSubsystem elevatorSubsystem, IntakeSubsystem intakeSubsystem, + ElevatorSubsystem elevatorSubsystem, Supplier modeSupplier) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); From ae4feb14d3551c0ce52c5cf24ac026ca32082d86 Mon Sep 17 00:00:00 2001 From: Derek Date: Thu, 19 Oct 2023 17:07:37 -0700 Subject: [PATCH 02/63] Changed driver names --- src/main/java/frc/robot/RobotContainer.java | 148 +++++++++++--------- 1 file changed, 81 insertions(+), 67 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5598bf2..d16d3ea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -106,11 +106,11 @@ public class RobotContainer { new SharedReference<>(new NodeSelection(NodeSelectorUtility.defaultNodeStack, Height.HIGH)); /** controller 1 */ - private final CommandXboxController jason = new CommandXboxController(1); + private final CommandXboxController jacob = new CommandXboxController(1); /** controller 1 layer */ - private final Layer jasonLayer = new Layer(jason.rightBumper()); + private final Layer jacobLayer = new Layer(jacob.rightBumper()); /** controller 0 */ - private final CommandXboxController will = new CommandXboxController(0); + private final CommandXboxController anthony = new CommandXboxController(0); /** the sendable chooser to select which auto to run. */ private final SendableChooser autoSelector = new SendableChooser<>(); @@ -121,9 +121,9 @@ public class RobotContainer { /* drive joystick "y" is passed to x because controller is inverted */ private final DoubleSupplier translationXSupplier = - () -> (-modifyAxis(will.getLeftY()) * Drive.MAX_VELOCITY_METERS_PER_SECOND); + () -> (-modifyAxis(anthony.getLeftY()) * Drive.MAX_VELOCITY_METERS_PER_SECOND); private final DoubleSupplier translationYSupplier = - () -> (-modifyAxis(will.getLeftX()) * Drive.MAX_VELOCITY_METERS_PER_SECOND); + () -> (-modifyAxis(anthony.getLeftX()) * Drive.MAX_VELOCITY_METERS_PER_SECOND); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -137,23 +137,23 @@ public RobotContainer() { drivebaseSubsystem, translationXSupplier, translationYSupplier, - will.rightBumper(), - will.leftBumper())); + anthony.rightBumper(), + anthony.leftBumper())); // // FIXME: This error is here to kind of guide you... // elevatorSubsystem.setDefaultCommand( // new ArmManualCommand( // elevatorSubsystem, - // () -> ControllerUtil.deadband(-jason.getLeftY(), 0.2), - // () -> ControllerUtil.deadband(jason.getRightY(), 0.2))); + // () -> ControllerUtil.deadband(-jacob.getLeftY(), 0.2), + // () -> ControllerUtil.deadband(jacob.getRightY(), 0.2))); elevatorSubsystem.setDefaultCommand( new ElevatorManualCommand( - elevatorSubsystem, () -> ControllerUtil.deadband(jason.getLeftY(), 0.2))); + elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getLeftY(), 0.2))); elevatorSubsystem.setDefaultCommand( new WristManualCommand( - elevatorSubsystem, () -> ControllerUtil.deadband(jason.getRightY(), 0.2))); + elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getRightY(), 0.2))); SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT); SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA); @@ -174,7 +174,7 @@ public RobotContainer() { */ public void containerTeleopInit() { // runs when teleop happens - CommandScheduler.getInstance().schedule(new VibrateHIDCommand(jason.getHID(), 5, .5)); + CommandScheduler.getInstance().schedule(new VibrateHIDCommand(jacob.getHID(), 5, .5)); } /** @@ -200,28 +200,36 @@ public void containerMatchStarting() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - // vibrate jason controller when in layer - jasonLayer.whenChanged( + + /* Current Controls: + * Back buttons: Zero gyroscope, + */ + + // vibrate jacob controller when in layer + jacobLayer.whenChanged( (enabled) -> { final double power = enabled ? .1 : 0; - jason.getHID().setRumble(RumbleType.kLeftRumble, power); - jason.getHID().setRumble(RumbleType.kRightRumble, power); + jacob.getHID().setRumble(RumbleType.kLeftRumble, power); + jacob.getHID().setRumble(RumbleType.kRightRumble, power); }); - will.start().onTrue(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem)); - will.back() + anthony + .start() + .onTrue(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem)); + anthony + .back() .onTrue(new InstantCommand(drivebaseSubsystem::smartZeroGyroscope, drivebaseSubsystem)); - will.leftBumper().onTrue(new DefenseModeCommand(drivebaseSubsystem)); + anthony.leftBumper().onTrue(new DefenseModeCommand(drivebaseSubsystem)); - will.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); - jason.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); + anthony.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + jacob.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); DoubleSupplier rotation = exponential( () -> ControllerUtil.deadband( - (will.getRightTriggerAxis() + -will.getLeftTriggerAxis()), .1), + (anthony.getRightTriggerAxis() + -anthony.getLeftTriggerAxis()), .1), 2); DoubleSupplier rotationVelocity = () -> @@ -229,7 +237,7 @@ private void configureButtonBindings() { * Drive.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * /** percent of fraction power */ - (will.getHID().getAButton() ? .3 : .8); + (anthony.getHID().getAButton() ? .3 : .8); // new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0) // .whileTrue( @@ -238,23 +246,24 @@ private void configureButtonBindings() { // translationXSupplier, // translationYSupplier, // rotationVelocity, - // will.rightBumper())); + // anthony.rightBumper())); new Trigger( () -> - Util.vectorMagnitude(will.getRightY(), will.getRightX()) + Util.vectorMagnitude(anthony.getRightY(), anthony.getRightX()) > Drive.ROTATE_VECTOR_MAGNITUDE) .onTrue( new RotateVectorDriveCommand( drivebaseSubsystem, translationXSupplier, translationYSupplier, - will::getRightY, - will::getRightX, - will.rightBumper())); + anthony::getRightY, + anthony::getRightX, + anthony.rightBumper())); // start driving to score - will.b() + anthony + .b() .onTrue( new DriveToPlaceCommand( drivebaseSubsystem, @@ -262,11 +271,11 @@ private void configureButtonBindings() { () -> currentNodeSelection.get().nodeStack().position().get(), translationXSupplier, translationYSupplier, - will.rightBumper(), + anthony.rightBumper(), Optional.of(rgbSubsystem), - Optional.of(will.getHID()))); + Optional.of(anthony.getHID()))); - // will.y() + // anthony.y() // .onTrue( // new DriveToPlaceCommand( // drivebaseSubsystem, @@ -274,70 +283,74 @@ private void configureButtonBindings() { // (new AlliancePose2d(15.3639 - 1.5, 7.3965, Rotation2d.fromDegrees(0)))::get, // translationXSupplier, // translationYSupplier, - // will.rightBumper(), + // anthony.rightBumper(), // Optional.of(rgbSubsystem), - // Optional.of(will.getHID()))); + // Optional.of(anthony.getHID()))); - will.x() + anthony + .x() .onTrue( new EngageCommand( drivebaseSubsystem, elevatorSubsystem, EngageCommand.EngageDirection.GO_BACKWARD)); // outtake states - jasonLayer - .off(jason.leftTrigger()) + jacobLayer + .off(jacob.leftTrigger()) .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); - jasonLayer - .off(jason.rightTrigger()) + jacobLayer + .off(jacob.rightTrigger()) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE)); - jasonLayer - .off(jason.x()) + jacobLayer + .off(jacob.x()) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) .onTrue( new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)); // intake presets - // jasonLayer - // .off(jason.a()) + // jacobLayer + // .off(jacob.a()) // .onTrue(new ScoreCommand(intakeSubsystem, elevatorSubsystem, Setpoints.GROUND_INTAKE)) // .whileTrue( // new ForceintakeSubsystemModeCommand(intakeSubsystem, // Modes.INTAKE)); - will.povUp() + anthony + .povUp() .onTrue( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); // reset - jasonLayer - .off(jason.y()) + jacobLayer + .off(jacob.y()) .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); - jason.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); + jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); - will.povLeft() + anthony + .povLeft() .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) .onTrue(new SetZeroModeCommand(elevatorSubsystem)); - will.povDown() + anthony + .povDown() .onTrue( new GroundPickupCommand( intakeSubsystem, elevatorSubsystem, - () -> jason.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); + () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); - jason + jacob .leftBumper() .onTrue( new GroundPickupCommand( intakeSubsystem, elevatorSubsystem, - () -> jason.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); + () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); - jason + jacob .povUp() .onTrue( new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) @@ -345,9 +358,9 @@ private void configureButtonBindings() { new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))); - // jason.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem)); + // jacob.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem)); - jason + jacob .back() .whileTrue( new IntakeModeCommand(intakeSubsystem, Modes.INTAKE) @@ -360,26 +373,26 @@ private void configureButtonBindings() { .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); // scoring - // jasonLayer - // .on(jason.a()) + // jacobLayer + // .on(jacob.a()) // low - jasonLayer - .on(jason.a()) + jacobLayer + .on(jacob.a()) .onTrue( new InstantCommand( () -> currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.LOW)))); - jasonLayer - .on(jason.b()) + jacobLayer + .on(jacob.b()) .onTrue( new InstantCommand( () -> currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.MID)), elevatorSubsystem)); - jasonLayer - .on(jason.y()) + jacobLayer + .on(jacob.y()) .onTrue( new InstantCommand( () -> @@ -395,15 +408,16 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), - will.povRight())); + anthony.povRight())); - will.povRight() + anthony + .povRight() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); - jason.povRight().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(1)))); - jason.povLeft().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(-1)))); + jacob.povRight().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(1)))); + jacob.povLeft().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(-1)))); // control the lights currentNodeSelection.subscribe( From a5485ad93faf991006d9456835a76e42c87f2ff6 Mon Sep 17 00:00:00 2001 From: Derek Date: Tue, 24 Oct 2023 16:12:44 -0700 Subject: [PATCH 03/63] Updated zero mode command --- src/main/java/frc/robot/Constants.java | 5 +++-- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++-- .../java/frc/robot/commands/SetZeroModeCommand.java | 5 ++++- .../java/frc/robot/subsystems/ElevatorSubsystem.java | 10 +++++----- .../java/frc/robot/subsystems/IntakeSubsystem.java | 3 ++- 5 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 27fac49..67c4dfd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -201,10 +201,11 @@ public static final class Ports { public static final class Setpoints { public static final ElevatorState STOWED = new ElevatorState(20, 0); public static final ElevatorState SHELF_INTAKE = new ElevatorState(20, 20); - public static final ElevatorState GROUND_INTAKE = new ElevatorState(0, 0); + public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_HEIGHT, 0); public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_HEIGHT, 0); - public static final ElevatorState SCORE_MID = new ElevatorState(15, 20); + public static final ElevatorState SCORE_MID = new ElevatorState(MAX_HEIGHT / 2, 20); public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_HEIGHT, 40); + public static final ElevatorState ZERO = new ElevatorState(MIN_HEIGHT, 0); } public static final double MAX_HEIGHT = 20; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d16d3ea..d604d27 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -216,6 +216,7 @@ private void configureButtonBindings() { anthony .start() .onTrue(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem)); + anthony .back() .onTrue(new InstantCommand(drivebaseSubsystem::smartZeroGyroscope, drivebaseSubsystem)); @@ -223,6 +224,7 @@ private void configureButtonBindings() { anthony.leftBumper().onTrue(new DefenseModeCommand(drivebaseSubsystem)); anthony.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + jacob.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); DoubleSupplier rotation = @@ -231,6 +233,7 @@ private void configureButtonBindings() { ControllerUtil.deadband( (anthony.getRightTriggerAxis() + -anthony.getLeftTriggerAxis()), .1), 2); + DoubleSupplier rotationVelocity = () -> rotation.getAsDouble() @@ -297,9 +300,11 @@ private void configureButtonBindings() { jacobLayer .off(jacob.leftTrigger()) .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); + jacobLayer .off(jacob.rightTrigger()) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE)); + jacobLayer .off(jacob.x()) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) @@ -326,11 +331,12 @@ private void configureButtonBindings() { .off(jacob.y()) .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); + jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); anthony .povLeft() - .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) + .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.ZERO)) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) .onTrue(new SetZeroModeCommand(elevatorSubsystem)); @@ -356,7 +362,7 @@ private void configureButtonBindings() { new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) .alongWith( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))); + elevatorSubsystem, Constants.Elevator.Setpoints.GROUND_INTAKE))); // jacob.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem)); diff --git a/src/main/java/frc/robot/commands/SetZeroModeCommand.java b/src/main/java/frc/robot/commands/SetZeroModeCommand.java index bbc0b19..3bcb385 100644 --- a/src/main/java/frc/robot/commands/SetZeroModeCommand.java +++ b/src/main/java/frc/robot/commands/SetZeroModeCommand.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; import frc.robot.subsystems.ElevatorSubsystem; import java.util.Optional; @@ -34,7 +35,9 @@ public SetZeroModeCommand(ElevatorSubsystem elevatorSubsystem, boolean includeWr // FIXME make it zero elevator + wrist @Override - public void initialize() {} + public void initialize() { + new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.ZERO); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index cfa0649..b59ac3a 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -28,7 +28,7 @@ public class ElevatorSubsystem extends SubsystemBase { private double currentHeight; private double targetHeight; - private double currentAngle; + private double currentWristAngle; private double targetAngle; private double statorCurrentLimit; @@ -63,7 +63,7 @@ public ElevatorSubsystem() { currentHeight = 0.0; targetHeight = 0.0; - currentAngle = 0.0; + currentWristAngle = 0.0; targetAngle = 0.0; statorCurrentLimit = 50.0; @@ -89,7 +89,7 @@ public ElevatorSubsystem() { tab.addDouble("Wrist Motor Position", () -> canCoder.getAbsolutePosition()); tab.addDouble("Wrist Target Angle", () -> targetAngle); - tab.addDouble("Wrist Current Angle", () -> currentAngle); + tab.addDouble("Wrist Current Angle", () -> currentWristAngle); tab.addDouble("Elevator Target Height", () -> targetHeight); tab.addDouble("Elevator Current Height", () -> currentHeight); } @@ -142,7 +142,7 @@ public void setTargetAngle(double targetAngle) { @Override public void periodic() { currentHeight = getHeight(); - currentAngle = getCurrentAngleDegrees(); + currentWristAngle = getCurrentAngleDegrees(); if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit || proxySensor.get() == false) { @@ -154,6 +154,6 @@ public void periodic() { wristMotor.set( TalonFXControlMode.PercentOutput, - MathUtil.clamp(wristController.calculate(currentAngle, targetAngle), -0.25, 0.25)); + MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25)); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 790f738..a19eacd 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Constants.Intake; public class IntakeSubsystem extends SubsystemBase { @@ -22,7 +23,7 @@ public class IntakeSubsystem extends SubsystemBase { /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { - intakeMotor = new TalonFX(0); + intakeMotor = new TalonFX(Constants.Intake.Ports.INTAKE_MOTOR_PORT); currentIntakeMode = Modes.OFF; From 9f2496ee8e1114c71ac6c8b3f9c93304ff6636a2 Mon Sep 17 00:00:00 2001 From: Derek Date: Tue, 24 Oct 2023 18:05:36 -0700 Subject: [PATCH 04/63] Gear ratios --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 4 ---- src/main/java/frc/robot/commands/ScoreCommand.java | 3 --- src/main/java/frc/robot/commands/SetZeroModeCommand.java | 1 - 4 files changed, 3 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 67c4dfd..9da6259 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -212,12 +212,12 @@ public static final class Setpoints { public static final double MIN_HEIGHT = 0; public static final int ELEVATOR_TICKS = 2048; - public static final double ELEVATOR_GEAR_RATIO = 1.0; - public static final double ELEVATOR_GEAR_CIRCUMFERENCE = 1.5 * Math.PI; + public static final double ELEVATOR_GEAR_RATIO = 1 / 9.9206; + public static final double ELEVATOR_GEAR_CIRCUMFERENCE = 1.432 * Math.PI; public static final int WRIST_TICKS = 2048; public static final double WRIST_DEGREES = 360; - public static final double WRIST_GEAR_RATIO = 0.061; + public static final double WRIST_GEAR_RATIO = 1 / 75; public static final double ANGLE_EPSILON = 0.5; public static final double HEIGHT_EPSILON = 5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d604d27..e70def0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -201,10 +201,6 @@ public void containerMatchStarting() { */ private void configureButtonBindings() { - /* Current Controls: - * Back buttons: Zero gyroscope, - */ - // vibrate jacob controller when in layer jacobLayer.whenChanged( (enabled) -> { diff --git a/src/main/java/frc/robot/commands/ScoreCommand.java b/src/main/java/frc/robot/commands/ScoreCommand.java index 46d3c69..e51a883 100644 --- a/src/main/java/frc/robot/commands/ScoreCommand.java +++ b/src/main/java/frc/robot/commands/ScoreCommand.java @@ -70,12 +70,9 @@ private Command createStep(ScoreStep scoreStep) { var elevatorState = scoreStep.elevatorState(); var intakeState = scoreStep.intakeState(); if (elevatorState.isPresent() && intakeState.isPresent()) { - // FIXME: we need a working elevatorposition command here return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()) .deadlineWith(new IntakeModeCommand(intakeSubsystem, intakeState.get())); } else if (elevatorState.isPresent()) { - - // FIXME: we need a working elevatorposition command here return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()); } else if (intakeState.isPresent()) { return new IntakeModeCommand(intakeSubsystem, intakeState.get()); diff --git a/src/main/java/frc/robot/commands/SetZeroModeCommand.java b/src/main/java/frc/robot/commands/SetZeroModeCommand.java index 3bcb385..9c50fcd 100644 --- a/src/main/java/frc/robot/commands/SetZeroModeCommand.java +++ b/src/main/java/frc/robot/commands/SetZeroModeCommand.java @@ -33,7 +33,6 @@ public SetZeroModeCommand(ElevatorSubsystem elevatorSubsystem, boolean includeWr this(elevatorSubsystem, Optional.of(includeWrist), Optional.of(true)); } - // FIXME make it zero elevator + wrist @Override public void initialize() { new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.ZERO); From 1dcbdea021cd6f9f39f609f19cd8d3196fab54bd Mon Sep 17 00:00:00 2001 From: audrey Date: Tue, 24 Oct 2023 18:07:36 -0700 Subject: [PATCH 05/63] angular offsets for wrist --- src/main/java/frc/robot/Constants.java | 4 +++ .../java/frc/robot/subsystems/ElevatorIO.java | 29 +++++++++++++++++++ .../robot/subsystems/ElevatorSubsystem.java | 22 ++++++++++---- 3 files changed, 50 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/ElevatorIO.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 27fac49..03ad624 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.Constants.Elevator; import frc.robot.Constants.Drive.Dims; import frc.robot.commands.ScoreCommand.ScoreStep; import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; @@ -220,6 +221,9 @@ public static final class Setpoints { public static final double ANGLE_EPSILON = 0.5; public static final double HEIGHT_EPSILON = 5; + c static final double ANGULAR_OFFSET = 0; + } + } public static final class Intake { diff --git a/src/main/java/frc/robot/subsystems/ElevatorIO.java b/src/main/java/frc/robot/subsystems/ElevatorIO.java new file mode 100644 index 0000000..24f2f68 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorIO.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + @AutoLog + public static class FlywheelIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double[] currentAmps = new double[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(FlywheelIOInputs inputs) { + } + + /** Run closed loop at the specified velocity. */ + public default void setVelocity(double velocityRadPerSec, double ffVolts) { + } + + /** Stop in open loop. */ + public default void stop() { + } + + /** Set velocity PID constants. */ + public default void configurePID(double kP, double kI, double kD) { + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index cfa0649..6cd47a0 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -12,7 +12,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.wpilibj.DigitalInput; +// import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -40,7 +40,7 @@ public class ElevatorSubsystem extends SubsystemBase { private double filterOutput; - private DigitalInput proxySensor; + // private DigitalInput proxySensor; // add soft limits - check 2022 frc code @@ -59,7 +59,7 @@ public ElevatorSubsystem() { heightController = new PIDController(0.0001, 0, 0); wristController = new PIDController(0, 0, 0); canCoder = new CANCoder(0); - proxySensor = new DigitalInput(0); + // proxySensor = new DigitalInput(0); currentHeight = 0.0; targetHeight = 0.0; @@ -81,9 +81,17 @@ public ElevatorSubsystem() { rightMotor.configForwardSoftLimitThreshold(heightToTicks(Constants.Elevator.MAX_HEIGHT), 20); rightMotor.configReverseSoftLimitThreshold(heightToTicks(Elevator.MIN_HEIGHT), 20); + wristMotor.configForwardSoftLimitThreshold(angleToTicks(0)); rightMotor.configForwardSoftLimitEnable(true, 20); rightMotor.configReverseSoftLimitEnable(true, 20); + wristMotor.configForwardSoftLimitThreshold(angleToTicks(0)); + + canCoder.configMagnetOffset(Elevator.ANGULAR_OFFSET); + + canCoder.configSensorDirection(true); + + canCoder.setPositionToAbsolute(10); // ms filter = LinearFilter.movingAverage(30); @@ -135,6 +143,10 @@ public double getCurrentAngleDegrees() { return canCoder.getAbsolutePosition(); } + private double angleToTicks(double angle) { + return angle / Elevator.WRIST_TICKS; + } + public void setTargetAngle(double targetAngle) { this.targetAngle = targetAngle; } @@ -144,8 +156,8 @@ public void periodic() { currentHeight = getHeight(); currentAngle = getCurrentAngleDegrees(); - if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit - || proxySensor.get() == false) { + if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit) { + // || proxySensor.get() == false) { double motorPower = heightController.calculate(currentHeight, targetHeight); rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); } else { From b9e9e970f6cd466fb60dd09312684fbbfddcb178 Mon Sep 17 00:00:00 2001 From: audrey Date: Tue, 24 Oct 2023 18:12:20 -0700 Subject: [PATCH 06/63] fix funny --- src/main/java/frc/robot/Constants.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 68bc6e2..54996d2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -222,10 +222,9 @@ public static final class Setpoints { public static final double ANGLE_EPSILON = 0.5; public static final double HEIGHT_EPSILON = 5; - c static final double ANGULAR_OFFSET = 0; + public static final double ANGULAR_OFFSET = 0; } - } public static final class Intake { From b08ce22c0a9a54faeaaf29ae69665007f8833fc7 Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 25 Oct 2023 22:34:22 -0700 Subject: [PATCH 07/63] delete funny --- .../java/frc/robot/subsystems/ElevatorIO.java | 29 ------------------- 1 file changed, 29 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/ElevatorIO.java diff --git a/src/main/java/frc/robot/subsystems/ElevatorIO.java b/src/main/java/frc/robot/subsystems/ElevatorIO.java deleted file mode 100644 index 24f2f68..0000000 --- a/src/main/java/frc/robot/subsystems/ElevatorIO.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.subsystems; - -import org.littletonrobotics.junction.AutoLog; - -public interface ElevatorIO { - @AutoLog - public static class FlywheelIOInputs { - public double positionRad = 0.0; - public double velocityRadPerSec = 0.0; - public double appliedVolts = 0.0; - public double[] currentAmps = new double[] {}; - } - - /** Updates the set of loggable inputs. */ - public default void updateInputs(FlywheelIOInputs inputs) { - } - - /** Run closed loop at the specified velocity. */ - public default void setVelocity(double velocityRadPerSec, double ffVolts) { - } - - /** Stop in open loop. */ - public default void stop() { - } - - /** Set velocity PID constants. */ - public default void configurePID(double kP, double kI, double kD) { - } -} From 27889823214fd06a9b29c5ae0d984500c1ae62a5 Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 25 Oct 2023 22:36:48 -0700 Subject: [PATCH 08/63] spotless funny it builds now :D --- src/main/java/frc/robot/Constants.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 54996d2..03e0d84 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,8 +16,8 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.Constants.Elevator; import frc.robot.Constants.Drive.Dims; +import frc.robot.Constants.Elevator; import frc.robot.commands.ScoreCommand.ScoreStep; import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.IntakeSubsystem; @@ -225,7 +225,6 @@ public static final class Setpoints { public static final double ANGULAR_OFFSET = 0; } - public static final class Intake { public static final double INTAKE_PERCENT = 1; From a839436da0f8e3f0991dccb4239ce032d17818c5 Mon Sep 17 00:00:00 2001 From: audrey Date: Thu, 26 Oct 2023 16:07:31 -0700 Subject: [PATCH 09/63] update wrist gear ratio --- src/main/java/frc/robot/Constants.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 54996d2..90f302a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,8 +16,8 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.Constants.Elevator; import frc.robot.Constants.Drive.Dims; +import frc.robot.Constants.Elevator; import frc.robot.commands.ScoreCommand.ScoreStep; import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.IntakeSubsystem; @@ -218,14 +218,13 @@ public static final class Setpoints { public static final int WRIST_TICKS = 2048; public static final double WRIST_DEGREES = 360; - public static final double WRIST_GEAR_RATIO = 1 / 75; + public static final double WRIST_GEAR_RATIO = 1 / 38.6719; public static final double ANGLE_EPSILON = 0.5; public static final double HEIGHT_EPSILON = 5; public static final double ANGULAR_OFFSET = 0; } - public static final class Intake { public static final double INTAKE_PERCENT = 1; From d53fc6b216cb4d4dd1a48a8881eab3fe62e9dd6f Mon Sep 17 00:00:00 2001 From: Derek Date: Thu, 26 Oct 2023 16:56:30 -0700 Subject: [PATCH 10/63] Updated Can IDs --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 90f302a..ced32b5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -157,8 +157,8 @@ public static final class Module1 { // historically front right } public static final class Module2 { // historically front left - public static final int DRIVE_MOTOR = CAN.at(11, "module 2 drive motor"); - public static final int STEER_MOTOR = CAN.at(10, "module 2 steer motor"); + public static final int DRIVE_MOTOR = CAN.at(10, "module 2 drive motor"); + public static final int STEER_MOTOR = CAN.at(11, "module 2 steer motor"); public static final int STEER_ENCODER = CAN.at(25, "module 2 steer encoder"); public static final double STEER_OFFSET = @@ -194,8 +194,8 @@ public static final class Module4 { // historically back right public static final class Elevator { public static final class Ports { - public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(0, "elevator left motor"); - public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(0, "elevator right motor"); + public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(14, "elevator left motor"); + public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(15, "elevator right motor"); public static final int WRIST_MOTOR_PORT = CAN.at(0, "wrist motor port"); } From 13141d60e0d2f2ce791df3af5a3281c2589e5345 Mon Sep 17 00:00:00 2001 From: Derek Date: Thu, 26 Oct 2023 18:32:59 -0700 Subject: [PATCH 11/63] Working intake --- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 172 +++++++++--------- .../frc/robot/subsystems/IntakeSubsystem.java | 22 ++- 3 files changed, 105 insertions(+), 95 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ced32b5..c9de362 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -227,11 +227,11 @@ public static final class Setpoints { public static final class Intake { - public static final double INTAKE_PERCENT = 1; + public static final double INTAKE_PERCENT = 0.25; - public static final double OUTTAKE_PERCENT = -1; + public static final double OUTTAKE_PERCENT = -0.25; - public static final double HOLD_PERCENT = 0.15; + public static final double HOLD_PERCENT = 0.1; public static final class Ports { public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor"); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e70def0..12dc965 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,7 +5,6 @@ package frc.robot; import com.pathplanner.lib.server.PathPlannerServer; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -24,16 +23,6 @@ import frc.robot.Constants.Config; import frc.robot.Constants.Drive; import frc.robot.Constants.Elevator; -import frc.robot.autonomous.commands.MobilityAuto; -import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobility; -import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobilityEngage; -import frc.robot.autonomous.commands.N2_Engage; -import frc.robot.autonomous.commands.N3_1ConePlusMobility; -import frc.robot.autonomous.commands.N3_1ConePlusMobilityEngage; -import frc.robot.autonomous.commands.N6_1ConePlusEngage; -import frc.robot.autonomous.commands.N9_1ConePlus2CubeMobility; -import frc.robot.autonomous.commands.N9_1ConePlusMobility; -import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.DriveToPlaceCommand; @@ -68,7 +57,6 @@ import frc.util.NodeSelectorUtility.NodeType; import frc.util.SharedReference; import frc.util.Util; -import frc.util.pathing.AlliancePose2d; import frc.util.pathing.RubenManueverGenerator; import java.util.HashMap; import java.util.List; @@ -295,7 +283,7 @@ private void configureButtonBindings() { // outtake states jacobLayer .off(jacob.leftTrigger()) - .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); jacobLayer .off(jacob.rightTrigger()) @@ -368,8 +356,8 @@ private void configureButtonBindings() { new IntakeModeCommand(intakeSubsystem, Modes.INTAKE) .alongWith( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))) + elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))) + // .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))) .onFalse( new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); @@ -516,82 +504,84 @@ public void end(boolean interrupted) { new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.STOWED))); - autoSelector.setDefaultOption( - "N1 1Cone + 2Cube Low Mobility Engage", - new N1_1ConePlus2CubeHybridMobilityEngage( - 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N1 1Cone + 2Cube Low Mobility NO ENGAGE", - new N1_1ConePlus2CubeHybridMobility( - 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N9 1Cone + 1Cube + Grab Cube Mobility", - new N9_1ConePlus2CubeMobility( - 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "Just Zero Elevator [DOES NOT CALIBRATE]", - new SetZeroModeCommand(elevatorSubsystem, false)); - - autoSelector.addOption( - "Near Substation Mobility [APRILTAG]", - new MobilityAuto( - manueverGenerator, - drivebaseSubsystem, - intakeSubsystem, - elevatorSubsystem, - rgbSubsystem, - new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); - - autoSelector.addOption( - "Far Substation Mobility [APRILTAG]", - new MobilityAuto( - manueverGenerator, - drivebaseSubsystem, - intakeSubsystem, - elevatorSubsystem, - rgbSubsystem, - new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); - - autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); - - autoSelector.addOption( - "N3 1Cone + Mobility Engage", - new N3_1ConePlusMobilityEngage( - 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N3 1Cone + Mobility", - new N3_1ConePlusMobility( - 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N6 1Cone + Engage", - new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "N9 1Cone + Mobility Engage", - new N9_1ConePlusMobilityEngage( - 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "N9 1Cone + Mobility", - new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "Score High Cone [DOES NOT CALIBRATE]", - new SetZeroModeCommand( - elevatorSubsystem, - false) // FIXME pretty sure this shouldn't zero wrist, double check later - .raceWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)) - .andThen( - new ScoreCommand( - intakeSubsystem, - elevatorSubsystem, - Constants.SCORE_STEP_MAP.get( - NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))); + // autoSelector.setDefaultOption( + // FIXME causes code error, getMarkers() returns null + // "N1 1Cone + 2Cube Low Mobility Engage", + // new N1_1ConePlus2CubeHybridMobilityEngage( + // 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N1 1Cone + 2Cube Low Mobility NO ENGAGE", + // new N1_1ConePlus2CubeHybridMobility( + // 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N9 1Cone + 1Cube + Grab Cube Mobility", + // new N9_1ConePlus2CubeMobility( + // 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "Just Zero Elevator [DOES NOT CALIBRATE]", + // new SetZeroModeCommand(elevatorSubsystem, false)); + + // autoSelector.addOption( + // "Near Substation Mobility [APRILTAG]", + // new MobilityAuto( + // manueverGenerator, + // drivebaseSubsystem, + // intakeSubsystem, + // elevatorSubsystem, + // rgbSubsystem, + // new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); + + // autoSelector.addOption( + // "Far Substation Mobility [APRILTAG]", + // new MobilityAuto( + // manueverGenerator, + // drivebaseSubsystem, + // intakeSubsystem, + // elevatorSubsystem, + // rgbSubsystem, + // new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); + + // autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N3 1Cone + Mobility Engage", + // new N3_1ConePlusMobilityEngage( + // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N3 1Cone + Mobility", + // new N3_1ConePlusMobility( + // 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N6 1Cone + Engage", + // new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N9 1Cone + Mobility Engage", + // new N9_1ConePlusMobilityEngage( + // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N9 1Cone + Mobility", + // new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, + // drivebaseSubsystem)); + + // autoSelector.addOption( + // "Score High Cone [DOES NOT CALIBRATE]", + // new SetZeroModeCommand( + // elevatorSubsystem, + // false) // FIXME pretty sure this shouldn't zero wrist, double check later + // .raceWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)) + // .andThen( + // new ScoreCommand( + // intakeSubsystem, + // elevatorSubsystem, + // Constants.SCORE_STEP_MAP.get( + // NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))); driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a19eacd..547f7d2 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -28,10 +28,22 @@ public IntakeSubsystem() { currentIntakeMode = Modes.OFF; intakeMotor.setNeutralMode(NeutralMode.Brake); + intakeMotor.setInverted(true); filter = LinearFilter.movingAverage(30); shuffleboard.addDouble("Intake Motor", () -> intakeMotor.getSelectedSensorPosition()); + + shuffleboard.addString("Current Mode", () -> currentIntakeMode.toString()); + + shuffleboard.addDouble("stator current", intakeMotor::getStatorCurrent); + + shuffleboard.addDouble("motor power", intakeMotor::getMotorOutputPercent); + + shuffleboard.addDouble("filter output", this::getFilterCalculatedValue); + + // FIXME wrong current limit + statorCurrentLimit = 85; } public enum Modes { @@ -46,13 +58,17 @@ public void intakePeriodic(Modes mode) { switch (mode) { case INTAKE: intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_PERCENT); + break; case OUTTAKE: intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_PERCENT); + break; case HOLD: intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_PERCENT); + break; case OFF: default: intakeMotor.set(TalonFXControlMode.PercentOutput, 0); + break; } } @@ -64,9 +80,13 @@ public void setMode(Modes mode) { currentIntakeMode = mode; } + private double getFilterCalculatedValue() { + return filter.calculate(intakeMotor.getStatorCurrent()); + } + @Override public void periodic() { - if (filter.calculate(intakeMotor.getStatorCurrent()) >= statorCurrentLimit) { + if (getFilterCalculatedValue() >= statorCurrentLimit) { currentIntakeMode = Modes.HOLD; } From d8002d2a3511793f462d355a10bbf2f4e333a872 Mon Sep 17 00:00:00 2001 From: audrey Date: Thu, 26 Oct 2023 18:46:02 -0700 Subject: [PATCH 12/63] change "height" to "extension" --- src/main/java/frc/robot/Constants.java | 18 +++++----- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/commands/ElevatorManualCommand.java | 2 +- .../commands/ElevatorPositionCommand.java | 16 ++++----- .../robot/subsystems/ElevatorSubsystem.java | 34 +++++++++---------- 5 files changed, 36 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ced32b5..909774e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -202,18 +202,18 @@ public static final class Ports { public static final class Setpoints { public static final ElevatorState STOWED = new ElevatorState(20, 0); public static final ElevatorState SHELF_INTAKE = new ElevatorState(20, 20); - public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_HEIGHT, 0); - public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_HEIGHT, 0); - public static final ElevatorState SCORE_MID = new ElevatorState(MAX_HEIGHT / 2, 20); - public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_HEIGHT, 40); - public static final ElevatorState ZERO = new ElevatorState(MIN_HEIGHT, 0); + public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION, 0); + public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION, 0); + public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION / 2, 20); + public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION, 40); + public static final ElevatorState ZERO = new ElevatorState(MIN_EXTENSION, 0); } - public static final double MAX_HEIGHT = 20; - public static final double MIN_HEIGHT = 0; + public static final double MAX_EXTENSION = 20; + public static final double MIN_EXTENSION = 0; public static final int ELEVATOR_TICKS = 2048; - public static final double ELEVATOR_GEAR_RATIO = 1 / 9.9206; + public static final double ELEVATOR_GEAR_RATIO = 0.1008; public static final double ELEVATOR_GEAR_CIRCUMFERENCE = 1.432 * Math.PI; public static final int WRIST_TICKS = 2048; @@ -221,7 +221,7 @@ public static final class Setpoints { public static final double WRIST_GEAR_RATIO = 1 / 38.6719; public static final double ANGLE_EPSILON = 0.5; - public static final double HEIGHT_EPSILON = 5; + public static final double EXTENSION_EPSILON = 5; public static final double ANGULAR_OFFSET = 0; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e70def0..933e606 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -452,7 +452,7 @@ private void setupAutonomousCommands() { final List drivingCubeOuttake = List.of( - new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_HEIGHT)).canWaitHere(), + new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_EXTENSION)).canWaitHere(), new ScoreStep(Modes.OUTTAKE)); final boolean[] intakeLow = {false}; // FIXME go through each auto and make sure that we dont use a leftover event marker from Simba diff --git a/src/main/java/frc/robot/commands/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index 17ce315..7845378 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -27,7 +27,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - elevatorSubsystem.setTargetHeight(elevatorSubsystem.getHeight() + rate.getAsDouble()); + elevatorSubsystem.setTargetExtension(elevatorSubsystem.getExtension() + rate.getAsDouble()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java index d4ce2f0..ba12be9 100644 --- a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java @@ -12,14 +12,14 @@ public class ElevatorPositionCommand extends CommandBase { private final ElevatorSubsystem elevatorSubsystem; - private double targetHeight; + private double targetExtension; private double targetAngle; /** Creates a new ElevatorPositionCommand. */ public ElevatorPositionCommand( - ElevatorSubsystem subsystem, double targetHeight, double targetAngle) { + ElevatorSubsystem subsystem, double targetExtension, double targetAngle) { this.elevatorSubsystem = subsystem; - this.targetHeight = targetHeight; + this.targetExtension = targetExtension; this.targetAngle = targetAngle; // Use addRequirements() here to declare subsystem dependencies. addRequirements(elevatorSubsystem); @@ -28,7 +28,7 @@ public ElevatorPositionCommand( public ElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, ElevatorState elevatorState) { // height and angle this.elevatorSubsystem = elevatorSubsystem; - targetHeight = elevatorState.height(); + targetExtension = elevatorState.extension(); targetAngle = elevatorState.angle(); // Use addRequirements() here to declare subsystem dependencies. addRequirements(elevatorSubsystem); @@ -37,7 +37,7 @@ public ElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, ElevatorStat // Called when the command is initially scheduled. @Override public void initialize() { - elevatorSubsystem.setTargetHeight(targetHeight); + elevatorSubsystem.setTargetExtension(targetExtension); elevatorSubsystem.setTargetAngle(targetAngle); } @@ -57,9 +57,9 @@ public boolean isFinished() { elevatorSubsystem.getTargetAngle(), Elevator.ANGLE_EPSILON) && Util.epsilonEquals( - elevatorSubsystem.getHeight(), - elevatorSubsystem.getTargetHeight(), - Elevator.HEIGHT_EPSILON); + elevatorSubsystem.getExtension(), + elevatorSubsystem.getTargetExtension(), + Elevator.EXTENSION_EPSILON); // && extensionController.atSetpoint() // && angleController.atSetpoint(); // add a condition to end the command when the elevator diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 3b3d93e..4d6f8e3 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -26,13 +26,13 @@ public class ElevatorSubsystem extends SubsystemBase { private TalonFX rightMotor; private TalonFX wristMotor; - private double currentHeight; - private double targetHeight; + private double currentExtension; + private double targetExtension; private double currentWristAngle; private double targetAngle; private double statorCurrentLimit; - private PIDController heightController; + private PIDController ExtensionController; private PIDController wristController; private CANCoder canCoder; @@ -98,18 +98,18 @@ public ElevatorSubsystem() { tab.addDouble("Wrist Motor Position", () -> canCoder.getAbsolutePosition()); tab.addDouble("Wrist Target Angle", () -> targetAngle); tab.addDouble("Wrist Current Angle", () -> currentWristAngle); - tab.addDouble("Elevator Target Height", () -> targetHeight); - tab.addDouble("Elevator Current Height", () -> currentHeight); + tab.addDouble("Elevator Target Extension", () -> targetExtension); + tab.addDouble("Elevator Current Extension", () -> currentExtension); } // FIXME: all the numbers wrong in constants - public static double heightToTicks(double height) { - return height + public static double ExtensionToTicks(double Extension) { + return Extension * ((Elevator.ELEVATOR_GEAR_RATIO * Elevator.ELEVATOR_TICKS) / (Elevator.ELEVATOR_GEAR_CIRCUMFERENCE)); } - public static double ticksToHeight(double ticks) { + public static double ticksToExtension(double ticks) { return (ticks * Elevator.ELEVATOR_GEAR_CIRCUMFERENCE) / (Elevator.ELEVATOR_TICKS * Elevator.ELEVATOR_GEAR_RATIO); } @@ -118,25 +118,25 @@ private double getCurrentTicks() { return rightMotor.getSelectedSensorPosition(); } - public void setTargetHeight(double targetHeight) { - this.targetHeight = targetHeight; + public void setTargetExtension(double targetExtension) { + this.targetExtension = targetExtension; } public void setTargetState(ElevatorState targetState) { - targetHeight = targetState.height(); + targetExtension = targetState.Extension(); targetAngle = targetState.angle(); } - public double getTargetHeight() { - return targetHeight; + public double getTargetExtension() { + return targetExtension; } public double getTargetAngle() { return targetAngle; } - public double getHeight() { - return ticksToHeight(getCurrentTicks()); + public double getExtension() { + return ticksToExtension(getCurrentTicks()); } public double getCurrentAngleDegrees() { @@ -153,12 +153,12 @@ public void setTargetAngle(double targetAngle) { @Override public void periodic() { - currentHeight = getHeight(); + currentExtension = getExtension(); currentWristAngle = getCurrentAngleDegrees(); if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit) { // || proxySensor.get() == false) { - double motorPower = heightController.calculate(currentHeight, targetHeight); + double motorPower = ExtensionController.calculate(currentExtension, targetExtension); rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); } else { rightMotor.set(TalonFXControlMode.PercentOutput, 0); From 33b253c9633cb8d9aa68e0b08ca9c30aa54e576a Mon Sep 17 00:00:00 2001 From: audrey Date: Thu, 26 Oct 2023 18:49:04 -0700 Subject: [PATCH 13/63] fix more extension vs height --- .../robot/subsystems/ElevatorSubsystem.java | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 4d6f8e3..3e3090f 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -32,7 +32,7 @@ public class ElevatorSubsystem extends SubsystemBase { private double targetAngle; private double statorCurrentLimit; - private PIDController ExtensionController; + private PIDController extensionController; private PIDController wristController; private CANCoder canCoder; @@ -47,7 +47,7 @@ public class ElevatorSubsystem extends SubsystemBase { // stator limits private LinearFilter filter; - public static record ElevatorState(double height, double angle) {} + public static record ElevatorState(double extension, double angle) {} public ElevatorSubsystem() { leftMotor = new TalonFX(Constants.Elevator.Ports.ELEVATOR_LEFT_MOTOR_PORT); @@ -56,13 +56,13 @@ public ElevatorSubsystem() { leftMotor.follow(rightMotor); - heightController = new PIDController(0.0001, 0, 0); + extensionController = new PIDController(0.0001, 0, 0); wristController = new PIDController(0, 0, 0); canCoder = new CANCoder(0); // proxySensor = new DigitalInput(0); - currentHeight = 0.0; - targetHeight = 0.0; + currentExtension = 0.0; + targetExtension = 0.0; currentWristAngle = 0.0; targetAngle = 0.0; statorCurrentLimit = 50.0; @@ -79,8 +79,9 @@ public ElevatorSubsystem() { leftMotor.setNeutralMode(NeutralMode.Brake); wristMotor.setNeutralMode(NeutralMode.Brake); - rightMotor.configForwardSoftLimitThreshold(heightToTicks(Constants.Elevator.MAX_HEIGHT), 20); - rightMotor.configReverseSoftLimitThreshold(heightToTicks(Elevator.MIN_HEIGHT), 20); + rightMotor.configForwardSoftLimitThreshold( + extensionToTicks(Constants.Elevator.MAX_EXTENSION), 20); + rightMotor.configReverseSoftLimitThreshold(extensionToTicks(Elevator.MIN_EXTENSION), 20); wristMotor.configForwardSoftLimitThreshold(angleToTicks(0)); rightMotor.configForwardSoftLimitEnable(true, 20); @@ -103,7 +104,7 @@ public ElevatorSubsystem() { } // FIXME: all the numbers wrong in constants - public static double ExtensionToTicks(double Extension) { + public static double extensionToTicks(double Extension) { return Extension * ((Elevator.ELEVATOR_GEAR_RATIO * Elevator.ELEVATOR_TICKS) / (Elevator.ELEVATOR_GEAR_CIRCUMFERENCE)); @@ -123,7 +124,7 @@ public void setTargetExtension(double targetExtension) { } public void setTargetState(ElevatorState targetState) { - targetExtension = targetState.Extension(); + targetExtension = targetState.extension(); targetAngle = targetState.angle(); } @@ -158,7 +159,7 @@ public void periodic() { if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit) { // || proxySensor.get() == false) { - double motorPower = ExtensionController.calculate(currentExtension, targetExtension); + double motorPower = extensionController.calculate(currentExtension, targetExtension); rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); } else { rightMotor.set(TalonFXControlMode.PercentOutput, 0); From b0e590ae9ebdf9d07e8e69eb66be6f36346f8896 Mon Sep 17 00:00:00 2001 From: Jay Date: Thu, 26 Oct 2023 21:21:34 -0700 Subject: [PATCH 14/63] refactored + corrected elevator unit conversions --- src/main/java/frc/robot/Constants.java | 18 +++++++------- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/commands/ElevatorManualCommand.java | 2 +- .../commands/ElevatorPositionCommand.java | 4 ++-- .../robot/subsystems/ElevatorSubsystem.java | 24 ++++++++----------- 5 files changed, 23 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 909774e..6e00026 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -202,19 +202,19 @@ public static final class Ports { public static final class Setpoints { public static final ElevatorState STOWED = new ElevatorState(20, 0); public static final ElevatorState SHELF_INTAKE = new ElevatorState(20, 20); - public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION, 0); - public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION, 0); - public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION / 2, 20); - public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION, 40); - public static final ElevatorState ZERO = new ElevatorState(MIN_EXTENSION, 0); + public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION_INCHES, 0); + public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 0); + public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 20); + public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 40); + public static final ElevatorState ZERO = new ElevatorState(MIN_EXTENSION_INCHES, 0); } - public static final double MAX_EXTENSION = 20; - public static final double MIN_EXTENSION = 0; + public static final double MAX_EXTENSION_INCHES = 20; + public static final double MIN_EXTENSION_INCHES = 0; - public static final int ELEVATOR_TICKS = 2048; + public static final int FALCON_CPR = 2048; public static final double ELEVATOR_GEAR_RATIO = 0.1008; - public static final double ELEVATOR_GEAR_CIRCUMFERENCE = 1.432 * Math.PI; + public static final double ELEVATOR_SPROCKET_DIAMETER_INCHES = 1.432; public static final int WRIST_TICKS = 2048; public static final double WRIST_DEGREES = 360; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 933e606..106ebc1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -452,7 +452,7 @@ private void setupAutonomousCommands() { final List drivingCubeOuttake = List.of( - new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_EXTENSION)).canWaitHere(), + new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_EXTENSION_INCHES)).canWaitHere(), new ScoreStep(Modes.OUTTAKE)); final boolean[] intakeLow = {false}; // FIXME go through each auto and make sure that we dont use a leftover event marker from Simba diff --git a/src/main/java/frc/robot/commands/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index 7845378..e94a1fb 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -27,7 +27,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - elevatorSubsystem.setTargetExtension(elevatorSubsystem.getExtension() + rate.getAsDouble()); + elevatorSubsystem.setTargetExtensionInches(elevatorSubsystem.getExtensionInches() + rate.getAsDouble()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java index ba12be9..c72d54c 100644 --- a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java @@ -37,7 +37,7 @@ public ElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, ElevatorStat // Called when the command is initially scheduled. @Override public void initialize() { - elevatorSubsystem.setTargetExtension(targetExtension); + elevatorSubsystem.setTargetExtensionInches(targetExtension); elevatorSubsystem.setTargetAngle(targetAngle); } @@ -57,7 +57,7 @@ public boolean isFinished() { elevatorSubsystem.getTargetAngle(), Elevator.ANGLE_EPSILON) && Util.epsilonEquals( - elevatorSubsystem.getExtension(), + elevatorSubsystem.getExtensionInches(), elevatorSubsystem.getTargetExtension(), Elevator.EXTENSION_EPSILON); // && extensionController.atSetpoint() diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 3e3090f..624797c 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -80,8 +80,8 @@ public ElevatorSubsystem() { wristMotor.setNeutralMode(NeutralMode.Brake); rightMotor.configForwardSoftLimitThreshold( - extensionToTicks(Constants.Elevator.MAX_EXTENSION), 20); - rightMotor.configReverseSoftLimitThreshold(extensionToTicks(Elevator.MIN_EXTENSION), 20); + extensionInchesToTicks(Constants.Elevator.MAX_EXTENSION_INCHES), 20); + rightMotor.configReverseSoftLimitThreshold(extensionInchesToTicks(Elevator.MIN_EXTENSION_INCHES), 20); wristMotor.configForwardSoftLimitThreshold(angleToTicks(0)); rightMotor.configForwardSoftLimitEnable(true, 20); @@ -103,23 +103,19 @@ public ElevatorSubsystem() { tab.addDouble("Elevator Current Extension", () -> currentExtension); } - // FIXME: all the numbers wrong in constants - public static double extensionToTicks(double Extension) { - return Extension - * ((Elevator.ELEVATOR_GEAR_RATIO * Elevator.ELEVATOR_TICKS) - / (Elevator.ELEVATOR_GEAR_CIRCUMFERENCE)); + public static double extensionInchesToTicks(double inches) { + return (Elevator.FALCON_CPR * inches) / ((Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * Elevator.ELEVATOR_GEAR_RATIO); } - public static double ticksToExtension(double ticks) { - return (ticks * Elevator.ELEVATOR_GEAR_CIRCUMFERENCE) - / (Elevator.ELEVATOR_TICKS * Elevator.ELEVATOR_GEAR_RATIO); + public double ticksToExtensionInches(double ticks) { + return (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * ((leftMotor.getSelectedSensorPosition() / Elevator.FALCON_CPR) * Elevator.ELEVATOR_GEAR_RATIO); } private double getCurrentTicks() { return rightMotor.getSelectedSensorPosition(); } - public void setTargetExtension(double targetExtension) { + public void setTargetExtensionInches(double targetExtension) { this.targetExtension = targetExtension; } @@ -136,8 +132,8 @@ public double getTargetAngle() { return targetAngle; } - public double getExtension() { - return ticksToExtension(getCurrentTicks()); + public double getExtensionInches() { + return ticksToExtensionInches(getCurrentTicks()); } public double getCurrentAngleDegrees() { @@ -154,7 +150,7 @@ public void setTargetAngle(double targetAngle) { @Override public void periodic() { - currentExtension = getExtension(); + currentExtension = getExtensionInches(); currentWristAngle = getCurrentAngleDegrees(); if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit) { From af8718ac9dbd5ecb2ee3c5d7de7cc5068c079a28 Mon Sep 17 00:00:00 2001 From: Jay Date: Thu, 26 Oct 2023 21:33:22 -0700 Subject: [PATCH 15/63] oops --- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 624797c..a567939 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -108,7 +108,7 @@ public static double extensionInchesToTicks(double inches) { } public double ticksToExtensionInches(double ticks) { - return (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * ((leftMotor.getSelectedSensorPosition() / Elevator.FALCON_CPR) * Elevator.ELEVATOR_GEAR_RATIO); + return (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * ((ticks / Elevator.FALCON_CPR) * Elevator.ELEVATOR_GEAR_RATIO); } private double getCurrentTicks() { From a5e2110a22fd34f3daa0b9c2f2226a056b694dec Mon Sep 17 00:00:00 2001 From: Jay Date: Thu, 26 Oct 2023 21:55:19 -0700 Subject: [PATCH 16/63] spotless :D --- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../java/frc/robot/commands/ElevatorManualCommand.java | 3 ++- .../java/frc/robot/subsystems/ElevatorSubsystem.java | 9 ++++++--- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 106ebc1..463db0f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -452,7 +452,8 @@ private void setupAutonomousCommands() { final List drivingCubeOuttake = List.of( - new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_EXTENSION_INCHES)).canWaitHere(), + new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_EXTENSION_INCHES)) + .canWaitHere(), new ScoreStep(Modes.OUTTAKE)); final boolean[] intakeLow = {false}; // FIXME go through each auto and make sure that we dont use a leftover event marker from Simba diff --git a/src/main/java/frc/robot/commands/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index e94a1fb..ebadd52 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -27,7 +27,8 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - elevatorSubsystem.setTargetExtensionInches(elevatorSubsystem.getExtensionInches() + rate.getAsDouble()); + elevatorSubsystem.setTargetExtensionInches( + elevatorSubsystem.getExtensionInches() + rate.getAsDouble()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a567939..b9d003d 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -81,7 +81,8 @@ public ElevatorSubsystem() { rightMotor.configForwardSoftLimitThreshold( extensionInchesToTicks(Constants.Elevator.MAX_EXTENSION_INCHES), 20); - rightMotor.configReverseSoftLimitThreshold(extensionInchesToTicks(Elevator.MIN_EXTENSION_INCHES), 20); + rightMotor.configReverseSoftLimitThreshold( + extensionInchesToTicks(Elevator.MIN_EXTENSION_INCHES), 20); wristMotor.configForwardSoftLimitThreshold(angleToTicks(0)); rightMotor.configForwardSoftLimitEnable(true, 20); @@ -104,11 +105,13 @@ public ElevatorSubsystem() { } public static double extensionInchesToTicks(double inches) { - return (Elevator.FALCON_CPR * inches) / ((Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * Elevator.ELEVATOR_GEAR_RATIO); + return (Elevator.FALCON_CPR * inches) + / ((Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * Elevator.ELEVATOR_GEAR_RATIO); } public double ticksToExtensionInches(double ticks) { - return (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * ((ticks / Elevator.FALCON_CPR) * Elevator.ELEVATOR_GEAR_RATIO); + return (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) + * ((ticks / Elevator.FALCON_CPR) * Elevator.ELEVATOR_GEAR_RATIO); } private double getCurrentTicks() { From d46cac769001e8edd4b8dd1b9389c6a8a41c18b4 Mon Sep 17 00:00:00 2001 From: Derek Date: Fri, 27 Oct 2023 17:19:35 -0700 Subject: [PATCH 17/63] tuning pid + zero command --- src/main/java/frc/robot/Constants.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 179 ++++++++---------- .../robot/commands/ElevatorManualCommand.java | 7 +- .../robot/commands/SetZeroModeCommand.java | 6 +- .../robot/subsystems/ElevatorSubsystem.java | 78 +++++++- 5 files changed, 169 insertions(+), 108 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6e00026..ce3867d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -200,7 +200,7 @@ public static final class Ports { } public static final class Setpoints { - public static final ElevatorState STOWED = new ElevatorState(20, 0); + public static final ElevatorState STOWED = new ElevatorState(0, 0); public static final ElevatorState SHELF_INTAKE = new ElevatorState(20, 20); public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION_INCHES, 0); public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 0); @@ -209,7 +209,7 @@ public static final class Setpoints { public static final ElevatorState ZERO = new ElevatorState(MIN_EXTENSION_INCHES, 0); } - public static final double MAX_EXTENSION_INCHES = 20; + public static final double MAX_EXTENSION_INCHES = 30; public static final double MIN_EXTENSION_INCHES = 0; public static final int FALCON_CPR = 2048; @@ -223,6 +223,9 @@ public static final class Setpoints { public static final double ANGLE_EPSILON = 0.5; public static final double EXTENSION_EPSILON = 5; public static final double ANGULAR_OFFSET = 0; + + public static final double ZERO_MOTOR_POWER = -0.2; + public static final double ZERO_STATOR_LIMIT = 25; } public static final class Intake { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 463db0f..ee83fb6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,7 +5,6 @@ package frc.robot; import com.pathplanner.lib.server.PathPlannerServer; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -24,16 +23,6 @@ import frc.robot.Constants.Config; import frc.robot.Constants.Drive; import frc.robot.Constants.Elevator; -import frc.robot.autonomous.commands.MobilityAuto; -import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobility; -import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobilityEngage; -import frc.robot.autonomous.commands.N2_Engage; -import frc.robot.autonomous.commands.N3_1ConePlusMobility; -import frc.robot.autonomous.commands.N3_1ConePlusMobilityEngage; -import frc.robot.autonomous.commands.N6_1ConePlusEngage; -import frc.robot.autonomous.commands.N9_1ConePlus2CubeMobility; -import frc.robot.autonomous.commands.N9_1ConePlusMobility; -import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.DriveToPlaceCommand; @@ -49,7 +38,6 @@ import frc.robot.commands.ScoreCommand.ScoreStep; import frc.robot.commands.SetZeroModeCommand; import frc.robot.commands.VibrateHIDCommand; -import frc.robot.commands.WristManualCommand; import frc.robot.subsystems.CANWatchdogSubsystem; import frc.robot.subsystems.DrivebaseSubsystem; import frc.robot.subsystems.ElevatorSubsystem; @@ -68,7 +56,6 @@ import frc.util.NodeSelectorUtility.NodeType; import frc.util.SharedReference; import frc.util.Util; -import frc.util.pathing.AlliancePose2d; import frc.util.pathing.RubenManueverGenerator; import java.util.HashMap; import java.util.List; @@ -149,11 +136,11 @@ public RobotContainer() { elevatorSubsystem.setDefaultCommand( new ElevatorManualCommand( - elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getLeftY(), 0.2))); + elevatorSubsystem, () -> ControllerUtil.deadband(-jacob.getLeftY() * 0.420, 0.2))); - elevatorSubsystem.setDefaultCommand( - new WristManualCommand( - elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getRightY(), 0.2))); + // elevatorSubsystem.setDefaultCommand( + // new WristManualCommand( + // elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getRightY(), 0.2))); SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT); SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA); @@ -223,6 +210,8 @@ private void configureButtonBindings() { jacob.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); + jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); + DoubleSupplier rotation = exponential( () -> @@ -368,8 +357,7 @@ private void configureButtonBindings() { new IntakeModeCommand(intakeSubsystem, Modes.INTAKE) .alongWith( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))) + elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))) .onFalse( new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); @@ -517,82 +505,83 @@ public void end(boolean interrupted) { new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.STOWED))); - autoSelector.setDefaultOption( - "N1 1Cone + 2Cube Low Mobility Engage", - new N1_1ConePlus2CubeHybridMobilityEngage( - 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N1 1Cone + 2Cube Low Mobility NO ENGAGE", - new N1_1ConePlus2CubeHybridMobility( - 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N9 1Cone + 1Cube + Grab Cube Mobility", - new N9_1ConePlus2CubeMobility( - 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "Just Zero Elevator [DOES NOT CALIBRATE]", - new SetZeroModeCommand(elevatorSubsystem, false)); - - autoSelector.addOption( - "Near Substation Mobility [APRILTAG]", - new MobilityAuto( - manueverGenerator, - drivebaseSubsystem, - intakeSubsystem, - elevatorSubsystem, - rgbSubsystem, - new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); - - autoSelector.addOption( - "Far Substation Mobility [APRILTAG]", - new MobilityAuto( - manueverGenerator, - drivebaseSubsystem, - intakeSubsystem, - elevatorSubsystem, - rgbSubsystem, - new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); - - autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); - - autoSelector.addOption( - "N3 1Cone + Mobility Engage", - new N3_1ConePlusMobilityEngage( - 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N3 1Cone + Mobility", - new N3_1ConePlusMobility( - 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N6 1Cone + Engage", - new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "N9 1Cone + Mobility Engage", - new N9_1ConePlusMobilityEngage( - 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "N9 1Cone + Mobility", - new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "Score High Cone [DOES NOT CALIBRATE]", - new SetZeroModeCommand( - elevatorSubsystem, - false) // FIXME pretty sure this shouldn't zero wrist, double check later - .raceWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)) - .andThen( - new ScoreCommand( - intakeSubsystem, - elevatorSubsystem, - Constants.SCORE_STEP_MAP.get( - NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))); + // autoSelector.setDefaultOption( + // "N1 1Cone + 2Cube Low Mobility Engage", + // new N1_1ConePlus2CubeHybridMobilityEngage( + // 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N1 1Cone + 2Cube Low Mobility NO ENGAGE", + // new N1_1ConePlus2CubeHybridMobility( + // 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N9 1Cone + 1Cube + Grab Cube Mobility", + // new N9_1ConePlus2CubeMobility( + // 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "Just Zero Elevator [DOES NOT CALIBRATE]", + // new SetZeroModeCommand(elevatorSubsystem, false)); + + // autoSelector.addOption( + // "Near Substation Mobility [APRILTAG]", + // new MobilityAuto( + // manueverGenerator, + // drivebaseSubsystem, + // intakeSubsystem, + // elevatorSubsystem, + // rgbSubsystem, + // new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); + + // autoSelector.addOption( + // "Far Substation Mobility [APRILTAG]", + // new MobilityAuto( + // manueverGenerator, + // drivebaseSubsystem, + // intakeSubsystem, + // elevatorSubsystem, + // rgbSubsystem, + // new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); + + // autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N3 1Cone + Mobility Engage", + // new N3_1ConePlusMobilityEngage( + // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N3 1Cone + Mobility", + // new N3_1ConePlusMobility( + // 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N6 1Cone + Engage", + // new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N9 1Cone + Mobility Engage", + // new N9_1ConePlusMobilityEngage( + // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N9 1Cone + Mobility", + // new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, + // drivebaseSubsystem)); + + // autoSelector.addOption( + // "Score High Cone [DOES NOT CALIBRATE]", + // new SetZeroModeCommand( + // elevatorSubsystem, + // false) // FIXME pretty sure this shouldn't zero wrist, double check later + // .raceWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)) + // .andThen( + // new ScoreCommand( + // intakeSubsystem, + // elevatorSubsystem, + // Constants.SCORE_STEP_MAP.get( + // NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))); driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0); diff --git a/src/main/java/frc/robot/commands/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index ebadd52..15e6b8b 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -4,7 +4,9 @@ package frc.robot.commands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; import java.util.function.DoubleSupplier; @@ -28,7 +30,10 @@ public void initialize() {} @Override public void execute() { elevatorSubsystem.setTargetExtensionInches( - elevatorSubsystem.getExtensionInches() + rate.getAsDouble()); + MathUtil.clamp( + (elevatorSubsystem.getTargetExtension() + rate.getAsDouble()), + Elevator.MIN_EXTENSION_INCHES, + Elevator.MAX_EXTENSION_INCHES)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/SetZeroModeCommand.java b/src/main/java/frc/robot/commands/SetZeroModeCommand.java index 9c50fcd..054b8f0 100644 --- a/src/main/java/frc/robot/commands/SetZeroModeCommand.java +++ b/src/main/java/frc/robot/commands/SetZeroModeCommand.java @@ -5,8 +5,8 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.Modes; import java.util.Optional; public class SetZeroModeCommand extends CommandBase { @@ -35,7 +35,7 @@ public SetZeroModeCommand(ElevatorSubsystem elevatorSubsystem, boolean includeWr @Override public void initialize() { - new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.ZERO); + elevatorSubsystem.setMode(Modes.ZERO); } // Called every time the scheduler runs while the command is scheduled. @@ -49,6 +49,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return elevatorSubsystem.getMode() != Modes.ZERO; } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index b9d003d..f1dd693 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -5,6 +5,8 @@ // need: have a way for elevator to go up, take in double and goes up package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -22,6 +24,13 @@ /** Add your docs here. */ public class ElevatorSubsystem extends SubsystemBase { + public enum Modes { + DRIVE, + ZERO + } + + private Modes currentMode; + private TalonFX leftMotor; private TalonFX rightMotor; private TalonFX wristMotor; @@ -50,17 +59,25 @@ public class ElevatorSubsystem extends SubsystemBase { public static record ElevatorState(double extension, double angle) {} public ElevatorSubsystem() { + + currentMode = Modes.ZERO; + leftMotor = new TalonFX(Constants.Elevator.Ports.ELEVATOR_LEFT_MOTOR_PORT); rightMotor = new TalonFX(Constants.Elevator.Ports.ELEVATOR_RIGHT_MOTOR_PORT); wristMotor = new TalonFX(Constants.Elevator.Ports.WRIST_MOTOR_PORT); leftMotor.follow(rightMotor); - extensionController = new PIDController(0.0001, 0, 0); + extensionController = new PIDController(0.1, 0.03, 0.035); wristController = new PIDController(0, 0, 0); canCoder = new CANCoder(0); // proxySensor = new DigitalInput(0); + extensionController.setTolerance(0.25); + + rightMotor.setSelectedSensorPosition(0); + leftMotor.setSelectedSensorPosition(0); + currentExtension = 0.0; targetExtension = 0.0; currentWristAngle = 0.0; @@ -69,9 +86,14 @@ public ElevatorSubsystem() { rightMotor.configFactoryDefault(); leftMotor.configFactoryDefault(); + wristMotor.configFactoryDefault(); rightMotor.clearStickyFaults(); leftMotor.clearStickyFaults(); + wristMotor.clearStickyFaults(); + + rightMotor.setInverted(true); + leftMotor.setInverted(InvertType.FollowMaster); rightMotor.configOpenloopRamp(.5); @@ -102,6 +124,19 @@ public ElevatorSubsystem() { tab.addDouble("Wrist Current Angle", () -> currentWristAngle); tab.addDouble("Elevator Target Extension", () -> targetExtension); tab.addDouble("Elevator Current Extension", () -> currentExtension); + tab.addDouble("Elevator Output", rightMotor::getMotorOutputPercent); + tab.addDouble("filter output", () -> filterOutput); + tab.addDouble("Stator current", rightMotor::getStatorCurrent); + tab.add("PID", extensionController); + tab.addString("mode", () -> currentMode.toString()); + } + + public void setMode(Modes mode) { + currentMode = mode; + } + + public Modes getMode() { + return currentMode; } public static double extensionInchesToTicks(double inches) { @@ -151,12 +186,8 @@ public void setTargetAngle(double targetAngle) { this.targetAngle = targetAngle; } - @Override - public void periodic() { - currentExtension = getExtensionInches(); - currentWristAngle = getCurrentAngleDegrees(); - - if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit) { + private void drivePeriodic() { + if (filterOutput < statorCurrentLimit) { // || proxySensor.get() == false) { double motorPower = extensionController.calculate(currentExtension, targetExtension); rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); @@ -168,4 +199,37 @@ public void periodic() { TalonFXControlMode.PercentOutput, MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25)); } + + private void zeroPeriodic() { + rightMotor.configForwardSoftLimitEnable(false, 20); + rightMotor.configReverseSoftLimitEnable(false, 20); + + rightMotor.set(ControlMode.PercentOutput, Elevator.ZERO_MOTOR_POWER); + + if (filterOutput > Elevator.ZERO_STATOR_LIMIT) { + rightMotor.setSelectedSensorPosition(0); + leftMotor.setSelectedSensorPosition(0); + rightMotor.set(ControlMode.PercentOutput, 0); + currentMode = Modes.DRIVE; + rightMotor.configForwardSoftLimitEnable(true, 20); + rightMotor.configReverseSoftLimitEnable(true, 20); + targetExtension = Elevator.MIN_EXTENSION_INCHES; + } + } + + @Override + public void periodic() { + currentExtension = getExtensionInches(); + currentWristAngle = getCurrentAngleDegrees(); + filterOutput = filter.calculate(rightMotor.getStatorCurrent()); + + switch (currentMode) { + case ZERO: + zeroPeriodic(); + break; + case DRIVE: + drivePeriodic(); + break; + } + } } From ba7691c86d0da043df3d4e9caeab9d59338226fd Mon Sep 17 00:00:00 2001 From: audrey Date: Mon, 30 Oct 2023 16:36:39 -0700 Subject: [PATCH 18/63] tuning stuff --- src/main/java/frc/robot/Constants.java | 4 ++- src/main/java/frc/robot/RobotContainer.java | 29 ++++++++++++------- .../robot/subsystems/ElevatorSubsystem.java | 21 +++++++++++--- 3 files changed, 38 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ce3867d..996489a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -197,6 +197,7 @@ public static final class Ports { public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(14, "elevator left motor"); public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(15, "elevator right motor"); public static final int WRIST_MOTOR_PORT = CAN.at(0, "wrist motor port"); + public static final int CANCODER = CAN.at(0, "cancoder"); } public static final class Setpoints { @@ -210,11 +211,12 @@ public static final class Setpoints { } public static final double MAX_EXTENSION_INCHES = 30; - public static final double MIN_EXTENSION_INCHES = 0; + public static final double MIN_EXTENSION_INCHES = 2; public static final int FALCON_CPR = 2048; public static final double ELEVATOR_GEAR_RATIO = 0.1008; public static final double ELEVATOR_SPROCKET_DIAMETER_INCHES = 1.432; + public static final double CARRIAGE_RATIO = 2; public static final int WRIST_TICKS = 2048; public static final double WRIST_DEGREES = 360; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ee83fb6..20979ad 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,7 +136,7 @@ public RobotContainer() { elevatorSubsystem.setDefaultCommand( new ElevatorManualCommand( - elevatorSubsystem, () -> ControllerUtil.deadband(-jacob.getLeftY() * 0.420, 0.2))); + elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getLeftY() * -0.42, 0.2))); // elevatorSubsystem.setDefaultCommand( // new WristManualCommand( @@ -333,13 +333,13 @@ private void configureButtonBindings() { elevatorSubsystem, () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); - jacob - .leftBumper() - .onTrue( - new GroundPickupCommand( - intakeSubsystem, - elevatorSubsystem, - () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); + // jacob + // .leftBumper() + // .onTrue( + // new GroundPickupCommand( + // intakeSubsystem, + // elevatorSubsystem, + // () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); jacob .povUp() @@ -398,14 +398,21 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), - anthony.povRight())); + jacob.leftBumper())); + // anthony.povRight())); - anthony - .povRight() + jacob + .leftBumper() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); + // anthony + // .povRight() + // .onTrue( + // new HashMapCommand<>( + // scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); + jacob.povRight().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(1)))); jacob.povLeft().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(-1)))); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index f1dd693..60bf34e 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -70,7 +70,7 @@ public ElevatorSubsystem() { extensionController = new PIDController(0.1, 0.03, 0.035); wristController = new PIDController(0, 0, 0); - canCoder = new CANCoder(0); + canCoder = new CANCoder(Elevator.Ports.CANCODER); // proxySensor = new DigitalInput(0); extensionController.setTolerance(0.25); @@ -82,7 +82,7 @@ public ElevatorSubsystem() { targetExtension = 0.0; currentWristAngle = 0.0; targetAngle = 0.0; - statorCurrentLimit = 50.0; + statorCurrentLimit = 20.0; rightMotor.configFactoryDefault(); leftMotor.configFactoryDefault(); @@ -141,11 +141,14 @@ public Modes getMode() { public static double extensionInchesToTicks(double inches) { return (Elevator.FALCON_CPR * inches) - / ((Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * Elevator.ELEVATOR_GEAR_RATIO); + / (Elevator.CARRIAGE_RATIO + * (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) + * Elevator.ELEVATOR_GEAR_RATIO); } public double ticksToExtensionInches(double ticks) { - return (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) + return Elevator.CARRIAGE_RATIO + * (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) * ((ticks / Elevator.FALCON_CPR) * Elevator.ELEVATOR_GEAR_RATIO); } @@ -190,6 +193,12 @@ private void drivePeriodic() { if (filterOutput < statorCurrentLimit) { // || proxySensor.get() == false) { double motorPower = extensionController.calculate(currentExtension, targetExtension); + if (currentExtension < 10 && motorPower < 0) { + motorPower = MathUtil.clamp(motorPower, -0.25, 0.25); + if (currentExtension < 3 && motorPower < 0) { + motorPower = MathUtil.clamp(motorPower, -0.1, 0.1); + } + } rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); } else { rightMotor.set(TalonFXControlMode.PercentOutput, 0); @@ -209,10 +218,14 @@ private void zeroPeriodic() { if (filterOutput > Elevator.ZERO_STATOR_LIMIT) { rightMotor.setSelectedSensorPosition(0); leftMotor.setSelectedSensorPosition(0); + rightMotor.set(ControlMode.PercentOutput, 0); + currentMode = Modes.DRIVE; + rightMotor.configForwardSoftLimitEnable(true, 20); rightMotor.configReverseSoftLimitEnable(true, 20); + targetExtension = Elevator.MIN_EXTENSION_INCHES; } } From 9d2e4f06e00b9affc924904c3b7748e88c305940 Mon Sep 17 00:00:00 2001 From: audrey Date: Tue, 31 Oct 2023 15:38:26 -0700 Subject: [PATCH 19/63] fixing elevator manual command to include wrist controls --- src/main/java/frc/robot/Constants.java | 3 ++ src/main/java/frc/robot/RobotContainer.java | 8 ++-- .../robot/commands/ElevatorManualCommand.java | 16 +++++-- .../robot/commands/WristManualCommand.java | 42 ------------------- 4 files changed, 18 insertions(+), 51 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/WristManualCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 996489a..4298568 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -213,6 +213,9 @@ public static final class Setpoints { public static final double MAX_EXTENSION_INCHES = 30; public static final double MIN_EXTENSION_INCHES = 2; + public static final double MIN_ANGLE_DEGREES = 0; + public static final double MAX_ANGLE_DEGREES = 109.25; + public static final int FALCON_CPR = 2048; public static final double ELEVATOR_GEAR_RATIO = 0.1008; public static final double ELEVATOR_SPROCKET_DIAMETER_INCHES = 1.432; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 20979ad..e0b3849 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,11 +136,9 @@ public RobotContainer() { elevatorSubsystem.setDefaultCommand( new ElevatorManualCommand( - elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getLeftY() * -0.42, 0.2))); - - // elevatorSubsystem.setDefaultCommand( - // new WristManualCommand( - // elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getRightY(), 0.2))); + elevatorSubsystem, + () -> ControllerUtil.deadband(jacob.getLeftY() * -0.42, 0.2), + () -> ControllerUtil.deadband(jacob.getRightY(), 0.2))); SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT); SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA); diff --git a/src/main/java/frc/robot/commands/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index 15e6b8b..b8dc33b 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -12,12 +12,15 @@ public class ElevatorManualCommand extends CommandBase { ElevatorSubsystem elevatorSubsystem; - DoubleSupplier rate; + DoubleSupplier extensionRate; + DoubleSupplier angleRate; /** Creates a new AngleArmCommand. */ - public ElevatorManualCommand(ElevatorSubsystem elevatorSubsystem, DoubleSupplier rate) { + public ElevatorManualCommand( + ElevatorSubsystem elevatorSubsystem, DoubleSupplier extentionRate, DoubleSupplier angleRate) { this.elevatorSubsystem = elevatorSubsystem; - this.rate = rate; + this.extensionRate = extentionRate; + this.angleRate = angleRate; addRequirements(elevatorSubsystem); } @@ -31,9 +34,14 @@ public void initialize() {} public void execute() { elevatorSubsystem.setTargetExtensionInches( MathUtil.clamp( - (elevatorSubsystem.getTargetExtension() + rate.getAsDouble()), + (elevatorSubsystem.getTargetExtension() + extensionRate.getAsDouble()), Elevator.MIN_EXTENSION_INCHES, Elevator.MAX_EXTENSION_INCHES)); + elevatorSubsystem.setTargetAngle( + MathUtil.clamp( + (elevatorSubsystem.getCurrentAngleDegrees() + angleRate.getAsDouble()), + Elevator.MIN_ANGLE, + Elevator.MAX_ANGLE)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/WristManualCommand.java b/src/main/java/frc/robot/commands/WristManualCommand.java deleted file mode 100644 index ab53896..0000000 --- a/src/main/java/frc/robot/commands/WristManualCommand.java +++ /dev/null @@ -1,42 +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 frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.ElevatorSubsystem; -import java.util.function.DoubleSupplier; - -public class WristManualCommand extends CommandBase { - private DoubleSupplier rate; - private ElevatorSubsystem elevatorSubsystem; - - public WristManualCommand(ElevatorSubsystem elevatorSubsystem, DoubleSupplier rate) { - this.rate = rate; - this.elevatorSubsystem = elevatorSubsystem; - addRequirements(elevatorSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - elevatorSubsystem.setTargetAngle(rate.getAsDouble()); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - elevatorSubsystem.setTargetAngle(rate.getAsDouble()); - } - - // 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; - } -} From 1977f2f6fe974c73fc1327a49a2ae9ae812edf95 Mon Sep 17 00:00:00 2001 From: Derek Date: Tue, 31 Oct 2023 18:29:04 -0700 Subject: [PATCH 20/63] Elevator percent output command --- src/main/java/frc/robot/Constants.java | 8 +-- src/main/java/frc/robot/RobotContainer.java | 4 +- .../robot/commands/ElevatorManualCommand.java | 20 ++++--- .../commands/ElevatorPositionCommand.java | 2 + .../robot/subsystems/ElevatorSubsystem.java | 59 ++++++++++++++++--- .../frc/robot/subsystems/IntakeSubsystem.java | 4 +- 6 files changed, 73 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4298568..30028b8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -157,8 +157,8 @@ public static final class Module1 { // historically front right } public static final class Module2 { // historically front left - public static final int DRIVE_MOTOR = CAN.at(10, "module 2 drive motor"); - public static final int STEER_MOTOR = CAN.at(11, "module 2 steer motor"); + public static final int DRIVE_MOTOR = CAN.at(11, "module 2 drive motor"); + public static final int STEER_MOTOR = CAN.at(10, "module 2 steer motor"); public static final int STEER_ENCODER = CAN.at(25, "module 2 steer encoder"); public static final double STEER_OFFSET = @@ -210,8 +210,8 @@ public static final class Setpoints { public static final ElevatorState ZERO = new ElevatorState(MIN_EXTENSION_INCHES, 0); } - public static final double MAX_EXTENSION_INCHES = 30; - public static final double MIN_EXTENSION_INCHES = 2; + public static final double MAX_EXTENSION_INCHES = 61; + public static final double MIN_EXTENSION_INCHES = 0; public static final double MIN_ANGLE_DEGREES = 0; public static final double MAX_ANGLE_DEGREES = 109.25; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0b3849..8af8ad1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -369,8 +369,8 @@ private void configureButtonBindings() { .on(jacob.a()) .onTrue( new InstantCommand( - () -> - currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.LOW)))); + () -> currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.LOW)), + elevatorSubsystem)); jacobLayer .on(jacob.b()) diff --git a/src/main/java/frc/robot/commands/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index b8dc33b..8852b88 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.Modes; import java.util.function.DoubleSupplier; public class ElevatorManualCommand extends CommandBase { @@ -27,21 +28,24 @@ public ElevatorManualCommand( // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + elevatorSubsystem.setMode(Modes.PERCENT_CONTROL); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - elevatorSubsystem.setTargetExtensionInches( - MathUtil.clamp( - (elevatorSubsystem.getTargetExtension() + extensionRate.getAsDouble()), - Elevator.MIN_EXTENSION_INCHES, - Elevator.MAX_EXTENSION_INCHES)); + // elevatorSubsystem.setTargetExtensionInches( + // MathUtil.clamp( + // (elevatorSubsystem.getTargetExtension() + extensionRate.getAsDouble()), + // Elevator.MIN_EXTENSION_INCHES, + // Elevator.MAX_EXTENSION_INCHES)); elevatorSubsystem.setTargetAngle( MathUtil.clamp( (elevatorSubsystem.getCurrentAngleDegrees() + angleRate.getAsDouble()), - Elevator.MIN_ANGLE, - Elevator.MAX_ANGLE)); + Elevator.MIN_ANGLE_DEGREES, + Elevator.MAX_ANGLE_DEGREES)); + elevatorSubsystem.setPercentControl(extensionRate.getAsDouble()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java index c72d54c..d8cc35a 100644 --- a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java @@ -8,6 +8,7 @@ import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; +import frc.robot.subsystems.ElevatorSubsystem.Modes; import frc.util.Util; public class ElevatorPositionCommand extends CommandBase { @@ -39,6 +40,7 @@ public ElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, ElevatorStat public void initialize() { elevatorSubsystem.setTargetExtensionInches(targetExtension); elevatorSubsystem.setTargetAngle(targetAngle); + elevatorSubsystem.setMode(Modes.POSITION_CONTROL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 60bf34e..6447502 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -25,7 +25,8 @@ public class ElevatorSubsystem extends SubsystemBase { public enum Modes { - DRIVE, + PERCENT_CONTROL, + POSITION_CONTROL, ZERO } @@ -40,11 +41,14 @@ public enum Modes { private double currentWristAngle; private double targetAngle; private double statorCurrentLimit; + private double percentControl; private PIDController extensionController; private PIDController wristController; private CANCoder canCoder; + private boolean isInSlowZone; + private final ShuffleboardTab tab = Shuffleboard.getTab("Elevator"); private double filterOutput; @@ -83,6 +87,9 @@ public ElevatorSubsystem() { currentWristAngle = 0.0; targetAngle = 0.0; statorCurrentLimit = 20.0; + percentControl = 0.0; + + isInSlowZone = false; rightMotor.configFactoryDefault(); leftMotor.configFactoryDefault(); @@ -129,6 +136,7 @@ public ElevatorSubsystem() { tab.addDouble("Stator current", rightMotor::getStatorCurrent); tab.add("PID", extensionController); tab.addString("mode", () -> currentMode.toString()); + tab.addBoolean("In slow zone", () -> isInSlowZone); } public void setMode(Modes mode) { @@ -165,6 +173,14 @@ public void setTargetState(ElevatorState targetState) { targetAngle = targetState.angle(); } + public void setPercentControl(double percentControl) { + this.percentControl = percentControl; + } + + public double getPercentControl() { + return percentControl; + } + public double getTargetExtension() { return targetExtension; } @@ -189,14 +205,36 @@ public void setTargetAngle(double targetAngle) { this.targetAngle = targetAngle; } - private void drivePeriodic() { + private double applySlowZoneToPercent(double percentControl) { + if (getExtensionInches() < 15) { + isInSlowZone = true; + return percentControl * 0.25; + } else { + isInSlowZone = false; + return percentControl; + } + } + + private void percentDrivePeriodic() { + if (targetExtension > currentExtension) { + rightMotor.set(TalonFXControlMode.PercentOutput, applySlowZoneToPercent(percentControl)); + } else { + rightMotor.set(TalonFXControlMode.PercentOutput, percentControl); + } + + wristMotor.set( + TalonFXControlMode.PercentOutput, + MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25)); + } + + private void positionDrivePeriodic() { if (filterOutput < statorCurrentLimit) { // || proxySensor.get() == false) { double motorPower = extensionController.calculate(currentExtension, targetExtension); - if (currentExtension < 10 && motorPower < 0) { - motorPower = MathUtil.clamp(motorPower, -0.25, 0.25); - if (currentExtension < 3 && motorPower < 0) { - motorPower = MathUtil.clamp(motorPower, -0.1, 0.1); + if (currentExtension < 20 && motorPower < 0) { + motorPower = MathUtil.clamp(motorPower, -0.15, 0.15); + if (currentExtension < 6 && motorPower < 0) { + motorPower = MathUtil.clamp(motorPower, -0.075, 0.075); } } rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); @@ -221,7 +259,7 @@ private void zeroPeriodic() { rightMotor.set(ControlMode.PercentOutput, 0); - currentMode = Modes.DRIVE; + currentMode = Modes.POSITION_CONTROL; rightMotor.configForwardSoftLimitEnable(true, 20); rightMotor.configReverseSoftLimitEnable(true, 20); @@ -240,8 +278,11 @@ public void periodic() { case ZERO: zeroPeriodic(); break; - case DRIVE: - drivePeriodic(); + case POSITION_CONTROL: + positionDrivePeriodic(); + break; + case PERCENT_CONTROL: + percentDrivePeriodic(); break; } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a19eacd..51f50c0 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -31,7 +31,9 @@ public IntakeSubsystem() { filter = LinearFilter.movingAverage(30); - shuffleboard.addDouble("Intake Motor", () -> intakeMotor.getSelectedSensorPosition()); + shuffleboard.addDouble( + "Intake motor sensor position", () -> intakeMotor.getSelectedSensorPosition()); + shuffleboard.addString("Current mode", () -> currentIntakeMode.toString()); } public enum Modes { From a7ca8667a014046d878b7e5d0a0b47d2717fbdd6 Mon Sep 17 00:00:00 2001 From: audrey Date: Wed, 1 Nov 2023 21:58:48 -0700 Subject: [PATCH 21/63] testing different slowzone --- .../robot/subsystems/ElevatorSubsystem.java | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 6447502..bee2a37 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -205,21 +205,23 @@ public void setTargetAngle(double targetAngle) { this.targetAngle = targetAngle; } - private double applySlowZoneToPercent(double percentControl) { - if (getExtensionInches() < 15) { - isInSlowZone = true; - return percentControl * 0.25; - } else { - isInSlowZone = false; - return percentControl; - } - } + // private double applySlowZoneToPercent(double percentControl) { + // if (getExtensionInches() < 15) { + // isInSlowZone = true; + // return percentControl * 0.25; + // } else { + // isInSlowZone = false; + // return percentControl; + // } + // } private void percentDrivePeriodic() { - if (targetExtension > currentExtension) { - rightMotor.set(TalonFXControlMode.PercentOutput, applySlowZoneToPercent(percentControl)); + if (currentExtension < 15) { + rightMotor.set(TalonFXControlMode.PercentOutput, 0.15); + isInSlowZone = true; } else { rightMotor.set(TalonFXControlMode.PercentOutput, percentControl); + isInSlowZone = false; } wristMotor.set( @@ -232,12 +234,14 @@ private void positionDrivePeriodic() { // || proxySensor.get() == false) { double motorPower = extensionController.calculate(currentExtension, targetExtension); if (currentExtension < 20 && motorPower < 0) { + isInSlowZone = true; motorPower = MathUtil.clamp(motorPower, -0.15, 0.15); if (currentExtension < 6 && motorPower < 0) { motorPower = MathUtil.clamp(motorPower, -0.075, 0.075); } + } else { + rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); } - rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); } else { rightMotor.set(TalonFXControlMode.PercentOutput, 0); } From b4a5997e21c89b1ee1fa628625b92ecea168ec8b Mon Sep 17 00:00:00 2001 From: audrey Date: Thu, 2 Nov 2023 17:29:32 -0700 Subject: [PATCH 22/63] profiled pid? --- src/main/java/frc/robot/RobotContainer.java | 8 ++-- .../robot/subsystems/ElevatorSubsystem.java | 41 ++++++++++++------- 2 files changed, 31 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8af8ad1..8bb1293 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -339,8 +339,8 @@ private void configureButtonBindings() { // elevatorSubsystem, // () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); - jacob - .povUp() + jacobLayer + .off(jacob.povUp()) .onTrue( new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) .alongWith( @@ -349,8 +349,8 @@ private void configureButtonBindings() { // jacob.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem)); - jacob - .back() + jacobLayer + .off(jacob.back()) .whileTrue( new IntakeModeCommand(intakeSubsystem, Modes.INTAKE) .alongWith( diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index bee2a37..88374be 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -13,7 +13,9 @@ import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.trajectory.TrapezoidProfile; // import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -43,7 +45,7 @@ public enum Modes { private double statorCurrentLimit; private double percentControl; - private PIDController extensionController; + private ProfiledPIDController extensionController; private PIDController wristController; private CANCoder canCoder; @@ -72,12 +74,14 @@ public ElevatorSubsystem() { leftMotor.follow(rightMotor); - extensionController = new PIDController(0.1, 0.03, 0.035); + // extensionController = new PIDController(0.1, 0.03, 0.035); + extensionController = + new ProfiledPIDController(0.1, 0, 0035, new TrapezoidProfile.Constraints(8, 8)); wristController = new PIDController(0, 0, 0); canCoder = new CANCoder(Elevator.Ports.CANCODER); // proxySensor = new DigitalInput(0); - extensionController.setTolerance(0.25); + extensionController.setTolerance(0.25, 0.05); rightMotor.setSelectedSensorPosition(0); leftMotor.setSelectedSensorPosition(0); @@ -116,7 +120,7 @@ public ElevatorSubsystem() { rightMotor.configForwardSoftLimitEnable(true, 20); rightMotor.configReverseSoftLimitEnable(true, 20); - wristMotor.configForwardSoftLimitThreshold(angleToTicks(0)); + wristMotor.configForwardSoftLimitEnable(true); canCoder.configMagnetOffset(Elevator.ANGULAR_OFFSET); @@ -132,6 +136,7 @@ public ElevatorSubsystem() { tab.addDouble("Elevator Target Extension", () -> targetExtension); tab.addDouble("Elevator Current Extension", () -> currentExtension); tab.addDouble("Elevator Output", rightMotor::getMotorOutputPercent); + tab.addDouble("velocity", rightMotor::getSelectedSensorVelocity); tab.addDouble("filter output", () -> filterOutput); tab.addDouble("Stator current", rightMotor::getStatorCurrent); tab.add("PID", extensionController); @@ -232,17 +237,25 @@ private void percentDrivePeriodic() { private void positionDrivePeriodic() { if (filterOutput < statorCurrentLimit) { // || proxySensor.get() == false) { - double motorPower = extensionController.calculate(currentExtension, targetExtension); - if (currentExtension < 20 && motorPower < 0) { - isInSlowZone = true; - motorPower = MathUtil.clamp(motorPower, -0.15, 0.15); - if (currentExtension < 6 && motorPower < 0) { - motorPower = MathUtil.clamp(motorPower, -0.075, 0.075); - } - } else { - rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); - } + double motorPower = + MathUtil.clamp( + extensionController.calculate( + currentExtension, new TrapezoidProfile.State(targetExtension, 0)), + -1, + 1); + + // if (currentExtension < 20 && motorPower < 0) { + // isInSlowZone = true; + // motorPower = MathUtil.clamp(motorPower, -0.15, 0.15); + // if (currentExtension < 6 && motorPower < 0) { + // motorPower = MathUtil.clamp(motorPower, -0.075, 0.075); + // } + // } else { + // isInSlowZone = false; + // rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); + // } } else { + isInSlowZone = false; rightMotor.set(TalonFXControlMode.PercentOutput, 0); } From 429ba04a7cd234654b4563a62249606d98e16e8b Mon Sep 17 00:00:00 2001 From: Derek Date: Sat, 4 Nov 2023 15:44:25 -0700 Subject: [PATCH 23/63] replacing elevator pid with motion magic --- src/main/java/frc/robot/Constants.java | 8 ++- .../robot/commands/ElevatorManualCommand.java | 5 ++ .../commands/ElevatorPositionCommand.java | 14 ++--- .../robot/subsystems/ElevatorSubsystem.java | 56 +++++++++---------- 4 files changed, 43 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 30028b8..0823e4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -194,8 +194,8 @@ public static final class Module4 { // historically back right public static final class Elevator { public static final class Ports { - public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(14, "elevator left motor"); - public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(15, "elevator right motor"); + public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(7, "elevator left motor"); + public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(6, "elevator right motor"); public static final int WRIST_MOTOR_PORT = CAN.at(0, "wrist motor port"); public static final int CANCODER = CAN.at(0, "cancoder"); } @@ -230,7 +230,9 @@ public static final class Setpoints { public static final double ANGULAR_OFFSET = 0; public static final double ZERO_MOTOR_POWER = -0.2; - public static final double ZERO_STATOR_LIMIT = 25; + public static final double ZERO_STATOR_LIMIT = 4; + + public static final double GRAVITY_OFFSET_PERCENT = .2; } public static final class Intake { diff --git a/src/main/java/frc/robot/commands/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index 8852b88..e0b579a 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -45,6 +45,11 @@ public void execute() { (elevatorSubsystem.getCurrentAngleDegrees() + angleRate.getAsDouble()), Elevator.MIN_ANGLE_DEGREES, Elevator.MAX_ANGLE_DEGREES)); + // elevatorSubsystem.setTargetExtensionInches( + // MathUtil.clamp( + // (elevatorSubsystem.getExtensionInches() + extensionRate.getAsDouble()), + // Elevator.MIN_EXTENSION_INCHES, + // Elevator.MAX_EXTENSION_INCHES)); elevatorSubsystem.setPercentControl(extensionRate.getAsDouble()); } diff --git a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java index d8cc35a..7530175 100644 --- a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java @@ -54,14 +54,14 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { + // return Util.epsilonEquals( + // elevatorSubsystem.getCurrentAngleDegrees(), + // elevatorSubsystem.getTargetAngle(), + // Elevator.ANGLE_EPSILON) return Util.epsilonEquals( - elevatorSubsystem.getCurrentAngleDegrees(), - elevatorSubsystem.getTargetAngle(), - Elevator.ANGLE_EPSILON) - && Util.epsilonEquals( - elevatorSubsystem.getExtensionInches(), - elevatorSubsystem.getTargetExtension(), - Elevator.EXTENSION_EPSILON); + elevatorSubsystem.getExtensionInches(), + elevatorSubsystem.getTargetExtension(), + Elevator.EXTENSION_EPSILON); // && extensionController.atSetpoint() // && angleController.atSetpoint(); // add a condition to end the command when the elevator diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 88374be..0d7a709 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -42,7 +42,6 @@ public enum Modes { private double targetExtension; private double currentWristAngle; private double targetAngle; - private double statorCurrentLimit; private double percentControl; private ProfiledPIDController extensionController; @@ -54,6 +53,7 @@ public enum Modes { private final ShuffleboardTab tab = Shuffleboard.getTab("Elevator"); private double filterOutput; + private double gravityOffset; // private DigitalInput proxySensor; @@ -75,8 +75,6 @@ public ElevatorSubsystem() { leftMotor.follow(rightMotor); // extensionController = new PIDController(0.1, 0.03, 0.035); - extensionController = - new ProfiledPIDController(0.1, 0, 0035, new TrapezoidProfile.Constraints(8, 8)); wristController = new PIDController(0, 0, 0); canCoder = new CANCoder(Elevator.Ports.CANCODER); // proxySensor = new DigitalInput(0); @@ -90,8 +88,8 @@ public ElevatorSubsystem() { targetExtension = 0.0; currentWristAngle = 0.0; targetAngle = 0.0; - statorCurrentLimit = 20.0; percentControl = 0.0; + gravityOffset = 0.15; isInSlowZone = false; @@ -122,6 +120,14 @@ public ElevatorSubsystem() { rightMotor.configReverseSoftLimitEnable(true, 20); wristMotor.configForwardSoftLimitEnable(true); + rightMotor.configMotionAcceleration(30000, 30); + rightMotor.configMotionCruiseVelocity(15000, 30); + + rightMotor.config_kP(0, 0.2); + rightMotor.config_kD(0, 0); + rightMotor.config_kI(0, 0); + rightMotor.config_kF(0, 0); + canCoder.configMagnetOffset(Elevator.ANGULAR_OFFSET); canCoder.configSensorDirection(true); @@ -139,7 +145,7 @@ public ElevatorSubsystem() { tab.addDouble("velocity", rightMotor::getSelectedSensorVelocity); tab.addDouble("filter output", () -> filterOutput); tab.addDouble("Stator current", rightMotor::getStatorCurrent); - tab.add("PID", extensionController); + // tab.add("PID", extensionController); tab.addString("mode", () -> currentMode.toString()); tab.addBoolean("In slow zone", () -> isInSlowZone); } @@ -221,43 +227,33 @@ public void setTargetAngle(double targetAngle) { // } private void percentDrivePeriodic() { - if (currentExtension < 15) { - rightMotor.set(TalonFXControlMode.PercentOutput, 0.15); + if (currentExtension < 15 && percentControl < 0) { + rightMotor.set(TalonFXControlMode.PercentOutput, percentControl * 0.4); isInSlowZone = true; } else { rightMotor.set(TalonFXControlMode.PercentOutput, percentControl); isInSlowZone = false; } + // rightMotor.set(TalonFXControlMode.MotionMagic, extensionInchesToTicks(targetExtension)); + wristMotor.set( TalonFXControlMode.PercentOutput, MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25)); } private void positionDrivePeriodic() { - if (filterOutput < statorCurrentLimit) { - // || proxySensor.get() == false) { - double motorPower = - MathUtil.clamp( - extensionController.calculate( - currentExtension, new TrapezoidProfile.State(targetExtension, 0)), - -1, - 1); - - // if (currentExtension < 20 && motorPower < 0) { - // isInSlowZone = true; - // motorPower = MathUtil.clamp(motorPower, -0.15, 0.15); - // if (currentExtension < 6 && motorPower < 0) { - // motorPower = MathUtil.clamp(motorPower, -0.075, 0.075); - // } - // } else { - // isInSlowZone = false; - // rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); - // } - } else { - isInSlowZone = false; - rightMotor.set(TalonFXControlMode.PercentOutput, 0); - } + // if (filterOutput < statorCurrentLimit) { + // double motorPower = extensionController.calculate(currentExtension, targetExtension); + // final double gravityOffset = calculateElevatorGravityOffset(); + + // rightMotor.set( + // TalonFXControlMode.PercentOutput, MathUtil.clamp(motorPower + gravityOffset, -0.8, + // 0.8)); + // } else { + // rightMotor.set(TalonFXControlMode.PercentOutput, 0); + // } + rightMotor.set(TalonFXControlMode.MotionMagic, extensionInchesToTicks(targetExtension)); wristMotor.set( TalonFXControlMode.PercentOutput, From 8c3effe0ce8e74afba88fb45ee02758565224534 Mon Sep 17 00:00:00 2001 From: Derek Date: Sat, 4 Nov 2023 18:22:56 -0700 Subject: [PATCH 24/63] Working elevator --- src/main/java/frc/robot/Constants.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 41 ++++++++----------- 2 files changed, 17 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0823e4a..0a3fb68 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -230,7 +230,7 @@ public static final class Setpoints { public static final double ANGULAR_OFFSET = 0; public static final double ZERO_MOTOR_POWER = -0.2; - public static final double ZERO_STATOR_LIMIT = 4; + public static final double ZERO_STATOR_LIMIT = 15; public static final double GRAVITY_OFFSET_PERCENT = .2; } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 0d7a709..b677fa0 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -13,9 +13,7 @@ import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.trajectory.TrapezoidProfile; // import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -44,7 +42,7 @@ public enum Modes { private double targetAngle; private double percentControl; - private ProfiledPIDController extensionController; + // private ProfiledPIDController extensionController; private PIDController wristController; private CANCoder canCoder; @@ -53,7 +51,6 @@ public enum Modes { private final ShuffleboardTab tab = Shuffleboard.getTab("Elevator"); private double filterOutput; - private double gravityOffset; // private DigitalInput proxySensor; @@ -79,7 +76,7 @@ public ElevatorSubsystem() { canCoder = new CANCoder(Elevator.Ports.CANCODER); // proxySensor = new DigitalInput(0); - extensionController.setTolerance(0.25, 0.05); + // extensionController.setTolerance(0.25, 0.05); rightMotor.setSelectedSensorPosition(0); leftMotor.setSelectedSensorPosition(0); @@ -89,7 +86,6 @@ public ElevatorSubsystem() { currentWristAngle = 0.0; targetAngle = 0.0; percentControl = 0.0; - gravityOffset = 0.15; isInSlowZone = false; @@ -123,11 +119,11 @@ public ElevatorSubsystem() { rightMotor.configMotionAcceleration(30000, 30); rightMotor.configMotionCruiseVelocity(15000, 30); - rightMotor.config_kP(0, 0.2); + rightMotor.config_kP(0, 0.08); rightMotor.config_kD(0, 0); rightMotor.config_kI(0, 0); - rightMotor.config_kF(0, 0); - + rightMotor.config_kF(0, 0.0); + rightMotor.configNeutralDeadband(0.001); canCoder.configMagnetOffset(Elevator.ANGULAR_OFFSET); canCoder.configSensorDirection(true); @@ -216,24 +212,19 @@ public void setTargetAngle(double targetAngle) { this.targetAngle = targetAngle; } - // private double applySlowZoneToPercent(double percentControl) { - // if (getExtensionInches() < 15) { - // isInSlowZone = true; - // return percentControl * 0.25; - // } else { - // isInSlowZone = false; - // return percentControl; - // } - // } - - private void percentDrivePeriodic() { - if (currentExtension < 15 && percentControl < 0) { - rightMotor.set(TalonFXControlMode.PercentOutput, percentControl * 0.4); + private double applySlowZoneToPercent(double percentControl) { + if ((currentExtension > (Elevator.MAX_EXTENSION_INCHES - 7)) + || (currentExtension < (Elevator.MIN_EXTENSION_INCHES + 7))) { isInSlowZone = true; - } else { - rightMotor.set(TalonFXControlMode.PercentOutput, percentControl); - isInSlowZone = false; + return percentControl * 0.5; } + isInSlowZone = false; + return percentControl; + } + + private void percentDrivePeriodic() { + rightMotor.set(TalonFXControlMode.PercentOutput, applySlowZoneToPercent(percentControl) + 0.02); + isInSlowZone = false; // rightMotor.set(TalonFXControlMode.MotionMagic, extensionInchesToTicks(targetExtension)); From 8c81f55843d80d055451e6a9f5f0b0e0fb0494bf Mon Sep 17 00:00:00 2001 From: Derek Date: Tue, 7 Nov 2023 19:02:12 -0800 Subject: [PATCH 25/63] finish elevator, get wrist working --- src/main/java/frc/robot/Constants.java | 13 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../commands/N9_1ConePlus2CubeMobility.java | 7 +- .../robot/commands/ElevatorManualCommand.java | 14 +- .../commands/ElevatorPositionCommand.java | 14 +- .../robot/commands/SetZeroModeCommand.java | 19 +- .../robot/subsystems/ElevatorSubsystem.java | 189 ++++++++++++------ 7 files changed, 153 insertions(+), 107 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0a3fb68..9de2384 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -194,9 +194,9 @@ public static final class Module4 { // historically back right public static final class Elevator { public static final class Ports { - public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(7, "elevator left motor"); - public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(6, "elevator right motor"); - public static final int WRIST_MOTOR_PORT = CAN.at(0, "wrist motor port"); + public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(14, "elevator left motor"); + public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(15, "elevator right motor"); + public static final int WRIST_MOTOR_PORT = CAN.at(16, "wrist motor port"); public static final int CANCODER = CAN.at(0, "cancoder"); } @@ -210,8 +210,8 @@ public static final class Setpoints { public static final ElevatorState ZERO = new ElevatorState(MIN_EXTENSION_INCHES, 0); } - public static final double MAX_EXTENSION_INCHES = 61; - public static final double MIN_EXTENSION_INCHES = 0; + public static final double MAX_EXTENSION_INCHES = 54; + public static final double MIN_EXTENSION_INCHES = 1; public static final double MIN_ANGLE_DEGREES = 0; public static final double MAX_ANGLE_DEGREES = 109.25; @@ -230,7 +230,8 @@ public static final class Setpoints { public static final double ANGULAR_OFFSET = 0; public static final double ZERO_MOTOR_POWER = -0.2; - public static final double ZERO_STATOR_LIMIT = 15; + public static final double ZERO_STATOR_LIMIT = 10; + public static final double STATOR_LIMIT = 25; public static final double GRAVITY_OFFSET_PERCENT = .2; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8bb1293..1599c77 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -138,7 +138,7 @@ public RobotContainer() { new ElevatorManualCommand( elevatorSubsystem, () -> ControllerUtil.deadband(jacob.getLeftY() * -0.42, 0.2), - () -> ControllerUtil.deadband(jacob.getRightY(), 0.2))); + () -> ControllerUtil.deadband(jacob.getRightY() * 0.5, 0.2))); SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT); SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA); @@ -455,7 +455,7 @@ private void setupAutonomousCommands() { "stow elevator", new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED), "zero everything", - new SetZeroModeCommand(elevatorSubsystem, true), + new SetZeroModeCommand(elevatorSubsystem), "intake", new ElevatorPositionCommand( // edited so that it works with elevator - chooses between // ground or shelf intake diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java index 594104d..2dfbf8f 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java @@ -22,7 +22,6 @@ import frc.util.pathing.LoadMirrorPath; import java.util.List; import java.util.Map; -import java.util.Optional; import java.util.function.Supplier; public class N9_1ConePlus2CubeMobility extends SequentialCommandGroup { @@ -39,10 +38,8 @@ public N9_1ConePlus2CubeMobility( "n9 1cone + 2cube", maxVelocityMetersPerSecond, maxAccelerationMetersPerSecondSq); addCommands( - (new SetZeroModeCommand(elevatorSubsystem) - .alongWith( - new SetZeroModeCommand( - elevatorSubsystem, Optional.of(true), Optional.of(false)))) + new SetZeroModeCommand(elevatorSubsystem) + .alongWith(new SetZeroModeCommand(elevatorSubsystem)) .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)), new ScoreCommand( intakeSubsystem, diff --git a/src/main/java/frc/robot/commands/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index e0b579a..3cc5602 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -4,9 +4,7 @@ package frc.robot.commands; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.ElevatorSubsystem.Modes; import java.util.function.DoubleSupplier; @@ -40,17 +38,17 @@ public void execute() { // (elevatorSubsystem.getTargetExtension() + extensionRate.getAsDouble()), // Elevator.MIN_EXTENSION_INCHES, // Elevator.MAX_EXTENSION_INCHES)); - elevatorSubsystem.setTargetAngle( - MathUtil.clamp( - (elevatorSubsystem.getCurrentAngleDegrees() + angleRate.getAsDouble()), - Elevator.MIN_ANGLE_DEGREES, - Elevator.MAX_ANGLE_DEGREES)); + // elevatorSubsystem.setTargetAngle( + // MathUtil.clamp( + // (elevatorSubsystem.getCurrentAngleDegrees() + angleRate.getAsDouble()), + // Elevator.MIN_ANGLE_DEGREES, + // Elevator.MAX_ANGLE_DEGREES)); // elevatorSubsystem.setTargetExtensionInches( // MathUtil.clamp( // (elevatorSubsystem.getExtensionInches() + extensionRate.getAsDouble()), // Elevator.MIN_EXTENSION_INCHES, // Elevator.MAX_EXTENSION_INCHES)); - elevatorSubsystem.setPercentControl(extensionRate.getAsDouble()); + elevatorSubsystem.setPercentControl(extensionRate.getAsDouble(), angleRate.getAsDouble()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java index 7530175..87d3106 100644 --- a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java @@ -54,14 +54,14 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - // return Util.epsilonEquals( - // elevatorSubsystem.getCurrentAngleDegrees(), - // elevatorSubsystem.getTargetAngle(), - // Elevator.ANGLE_EPSILON) return Util.epsilonEquals( - elevatorSubsystem.getExtensionInches(), - elevatorSubsystem.getTargetExtension(), - Elevator.EXTENSION_EPSILON); + elevatorSubsystem.getCurrentAngleDegrees(), + elevatorSubsystem.getTargetAngle(), + Elevator.ANGLE_EPSILON) + && Util.epsilonEquals( + elevatorSubsystem.getCurrentExtensionInches(), + elevatorSubsystem.getTargetExtension(), + Elevator.EXTENSION_EPSILON); // && extensionController.atSetpoint() // && angleController.atSetpoint(); // add a condition to end the command when the elevator diff --git a/src/main/java/frc/robot/commands/SetZeroModeCommand.java b/src/main/java/frc/robot/commands/SetZeroModeCommand.java index 054b8f0..d50244c 100644 --- a/src/main/java/frc/robot/commands/SetZeroModeCommand.java +++ b/src/main/java/frc/robot/commands/SetZeroModeCommand.java @@ -7,34 +7,19 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.ElevatorSubsystem.Modes; -import java.util.Optional; public class SetZeroModeCommand extends CommandBase { private ElevatorSubsystem elevatorSubsystem; - private boolean includeWrist; - private boolean includeElevator; - public SetZeroModeCommand( - ElevatorSubsystem elevatorSubsystem, - Optional includeWrist, - Optional includeElevator) { + public SetZeroModeCommand(ElevatorSubsystem elevatorSubsystem) { this.elevatorSubsystem = elevatorSubsystem; - if (includeWrist.isPresent()) this.includeWrist = includeWrist.get(); - if (includeElevator.isPresent()) this.includeElevator = includeElevator.get(); addRequirements(elevatorSubsystem); } - public SetZeroModeCommand(ElevatorSubsystem elevatorSubsystem) { - this(elevatorSubsystem, Optional.of(true), Optional.of(true)); - } - - public SetZeroModeCommand(ElevatorSubsystem elevatorSubsystem, boolean includeWrist) { - this(elevatorSubsystem, Optional.of(includeWrist), Optional.of(true)); - } - @Override public void initialize() { + elevatorSubsystem.setNotZeroed(); elevatorSubsystem.setMode(Modes.ZERO); } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index b677fa0..e35ffa5 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -5,12 +5,10 @@ // need: have a way for elevator to go up, take in double and goes up package frc.robot.subsystems; -import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.LinearFilter; @@ -40,24 +38,31 @@ public enum Modes { private double targetExtension; private double currentWristAngle; private double targetAngle; - private double percentControl; + private double elevatorPercentControl; + private double wristPercentControl; + + private boolean elevatorZeroed; + private boolean wristZeroed; // private ProfiledPIDController extensionController; private PIDController wristController; - private CANCoder canCoder; + // private CANCoder canCoder; + + private final ShuffleboardTab elevatorTab = Shuffleboard.getTab("Elevator"); - private boolean isInSlowZone; + private final ShuffleboardTab wristTab = Shuffleboard.getTab("Wrist"); - private final ShuffleboardTab tab = Shuffleboard.getTab("Elevator"); + private double elevatorFilterOutput; - private double filterOutput; + private double wristFilterOutput; // private DigitalInput proxySensor; // add soft limits - check 2022 frc code // stator limits - private LinearFilter filter; + private LinearFilter elevatorFilter; + private LinearFilter wristFilter; public static record ElevatorState(double extension, double angle) {} @@ -72,22 +77,25 @@ public ElevatorSubsystem() { leftMotor.follow(rightMotor); // extensionController = new PIDController(0.1, 0.03, 0.035); - wristController = new PIDController(0, 0, 0); - canCoder = new CANCoder(Elevator.Ports.CANCODER); + wristController = new PIDController(0.1, 0, 0); + // canCoder = new CANCoder(Elevator.Ports.CANCODER); // proxySensor = new DigitalInput(0); // extensionController.setTolerance(0.25, 0.05); rightMotor.setSelectedSensorPosition(0); leftMotor.setSelectedSensorPosition(0); + wristMotor.setSelectedSensorPosition(0); currentExtension = 0.0; targetExtension = 0.0; currentWristAngle = 0.0; targetAngle = 0.0; - percentControl = 0.0; + elevatorPercentControl = 0.0; + wristPercentControl = 0.0; - isInSlowZone = false; + elevatorZeroed = false; + wristZeroed = false; rightMotor.configFactoryDefault(); leftMotor.configFactoryDefault(); @@ -99,6 +107,8 @@ public ElevatorSubsystem() { rightMotor.setInverted(true); leftMotor.setInverted(InvertType.FollowMaster); + // wristMotor.setInverted(false); + wristMotor.setSensorPhase(true); rightMotor.configOpenloopRamp(.5); @@ -110,11 +120,13 @@ public ElevatorSubsystem() { extensionInchesToTicks(Constants.Elevator.MAX_EXTENSION_INCHES), 20); rightMotor.configReverseSoftLimitThreshold( extensionInchesToTicks(Elevator.MIN_EXTENSION_INCHES), 20); - wristMotor.configForwardSoftLimitThreshold(angleToTicks(0)); + wristMotor.configReverseSoftLimitThreshold(angleToTicks(Elevator.MIN_ANGLE_DEGREES)); + wristMotor.configForwardSoftLimitThreshold(angleToTicks(Elevator.MAX_ANGLE_DEGREES)); rightMotor.configForwardSoftLimitEnable(true, 20); rightMotor.configReverseSoftLimitEnable(true, 20); wristMotor.configForwardSoftLimitEnable(true); + // wristMotor.configReverseSoftLimitEnable(true); rightMotor.configMotionAcceleration(30000, 30); rightMotor.configMotionCruiseVelocity(15000, 30); @@ -124,26 +136,31 @@ public ElevatorSubsystem() { rightMotor.config_kI(0, 0); rightMotor.config_kF(0, 0.0); rightMotor.configNeutralDeadband(0.001); - canCoder.configMagnetOffset(Elevator.ANGULAR_OFFSET); - - canCoder.configSensorDirection(true); - - canCoder.setPositionToAbsolute(10); // ms - - filter = LinearFilter.movingAverage(30); - - tab.addDouble("Wrist Motor Position", () -> canCoder.getAbsolutePosition()); - tab.addDouble("Wrist Target Angle", () -> targetAngle); - tab.addDouble("Wrist Current Angle", () -> currentWristAngle); - tab.addDouble("Elevator Target Extension", () -> targetExtension); - tab.addDouble("Elevator Current Extension", () -> currentExtension); - tab.addDouble("Elevator Output", rightMotor::getMotorOutputPercent); - tab.addDouble("velocity", rightMotor::getSelectedSensorVelocity); - tab.addDouble("filter output", () -> filterOutput); - tab.addDouble("Stator current", rightMotor::getStatorCurrent); - // tab.add("PID", extensionController); - tab.addString("mode", () -> currentMode.toString()); - tab.addBoolean("In slow zone", () -> isInSlowZone); + // canCoder.configMagnetOffset(Elevator.ANGULAR_OFFSET); + + // canCoder.configSensorDirection(true); + + // canCoder.setPositionToAbsolute(10); // ms + + elevatorFilter = LinearFilter.movingAverage(30); + wristFilter = LinearFilter.movingAverage(30); + + wristTab.addDouble("Wrist Target Angle", () -> targetAngle); + wristTab.addDouble("Wrist Current Angle", () -> currentWristAngle); + wristTab.addDouble("filter output", () -> wristFilterOutput); + wristTab.addDouble("percent control", () -> wristPercentControl); + wristTab.addBoolean("is zeroed", () -> wristZeroed); + wristTab.add("Wrist PID", wristController); + wristTab.addString("mode", () -> currentMode.toString()); + elevatorTab.addDouble("Elevator Target Extension", () -> targetExtension); + elevatorTab.addDouble("Elevator Current Extension", () -> currentExtension); + elevatorTab.addDouble("Elevator Output", rightMotor::getMotorOutputPercent); + elevatorTab.addDouble("percent control", () -> elevatorPercentControl); + elevatorTab.addDouble("velocity", rightMotor::getSelectedSensorVelocity); + elevatorTab.addDouble("filter output", () -> elevatorFilterOutput); + elevatorTab.addDouble("Stator current", rightMotor::getStatorCurrent); + elevatorTab.addBoolean("is zeroed", () -> elevatorZeroed); + elevatorTab.addString("mode", () -> currentMode.toString()); } public void setMode(Modes mode) { @@ -180,12 +197,17 @@ public void setTargetState(ElevatorState targetState) { targetAngle = targetState.angle(); } - public void setPercentControl(double percentControl) { - this.percentControl = percentControl; + public void setPercentControl(double elevatorPercentControl, double wristPercentControl) { + this.elevatorPercentControl = elevatorPercentControl; + this.wristPercentControl = wristPercentControl; + } + + public double getElevatorPercentControl() { + return elevatorPercentControl; } - public double getPercentControl() { - return percentControl; + public double getWristPercentControl() { + return wristPercentControl; } public double getTargetExtension() { @@ -196,41 +218,56 @@ public double getTargetAngle() { return targetAngle; } - public double getExtensionInches() { + public double getCurrentExtensionInches() { return ticksToExtensionInches(getCurrentTicks()); } public double getCurrentAngleDegrees() { - return canCoder.getAbsolutePosition(); + // 1:128 ratio + return (wristMotor.getSelectedSensorPosition() * 360) / (128 * 2048); + // return canCoder.getAbsolutePosition(); } private double angleToTicks(double angle) { - return angle / Elevator.WRIST_TICKS; + return angle / 360 * 128 * 2048; + } + + private double ticksToAngle(double ticks) { + return ticks / 2048 / 128 * 360; } public void setTargetAngle(double targetAngle) { this.targetAngle = targetAngle; } - private double applySlowZoneToPercent(double percentControl) { + public void setNotZeroed() { + elevatorZeroed = false; + wristZeroed = false; + } + + private double applySlowZoneToElevatorPercent(double elevatorPercentControl) { if ((currentExtension > (Elevator.MAX_EXTENSION_INCHES - 7)) || (currentExtension < (Elevator.MIN_EXTENSION_INCHES + 7))) { - isInSlowZone = true; - return percentControl * 0.5; + return MathUtil.clamp(elevatorPercentControl * 0.5, -1, 1); } - isInSlowZone = false; - return percentControl; + return MathUtil.clamp(elevatorPercentControl, -1, 1); } private void percentDrivePeriodic() { - rightMotor.set(TalonFXControlMode.PercentOutput, applySlowZoneToPercent(percentControl) + 0.02); - isInSlowZone = false; + if (elevatorFilterOutput > Elevator.STATOR_LIMIT) { + rightMotor.set(TalonFXControlMode.PercentOutput, 0); + } else { + rightMotor.set( + TalonFXControlMode.PercentOutput, + applySlowZoneToElevatorPercent(elevatorPercentControl) + 0.02); + } // rightMotor.set(TalonFXControlMode.MotionMagic, extensionInchesToTicks(targetExtension)); - - wristMotor.set( - TalonFXControlMode.PercentOutput, - MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25)); + if (wristFilterOutput > Elevator.STATOR_LIMIT) { + wristMotor.set(TalonFXControlMode.PercentOutput, 0); + } else { + wristMotor.set(TalonFXControlMode.PercentOutput, MathUtil.clamp(wristPercentControl, -1, 1)); + } } private void positionDrivePeriodic() { @@ -244,39 +281,67 @@ private void positionDrivePeriodic() { // } else { // rightMotor.set(TalonFXControlMode.PercentOutput, 0); // } - rightMotor.set(TalonFXControlMode.MotionMagic, extensionInchesToTicks(targetExtension)); + if (elevatorFilterOutput > Elevator.STATOR_LIMIT) { + rightMotor.set(TalonFXControlMode.PercentOutput, 0); + } else { + rightMotor.set(TalonFXControlMode.MotionMagic, extensionInchesToTicks(targetExtension)); + } - wristMotor.set( - TalonFXControlMode.PercentOutput, - MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25)); + if (wristFilterOutput > Elevator.STATOR_LIMIT) { + wristMotor.set(TalonFXControlMode.PercentOutput, 0); + } else { + wristMotor.set( + TalonFXControlMode.PercentOutput, + MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25)); + } } private void zeroPeriodic() { + rightMotor.configForwardSoftLimitEnable(false, 20); rightMotor.configReverseSoftLimitEnable(false, 20); - rightMotor.set(ControlMode.PercentOutput, Elevator.ZERO_MOTOR_POWER); + rightMotor.set(TalonFXControlMode.PercentOutput, Elevator.ZERO_MOTOR_POWER); - if (filterOutput > Elevator.ZERO_STATOR_LIMIT) { + if (elevatorFilterOutput > Elevator.ZERO_STATOR_LIMIT) { rightMotor.setSelectedSensorPosition(0); leftMotor.setSelectedSensorPosition(0); - rightMotor.set(ControlMode.PercentOutput, 0); - - currentMode = Modes.POSITION_CONTROL; + rightMotor.set(TalonFXControlMode.PercentOutput, 0); rightMotor.configForwardSoftLimitEnable(true, 20); rightMotor.configReverseSoftLimitEnable(true, 20); targetExtension = Elevator.MIN_EXTENSION_INCHES; + + elevatorZeroed = true; + } + if (elevatorZeroed) { + wristMotor.configForwardSoftLimitEnable(false, 20); + wristMotor.configReverseSoftLimitEnable(false); + wristMotor.set(TalonFXControlMode.PercentOutput, Elevator.ZERO_MOTOR_POWER); + if (wristFilterOutput > Elevator.ZERO_STATOR_LIMIT) { + wristMotor.setSelectedSensorPosition(0); + + wristMotor.set(TalonFXControlMode.PercentOutput, 0); + + wristMotor.configForwardSoftLimitEnable(true); + wristMotor.configReverseSoftLimitEnable(true); + + wristZeroed = true; + } + } + if (wristZeroed && elevatorZeroed) { + currentMode = Modes.POSITION_CONTROL; } } @Override public void periodic() { - currentExtension = getExtensionInches(); + currentExtension = getCurrentExtensionInches(); currentWristAngle = getCurrentAngleDegrees(); - filterOutput = filter.calculate(rightMotor.getStatorCurrent()); + elevatorFilterOutput = elevatorFilter.calculate(rightMotor.getStatorCurrent()); + wristFilterOutput = wristFilter.calculate(wristMotor.getStatorCurrent()); switch (currentMode) { case ZERO: From 67252209a707874f233ce61e817be23f542b74eb Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 7 Nov 2023 19:12:41 -0800 Subject: [PATCH 26/63] ToF implementaiton --- src/main/java/frc/robot/Constants.java | 2 ++ .../frc/robot/subsystems/IntakeSubsystem.java | 15 +++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9de2384..f55b2bd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -246,6 +246,8 @@ public static final class Intake { public static final class Ports { public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor"); + public static final int CONE_TOF_PORT = 0; + public static final int CUBE_TOF_PORT = 0; } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 51f50c0..99c6998 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.Intake; +import com.playingwithfusion.TimeOfFlight; public class IntakeSubsystem extends SubsystemBase { private TalonFX intakeMotor; @@ -20,6 +21,7 @@ public class IntakeSubsystem extends SubsystemBase { private Modes currentIntakeMode; private LinearFilter filter; private double statorCurrentLimit; + private final TimeOfFlight coneToF, cubeToF; /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { @@ -31,9 +33,14 @@ public IntakeSubsystem() { filter = LinearFilter.movingAverage(30); + coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT); + cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT); + shuffleboard.addDouble( "Intake motor sensor position", () -> intakeMotor.getSelectedSensorPosition()); shuffleboard.addString("Current mode", () -> currentIntakeMode.toString()); + shuffleboard.addDouble("coneToFInches", this::getConeToFInches); + shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches); } public enum Modes { @@ -58,6 +65,14 @@ public void intakePeriodic(Modes mode) { } } + private double getCubeToFInches() { + return cubeToF.getRange() / 25.4; + } + + private double getConeToFInches() { + return coneToF.getRange() / 25.4; + } + public Modes getMode() { return currentIntakeMode; } From 3f84e82f825e06a5b24d44a15535490c569fcc59 Mon Sep 17 00:00:00 2001 From: Derek Date: Tue, 7 Nov 2023 20:20:45 -0800 Subject: [PATCH 27/63] Working intake --- src/main/java/frc/robot/Constants.java | 19 ++++----- src/main/java/frc/robot/RobotContainer.java | 24 +++++------ .../frc/robot/subsystems/IntakeSubsystem.java | 41 ++++++++++++------- 3 files changed, 47 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f55b2bd..d7432cd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -197,23 +197,22 @@ public static final class Ports { public static final int ELEVATOR_LEFT_MOTOR_PORT = CAN.at(14, "elevator left motor"); public static final int ELEVATOR_RIGHT_MOTOR_PORT = CAN.at(15, "elevator right motor"); public static final int WRIST_MOTOR_PORT = CAN.at(16, "wrist motor port"); - public static final int CANCODER = CAN.at(0, "cancoder"); } public static final class Setpoints { - public static final ElevatorState STOWED = new ElevatorState(0, 0); + public static final ElevatorState STOWED = + new ElevatorState(MIN_EXTENSION_INCHES, MIN_ANGLE_DEGREES); public static final ElevatorState SHELF_INTAKE = new ElevatorState(20, 20); - public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION_INCHES, 0); - public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 0); - public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 20); - public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 40); - public static final ElevatorState ZERO = new ElevatorState(MIN_EXTENSION_INCHES, 0); + public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION_INCHES, 80); + public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 20); + public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); + public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 60); } public static final double MAX_EXTENSION_INCHES = 54; public static final double MIN_EXTENSION_INCHES = 1; - public static final double MIN_ANGLE_DEGREES = 0; + public static final double MIN_ANGLE_DEGREES = 3; public static final double MAX_ANGLE_DEGREES = 109.25; public static final int FALCON_CPR = 2048; @@ -238,9 +237,9 @@ public static final class Setpoints { public static final class Intake { - public static final double INTAKE_PERCENT = 1; + public static final double INTAKE_PERCENT = 0.7; - public static final double OUTTAKE_PERCENT = -1; + public static final double OUTTAKE_PERCENT = -0.7; public static final double HOLD_PERCENT = 0.15; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1599c77..74e01d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -288,11 +288,9 @@ private void configureButtonBindings() { .off(jacob.rightTrigger()) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE)); - jacobLayer - .off(jacob.x()) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) - .onTrue( - new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)); + jacobLayer.off(jacob.x()).onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); + // .onTrue( + // new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)); // intake presets // jacobLayer @@ -319,7 +317,7 @@ private void configureButtonBindings() { anthony .povLeft() - .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.ZERO)) + .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) .onTrue(new SetZeroModeCommand(elevatorSubsystem)); @@ -331,13 +329,13 @@ private void configureButtonBindings() { elevatorSubsystem, () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); - // jacob - // .leftBumper() - // .onTrue( - // new GroundPickupCommand( - // intakeSubsystem, - // elevatorSubsystem, - // () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); + jacob + .a() + .onTrue( + new GroundPickupCommand( + intakeSubsystem, + elevatorSubsystem, + () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); jacobLayer .off(jacob.povUp()) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 99c6998..9bec834 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -13,34 +13,43 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.Intake; -import com.playingwithfusion.TimeOfFlight; public class IntakeSubsystem extends SubsystemBase { private TalonFX intakeMotor; private ShuffleboardTab shuffleboard = Shuffleboard.getTab("Intake Subsystem"); private Modes currentIntakeMode; private LinearFilter filter; + private double filterOutput; private double statorCurrentLimit; - private final TimeOfFlight coneToF, cubeToF; + // private final TimeOfFlight coneToF, cubeToF; /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { intakeMotor = new TalonFX(Constants.Intake.Ports.INTAKE_MOTOR_PORT); - currentIntakeMode = Modes.OFF; + intakeMotor.configFactoryDefault(); + intakeMotor.clearStickyFaults(); intakeMotor.setNeutralMode(NeutralMode.Brake); filter = LinearFilter.movingAverage(30); - coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT); - cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT); + currentIntakeMode = Modes.OFF; + + filterOutput = 0.0; + + statorCurrentLimit = 30; + + // coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT); + // cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT); shuffleboard.addDouble( "Intake motor sensor position", () -> intakeMotor.getSelectedSensorPosition()); shuffleboard.addString("Current mode", () -> currentIntakeMode.toString()); - shuffleboard.addDouble("coneToFInches", this::getConeToFInches); - shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches); + shuffleboard.addDouble("filter output", () -> filterOutput); + shuffleboard.addDouble("motor output", intakeMotor::getMotorOutputPercent); + // shuffleboard.addDouble("coneToFInches", this::getConeToFInches); + // shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches); } public enum Modes { @@ -55,23 +64,26 @@ public void intakePeriodic(Modes mode) { switch (mode) { case INTAKE: intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_PERCENT); + break; case OUTTAKE: intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_PERCENT); + break; case HOLD: intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_PERCENT); + break; case OFF: default: intakeMotor.set(TalonFXControlMode.PercentOutput, 0); } } - private double getCubeToFInches() { - return cubeToF.getRange() / 25.4; - } + // private double getCubeToFInches() { + // return cubeToF.getRange() / 25.4; + // } - private double getConeToFInches() { - return coneToF.getRange() / 25.4; - } + // private double getConeToFInches() { + // return coneToF.getRange() / 25.4; + // } public Modes getMode() { return currentIntakeMode; @@ -83,7 +95,8 @@ public void setMode(Modes mode) { @Override public void periodic() { - if (filter.calculate(intakeMotor.getStatorCurrent()) >= statorCurrentLimit) { + filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); + if (filterOutput >= statorCurrentLimit) { currentIntakeMode = Modes.HOLD; } From f302a1561a9c0cf81a8a4ef97e05fbd3eec50d9e Mon Sep 17 00:00:00 2001 From: audrey Date: Thu, 9 Nov 2023 14:46:12 -0800 Subject: [PATCH 28/63] making it so intake is either cone/cube with right joystick Notes: - we should find a better button it was all thats available - I have a sneaky suspicion autos won't work properly, I can't tell how they know whether to intake cone vs cube --- src/main/java/frc/robot/Constants.java | 9 +- src/main/java/frc/robot/RobotContainer.java | 197 ++++++++++-------- .../autonomous/commands/MobilityAuto.java | 2 +- .../N1_1ConePlus2CubeHybridMobility.java | 3 +- ...N1_1ConePlus2CubeHybridMobilityEngage.java | 3 +- .../commands/N3_1ConePlusMobility.java | 5 +- .../commands/N3_1ConePlusMobilityEngage.java | 4 +- .../commands/N9_1ConePlus2CubeMobility.java | 5 +- .../commands/N9_1ConePlusMobility.java | 5 +- .../commands/N9_1ConePlusMobilityEngage.java | 5 +- .../robot/commands/GroundPickupCommand.java | 7 +- .../frc/robot/commands/IntakeModeCommand.java | 11 +- .../java/frc/robot/commands/ScoreCommand.java | 5 +- .../frc/robot/subsystems/IntakeSubsystem.java | 28 ++- 14 files changed, 178 insertions(+), 111 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 37c82ac..daf9431 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -237,11 +237,14 @@ public static final class Setpoints { public static final class Intake { - public static final double INTAKE_PERCENT = 0.7; + public static final double INTAKE_CONE_PERCENT = 0.7; + public static final double INTAKE_CUBE_PERCENT = -0.7; - public static final double OUTTAKE_PERCENT = -0.7; + public static final double OUTTAKE_CONE_PERCENT = -0.7; + public static final double OUTTAKE_CUBE_PERCENT = 0.7; - public static final double HOLD_PERCENT = 0.1; + public static final double HOLD_CONE_PERCENT = 0.1; + public static final double HOLD_CUBE_PERCENT = -0.1; public static final class Ports { public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor"); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e05e250..5e154e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import com.pathplanner.lib.server.PathPlannerServer; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -23,6 +24,16 @@ import frc.robot.Constants.Config; import frc.robot.Constants.Drive; import frc.robot.Constants.Elevator; +import frc.robot.autonomous.commands.MobilityAuto; +import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobility; +import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobilityEngage; +import frc.robot.autonomous.commands.N2_Engage; +import frc.robot.autonomous.commands.N3_1ConePlusMobility; +import frc.robot.autonomous.commands.N3_1ConePlusMobilityEngage; +import frc.robot.autonomous.commands.N6_1ConePlusEngage; +import frc.robot.autonomous.commands.N9_1ConePlus2CubeMobility; +import frc.robot.autonomous.commands.N9_1ConePlusMobility; +import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.DriveToPlaceCommand; @@ -56,6 +67,7 @@ import frc.util.NodeSelectorUtility.NodeType; import frc.util.SharedReference; import frc.util.Util; +import frc.util.pathing.AlliancePose2d; import frc.util.pathing.RubenManueverGenerator; import java.util.HashMap; import java.util.List; @@ -282,13 +294,16 @@ private void configureButtonBindings() { // outtake states jacobLayer .off(jacob.leftTrigger()) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); + .onTrue( + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(jacob.rightStick()))); jacobLayer .off(jacob.rightTrigger()) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE)); + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE, Optional.empty())); - jacobLayer.off(jacob.x()).onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); + jacobLayer + .off(jacob.x()) + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty())); // .onTrue( // new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)); @@ -305,20 +320,22 @@ private void configureButtonBindings() { .onTrue( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) - .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); + .whileTrue( + new IntakeModeCommand( + intakeSubsystem, Modes.INTAKE, Optional.of(anthony.rightStick()))); // reset jacobLayer .off(jacob.y()) .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty())); jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); anthony .povLeft() .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty())) .onTrue(new SetZeroModeCommand(elevatorSubsystem)); anthony @@ -327,7 +344,8 @@ private void configureButtonBindings() { new GroundPickupCommand( intakeSubsystem, elevatorSubsystem, - () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); + () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE, + Optional.of(jacob.rightStick()))); jacob .a() @@ -335,12 +353,13 @@ private void configureButtonBindings() { new GroundPickupCommand( intakeSubsystem, elevatorSubsystem, - () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE)); + () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE, + Optional.of(jacob.rightStick()))); jacobLayer .off(jacob.povUp()) .onTrue( - new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) + new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE, Optional.empty()) .alongWith( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.GROUND_INTAKE))); @@ -350,13 +369,13 @@ private void configureButtonBindings() { jacobLayer .off(jacob.back()) .whileTrue( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE) + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(jacob.rightStick())) .alongWith( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))) .onFalse( new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty()))); // scoring // jacobLayer @@ -501,90 +520,90 @@ public void end(boolean interrupted) { .andThen( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) - .andThen(new IntakeModeCommand(intakeSubsystem, Modes.OFF))), + .andThen( + new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty()))), "armbat preload", new ElevatorPositionCommand(elevatorSubsystem, 30, 0) .andThen( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.STOWED))); - // autoSelector.setDefaultOption( - // "N1 1Cone + 2Cube Low Mobility Engage", - // new N1_1ConePlus2CubeHybridMobilityEngage( - // 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.setDefaultOption( - // "N1 1Cone + 2Cube Low Mobility NO ENGAGE", - // new N1_1ConePlus2CubeHybridMobility( - // 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.setDefaultOption( - // "N9 1Cone + 1Cube + Grab Cube Mobility", - // new N9_1ConePlus2CubeMobility( - // 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.addOption( - // "Just Zero Elevator [DOES NOT CALIBRATE]", - // new SetZeroModeCommand(elevatorSubsystem, false)); - - // autoSelector.addOption( - // "Near Substation Mobility [APRILTAG]", - // new MobilityAuto( - // manueverGenerator, - // drivebaseSubsystem, - // intakeSubsystem, - // elevatorSubsystem, - // rgbSubsystem, - // new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); - - // autoSelector.addOption( - // "Far Substation Mobility [APRILTAG]", - // new MobilityAuto( - // manueverGenerator, - // drivebaseSubsystem, - // intakeSubsystem, - // elevatorSubsystem, - // rgbSubsystem, - // new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); - - // autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); - - // autoSelector.addOption( - // "N3 1Cone + Mobility Engage", - // new N3_1ConePlusMobilityEngage( - // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.setDefaultOption( - // "N3 1Cone + Mobility", - // new N3_1ConePlusMobility( - // 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.setDefaultOption( - // "N6 1Cone + Engage", - // new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.addOption( - // "N9 1Cone + Mobility Engage", - // new N9_1ConePlusMobilityEngage( - // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.addOption( - // "N9 1Cone + Mobility", - // new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, - // drivebaseSubsystem)); - - // autoSelector.addOption( - // "Score High Cone [DOES NOT CALIBRATE]", - // new SetZeroModeCommand( - // elevatorSubsystem, - // false) // FIXME pretty sure this shouldn't zero wrist, double check later - // .raceWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)) - // .andThen( - // new ScoreCommand( - // intakeSubsystem, - // elevatorSubsystem, - // Constants.SCORE_STEP_MAP.get( - // NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))); + autoSelector.setDefaultOption( + "N1 1Cone + 2Cube Low Mobility Engage", + new N1_1ConePlus2CubeHybridMobilityEngage( + 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.setDefaultOption( + "N1 1Cone + 2Cube Low Mobility NO ENGAGE", + new N1_1ConePlus2CubeHybridMobility( + 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.setDefaultOption( + "N9 1Cone + 1Cube + Grab Cube Mobility", + new N9_1ConePlus2CubeMobility( + 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.addOption( + "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem)); + + autoSelector.addOption( + "Near Substation Mobility [APRILTAG]", + new MobilityAuto( + manueverGenerator, + drivebaseSubsystem, + intakeSubsystem, + elevatorSubsystem, + rgbSubsystem, + new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); + + autoSelector.addOption( + "Far Substation Mobility [APRILTAG]", + new MobilityAuto( + manueverGenerator, + drivebaseSubsystem, + intakeSubsystem, + elevatorSubsystem, + rgbSubsystem, + new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); + + autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); + + autoSelector.addOption( + "N3 1Cone + Mobility Engage", + new N3_1ConePlusMobilityEngage( + 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.setDefaultOption( + "N3 1Cone + Mobility", + new N3_1ConePlusMobility( + 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.setDefaultOption( + "N6 1Cone + Engage", + new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.addOption( + "N9 1Cone + Mobility Engage", + new N9_1ConePlusMobilityEngage( + 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.addOption( + "N9 1Cone + Mobility", + new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.addOption( + "Score High Cone [DOES NOT CALIBRATE]", + new SetZeroModeCommand( + elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check + // later + .raceWith( + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(() -> false)) + .andThen( + new ScoreCommand( + intakeSubsystem, + elevatorSubsystem, + Constants.SCORE_STEP_MAP.get( + NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH)))))); driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0); diff --git a/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java b/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java index be0671e..854b841 100644 --- a/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java +++ b/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java @@ -40,7 +40,7 @@ public MobilityAuto( Optional.of(rgbSubsystem), Optional.empty()) .alongWith(new SetZeroModeCommand(elevatorSubsystem)) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.HOLD)) + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.HOLD, Optional.empty())) .andThen(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem))); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java index bbee325..13395de 100644 --- a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java @@ -17,6 +17,7 @@ import frc.robot.subsystems.IntakeSubsystem.Modes; import frc.util.pathing.LoadMirrorPath; import java.util.Map; +import java.util.Optional; import java.util.function.Supplier; public class N1_1ConePlus2CubeHybridMobility extends SequentialCommandGroup { @@ -39,7 +40,7 @@ public N1_1ConePlus2CubeHybridMobility( new FollowTrajectoryCommand(path, true, drivebaseSubsystem), path.get().getMarkers(), eventMap), - new IntakeModeCommand(intakeSubsystem, Modes.OFF) + new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty()) .alongWith(new SetZeroModeCommand(elevatorSubsystem))); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java index b9cf4c9..0cb13b0 100644 --- a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java @@ -18,6 +18,7 @@ import frc.robot.subsystems.IntakeSubsystem.Modes; import frc.util.pathing.LoadMirrorPath; import java.util.Map; +import java.util.Optional; import java.util.function.Supplier; public class N1_1ConePlus2CubeHybridMobilityEngage extends SequentialCommandGroup { @@ -40,7 +41,7 @@ public N1_1ConePlus2CubeHybridMobilityEngage( eventMap), new EngageCommand( drivebaseSubsystem, elevatorSubsystem, EngageCommand.EngageDirection.GO_BACKWARD) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty())) .alongWith(new SetZeroModeCommand(elevatorSubsystem))); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java index f98fda6..ab792eb 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java @@ -20,6 +20,7 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeType; import frc.util.pathing.LoadMirrorPath; +import java.util.Optional; import java.util.function.Supplier; public class N3_1ConePlusMobility extends SequentialCommandGroup { @@ -36,7 +37,9 @@ public N3_1ConePlusMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)), + .deadlineWith( + new IntakeModeCommand( + intakeSubsystem, IntakeSubsystem.Modes.INTAKE, Optional.of(() -> true))), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java index 1a85ee0..6d7151e 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java @@ -22,6 +22,7 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeType; import frc.util.pathing.LoadMirrorPath; +import java.util.Optional; import java.util.function.Supplier; public class N3_1ConePlusMobilityEngage extends SequentialCommandGroup { @@ -41,7 +42,8 @@ public N3_1ConePlusMobilityEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)), + .deadlineWith( + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(() -> false))), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java index 2dfbf8f..2e39a04 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java @@ -22,6 +22,7 @@ import frc.util.pathing.LoadMirrorPath; import java.util.List; import java.util.Map; +import java.util.Optional; import java.util.function.Supplier; public class N9_1ConePlus2CubeMobility extends SequentialCommandGroup { @@ -39,8 +40,8 @@ public N9_1ConePlus2CubeMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .alongWith(new SetZeroModeCommand(elevatorSubsystem)) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)), + .deadlineWith( + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(() -> false))), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java index f70baef..d08e289 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java @@ -20,6 +20,7 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeType; import frc.util.pathing.LoadMirrorPath; +import java.util.Optional; import java.util.function.Supplier; public class N9_1ConePlusMobility extends SequentialCommandGroup { @@ -37,7 +38,9 @@ public N9_1ConePlusMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)), + .deadlineWith( + new IntakeModeCommand( + intakeSubsystem, IntakeSubsystem.Modes.INTAKE, Optional.of(() -> false))), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java index 209e12d..79ce243 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java @@ -21,6 +21,7 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeType; import frc.util.pathing.LoadMirrorPath; +import java.util.Optional; import java.util.function.Supplier; public class N9_1ConePlusMobilityEngage extends SequentialCommandGroup { @@ -40,7 +41,9 @@ public N9_1ConePlusMobilityEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)), + .deadlineWith( + new IntakeModeCommand( + intakeSubsystem, IntakeSubsystem.Modes.INTAKE, Optional.of(() -> false))), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index f169246..e3e06db 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -9,6 +9,8 @@ import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; +import java.util.Optional; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -19,12 +21,13 @@ public class GroundPickupCommand extends SequentialCommandGroup { public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, - Supplier modeSupplier) { + Supplier modeSupplier, + Optional isCube) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)) + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube)) .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))); } } diff --git a/src/main/java/frc/robot/commands/IntakeModeCommand.java b/src/main/java/frc/robot/commands/IntakeModeCommand.java index 3277fc0..dd9020a 100644 --- a/src/main/java/frc/robot/commands/IntakeModeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeModeCommand.java @@ -7,17 +7,21 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; +import java.util.Optional; +import java.util.function.BooleanSupplier; public class IntakeModeCommand extends CommandBase { private IntakeSubsystem intakeSubsystem; private Modes mode; + private Optional isCone; /** Creates a new IntakeCommand. */ - public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode) { + public IntakeModeCommand( + IntakeSubsystem intakeSubsystem, Modes mode, Optional isCone) { // Use addRequirements() here to declare subsystem dependencies. this.intakeSubsystem = intakeSubsystem; this.mode = mode; - + this.isCone = isCone; addRequirements(intakeSubsystem); } @@ -26,6 +30,9 @@ public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode) { @Override public void initialize() { intakeSubsystem.setMode(mode); + if (isCone.isPresent()) { + intakeSubsystem.setIsCube(isCone.get().getAsBoolean()); + } } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ScoreCommand.java b/src/main/java/frc/robot/commands/ScoreCommand.java index e51a883..5e9680a 100644 --- a/src/main/java/frc/robot/commands/ScoreCommand.java +++ b/src/main/java/frc/robot/commands/ScoreCommand.java @@ -71,11 +71,12 @@ private Command createStep(ScoreStep scoreStep) { var intakeState = scoreStep.intakeState(); if (elevatorState.isPresent() && intakeState.isPresent()) { return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, intakeState.get())); + .deadlineWith( + new IntakeModeCommand(intakeSubsystem, intakeState.get(), Optional.empty())); } else if (elevatorState.isPresent()) { return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()); } else if (intakeState.isPresent()) { - return new IntakeModeCommand(intakeSubsystem, intakeState.get()); + return new IntakeModeCommand(intakeSubsystem, intakeState.get(), Optional.empty()); } else { throw new IllegalArgumentException("ScoreStep must have at least one state"); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 954da45..76a4f4b 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -21,6 +21,7 @@ public class IntakeSubsystem extends SubsystemBase { private LinearFilter filter; private double filterOutput; private double statorCurrentLimit; + private boolean isCube; // private final TimeOfFlight coneToF, cubeToF; /** Creates a new IntakeSubsystem. */ @@ -41,6 +42,8 @@ public IntakeSubsystem() { statorCurrentLimit = 30; + isCube = true; + // coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT); // cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT); @@ -49,6 +52,7 @@ public IntakeSubsystem() { shuffleboard.addString("Current mode", () -> currentIntakeMode.toString()); shuffleboard.addDouble("filter output", () -> filterOutput); shuffleboard.addDouble("motor output", intakeMotor::getMotorOutputPercent); + shuffleboard.addBoolean("is cone", () -> isCube); // shuffleboard.addDouble("coneToFInches", this::getConeToFInches); // shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches); } @@ -64,13 +68,25 @@ public void intakePeriodic(Modes mode) { switch (mode) { case INTAKE: - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_PERCENT); + if (isCube) { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_CUBE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_CONE_PERCENT); + } break; case OUTTAKE: - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_PERCENT); + if (isCube) { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_CUBE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_CONE_PERCENT); + } break; case HOLD: - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_PERCENT); + if (isCube) { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_CUBE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_CONE_PERCENT); + } break; case OFF: default: @@ -95,6 +111,10 @@ public void setMode(Modes mode) { currentIntakeMode = mode; } + public void setIsCube(boolean isCube) { + this.isCube = isCube; + } + private double getFilterCalculatedValue() { return filter.calculate(intakeMotor.getStatorCurrent()); } @@ -103,7 +123,7 @@ private double getFilterCalculatedValue() { public void periodic() { filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); if (filterOutput >= statorCurrentLimit) { - currentIntakeMode = Modes.HOLD; + currentIntakeMode = Modes.OFF; } intakePeriodic(currentIntakeMode); From bb50f7664133934fdfa51dda5171fd4ca40c1428 Mon Sep 17 00:00:00 2001 From: Derek Date: Thu, 9 Nov 2023 16:36:57 -0800 Subject: [PATCH 29/63] switch from optional to booleansuppliers --- src/main/java/frc/robot/RobotContainer.java | 32 ++++++++----------- .../autonomous/commands/MobilityAuto.java | 2 +- .../N1_1ConePlus2CubeHybridMobility.java | 3 +- ...N1_1ConePlus2CubeHybridMobilityEngage.java | 3 +- .../commands/N3_1ConePlusMobility.java | 4 +-- .../commands/N3_1ConePlusMobilityEngage.java | 4 +-- .../commands/N6_1ConePlusEngage.java | 2 +- .../commands/N9_1ConePlus2CubeMobility.java | 4 +-- .../commands/N9_1ConePlusMobility.java | 4 +-- .../commands/N9_1ConePlusMobilityEngage.java | 4 +-- .../robot/commands/GroundPickupCommand.java | 3 +- .../frc/robot/commands/IntakeModeCommand.java | 18 +++++++---- .../java/frc/robot/commands/ScoreCommand.java | 5 ++- 13 files changed, 37 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5e154e3..0fbd8dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -294,16 +294,13 @@ private void configureButtonBindings() { // outtake states jacobLayer .off(jacob.leftTrigger()) - .onTrue( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(jacob.rightStick()))); + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.rightStick())); jacobLayer .off(jacob.rightTrigger()) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE, Optional.empty())); + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE)); - jacobLayer - .off(jacob.x()) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty())); + jacobLayer.off(jacob.x()).onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); // .onTrue( // new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)); @@ -320,22 +317,20 @@ private void configureButtonBindings() { .onTrue( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) - .whileTrue( - new IntakeModeCommand( - intakeSubsystem, Modes.INTAKE, Optional.of(anthony.rightStick()))); + .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightStick())); // reset jacobLayer .off(jacob.y()) .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty())); + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); anthony .povLeft() .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty())) + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) .onTrue(new SetZeroModeCommand(elevatorSubsystem)); anthony @@ -345,7 +340,7 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE, - Optional.of(jacob.rightStick()))); + jacob.rightStick())); jacob .a() @@ -354,12 +349,12 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE, - Optional.of(jacob.rightStick()))); + jacob.rightStick())); jacobLayer .off(jacob.povUp()) .onTrue( - new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE, Optional.empty()) + new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) .alongWith( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.GROUND_INTAKE))); @@ -369,13 +364,13 @@ private void configureButtonBindings() { jacobLayer .off(jacob.back()) .whileTrue( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(jacob.rightStick())) + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.rightStick()) .alongWith( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))) .onFalse( new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty()))); + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); // scoring // jacobLayer @@ -520,8 +515,7 @@ public void end(boolean interrupted) { .andThen( new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) - .andThen( - new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty()))), + .andThen(new IntakeModeCommand(intakeSubsystem, Modes.OFF))), "armbat preload", new ElevatorPositionCommand(elevatorSubsystem, 30, 0) .andThen( @@ -597,7 +591,7 @@ public void end(boolean interrupted) { elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check // later .raceWith( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(() -> false)) + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false) .andThen( new ScoreCommand( intakeSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java b/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java index 854b841..be0671e 100644 --- a/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java +++ b/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java @@ -40,7 +40,7 @@ public MobilityAuto( Optional.of(rgbSubsystem), Optional.empty()) .alongWith(new SetZeroModeCommand(elevatorSubsystem)) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.HOLD, Optional.empty())) + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.HOLD)) .andThen(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem))); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java index 13395de..bbee325 100644 --- a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java @@ -17,7 +17,6 @@ import frc.robot.subsystems.IntakeSubsystem.Modes; import frc.util.pathing.LoadMirrorPath; import java.util.Map; -import java.util.Optional; import java.util.function.Supplier; public class N1_1ConePlus2CubeHybridMobility extends SequentialCommandGroup { @@ -40,7 +39,7 @@ public N1_1ConePlus2CubeHybridMobility( new FollowTrajectoryCommand(path, true, drivebaseSubsystem), path.get().getMarkers(), eventMap), - new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty()) + new IntakeModeCommand(intakeSubsystem, Modes.OFF) .alongWith(new SetZeroModeCommand(elevatorSubsystem))); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java index 0cb13b0..b9cf4c9 100644 --- a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java @@ -18,7 +18,6 @@ import frc.robot.subsystems.IntakeSubsystem.Modes; import frc.util.pathing.LoadMirrorPath; import java.util.Map; -import java.util.Optional; import java.util.function.Supplier; public class N1_1ConePlus2CubeHybridMobilityEngage extends SequentialCommandGroup { @@ -41,7 +40,7 @@ public N1_1ConePlus2CubeHybridMobilityEngage( eventMap), new EngageCommand( drivebaseSubsystem, elevatorSubsystem, EngageCommand.EngageDirection.GO_BACKWARD) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF, Optional.empty())) + .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) .alongWith(new SetZeroModeCommand(elevatorSubsystem))); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java index ab792eb..7ad7100 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java @@ -20,7 +20,6 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeType; import frc.util.pathing.LoadMirrorPath; -import java.util.Optional; import java.util.function.Supplier; public class N3_1ConePlusMobility extends SequentialCommandGroup { @@ -38,8 +37,7 @@ public N3_1ConePlusMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) .deadlineWith( - new IntakeModeCommand( - intakeSubsystem, IntakeSubsystem.Modes.INTAKE, Optional.of(() -> true))), + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> false)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java index 6d7151e..8bbe390 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java @@ -22,7 +22,6 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeType; import frc.util.pathing.LoadMirrorPath; -import java.util.Optional; import java.util.function.Supplier; public class N3_1ConePlusMobilityEngage extends SequentialCommandGroup { @@ -42,8 +41,7 @@ public N3_1ConePlusMobilityEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(() -> false))), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java b/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java index 55d3238..edf4932 100644 --- a/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java @@ -39,7 +39,7 @@ public N6_1ConePlusEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java index 2e39a04..33aed87 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java @@ -22,7 +22,6 @@ import frc.util.pathing.LoadMirrorPath; import java.util.List; import java.util.Map; -import java.util.Optional; import java.util.function.Supplier; public class N9_1ConePlus2CubeMobility extends SequentialCommandGroup { @@ -40,8 +39,7 @@ public N9_1ConePlus2CubeMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, Optional.of(() -> false))), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java index d08e289..c08f5dd 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java @@ -20,7 +20,6 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeType; import frc.util.pathing.LoadMirrorPath; -import java.util.Optional; import java.util.function.Supplier; public class N9_1ConePlusMobility extends SequentialCommandGroup { @@ -39,8 +38,7 @@ public N9_1ConePlusMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) .deadlineWith( - new IntakeModeCommand( - intakeSubsystem, IntakeSubsystem.Modes.INTAKE, Optional.of(() -> false))), + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> false)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java index 79ce243..91b4f3b 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java @@ -21,7 +21,6 @@ import frc.util.NodeSelectorUtility.Height; import frc.util.NodeSelectorUtility.NodeType; import frc.util.pathing.LoadMirrorPath; -import java.util.Optional; import java.util.function.Supplier; public class N9_1ConePlusMobilityEngage extends SequentialCommandGroup { @@ -42,8 +41,7 @@ public N9_1ConePlusMobilityEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) .deadlineWith( - new IntakeModeCommand( - intakeSubsystem, IntakeSubsystem.Modes.INTAKE, Optional.of(() -> false))), + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> false)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index e3e06db..238a684 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -9,7 +9,6 @@ import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; -import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -22,7 +21,7 @@ public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, Supplier modeSupplier, - Optional isCube) { + BooleanSupplier isCube) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( diff --git a/src/main/java/frc/robot/commands/IntakeModeCommand.java b/src/main/java/frc/robot/commands/IntakeModeCommand.java index dd9020a..cc3c63f 100644 --- a/src/main/java/frc/robot/commands/IntakeModeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeModeCommand.java @@ -7,17 +7,15 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; -import java.util.Optional; import java.util.function.BooleanSupplier; public class IntakeModeCommand extends CommandBase { private IntakeSubsystem intakeSubsystem; private Modes mode; - private Optional isCone; + private BooleanSupplier isCone; /** Creates a new IntakeCommand. */ - public IntakeModeCommand( - IntakeSubsystem intakeSubsystem, Modes mode, Optional isCone) { + public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone) { // Use addRequirements() here to declare subsystem dependencies. this.intakeSubsystem = intakeSubsystem; this.mode = mode; @@ -25,13 +23,21 @@ public IntakeModeCommand( addRequirements(intakeSubsystem); } + public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode) { + // Use addRequirements() here to declare subsystem dependencies. + this.intakeSubsystem = intakeSubsystem; + this.mode = mode; + this.isCone = null; + addRequirements(intakeSubsystem); + } + // Called when the command is initially scheduled. @Override public void initialize() { intakeSubsystem.setMode(mode); - if (isCone.isPresent()) { - intakeSubsystem.setIsCube(isCone.get().getAsBoolean()); + if (isCone != null) { + intakeSubsystem.setIsCube(isCone.getAsBoolean()); } } diff --git a/src/main/java/frc/robot/commands/ScoreCommand.java b/src/main/java/frc/robot/commands/ScoreCommand.java index 5e9680a..e51a883 100644 --- a/src/main/java/frc/robot/commands/ScoreCommand.java +++ b/src/main/java/frc/robot/commands/ScoreCommand.java @@ -71,12 +71,11 @@ private Command createStep(ScoreStep scoreStep) { var intakeState = scoreStep.intakeState(); if (elevatorState.isPresent() && intakeState.isPresent()) { return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()) - .deadlineWith( - new IntakeModeCommand(intakeSubsystem, intakeState.get(), Optional.empty())); + .deadlineWith(new IntakeModeCommand(intakeSubsystem, intakeState.get())); } else if (elevatorState.isPresent()) { return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()); } else if (intakeState.isPresent()) { - return new IntakeModeCommand(intakeSubsystem, intakeState.get(), Optional.empty()); + return new IntakeModeCommand(intakeSubsystem, intakeState.get()); } else { throw new IllegalArgumentException("ScoreStep must have at least one state"); } From 7337665f7e0dc7e41c07553527819d954c3da4cb Mon Sep 17 00:00:00 2001 From: Derek Date: Thu, 9 Nov 2023 17:29:46 -0800 Subject: [PATCH 30/63] make it work ish --- src/main/java/frc/robot/Constants.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 180 +++++++++--------- .../robot/commands/GroundPickupCommand.java | 18 +- .../frc/robot/commands/IntakeModeCommand.java | 9 +- .../frc/robot/subsystems/IntakeSubsystem.java | 4 +- 5 files changed, 119 insertions(+), 100 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index daf9431..3ad5583 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -202,8 +202,12 @@ public static final class Ports { public static final class Setpoints { public static final ElevatorState STOWED = new ElevatorState(MIN_EXTENSION_INCHES, MIN_ANGLE_DEGREES); - public static final ElevatorState SHELF_INTAKE = new ElevatorState(20, 20); - public static final ElevatorState GROUND_INTAKE = new ElevatorState(MIN_EXTENSION_INCHES, 80); + public static final ElevatorState SHELF_INTAKE_CONE = new ElevatorState(20, 20); + public static final ElevatorState SHELF_INTAKE_CUBE = new ElevatorState(20, 20); + public static final ElevatorState GROUND_INTAKE_CONE = + new ElevatorState(MIN_EXTENSION_INCHES, 75); + public static final ElevatorState GROUND_INTAKE_CUBE = + new ElevatorState(MIN_EXTENSION_INCHES, 85); public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 20); public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 60); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0fbd8dc..2445c47 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,7 +5,6 @@ package frc.robot; import com.pathplanner.lib.server.PathPlannerServer; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -24,16 +23,6 @@ import frc.robot.Constants.Config; import frc.robot.Constants.Drive; import frc.robot.Constants.Elevator; -import frc.robot.autonomous.commands.MobilityAuto; -import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobility; -import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobilityEngage; -import frc.robot.autonomous.commands.N2_Engage; -import frc.robot.autonomous.commands.N3_1ConePlusMobility; -import frc.robot.autonomous.commands.N3_1ConePlusMobilityEngage; -import frc.robot.autonomous.commands.N6_1ConePlusEngage; -import frc.robot.autonomous.commands.N9_1ConePlus2CubeMobility; -import frc.robot.autonomous.commands.N9_1ConePlusMobility; -import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.DriveToPlaceCommand; @@ -67,7 +56,6 @@ import frc.util.NodeSelectorUtility.NodeType; import frc.util.SharedReference; import frc.util.Util; -import frc.util.pathing.AlliancePose2d; import frc.util.pathing.RubenManueverGenerator; import java.util.HashMap; import java.util.List; @@ -357,7 +345,7 @@ private void configureButtonBindings() { new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) .alongWith( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.GROUND_INTAKE))); + elevatorSubsystem, Constants.Elevator.Setpoints.GROUND_INTAKE_CONE))); // jacob.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem)); @@ -468,11 +456,20 @@ private void setupAutonomousCommands() { new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED), "zero everything", new SetZeroModeCommand(elevatorSubsystem), - "intake", + "intake cone", new ElevatorPositionCommand( // edited so that it works with elevator - chooses between // ground or shelf intake elevatorSubsystem, - intakeLow[0] ? Elevator.Setpoints.GROUND_INTAKE : Elevator.Setpoints.SHELF_INTAKE), + intakeLow[0] + ? Elevator.Setpoints.GROUND_INTAKE_CONE + : Elevator.Setpoints.SHELF_INTAKE_CONE), + "intake cone", + new ElevatorPositionCommand( // edited so that it works with elevator - chooses between + // ground or shelf intake + elevatorSubsystem, + intakeLow[0] + ? Elevator.Setpoints.GROUND_INTAKE_CUBE + : Elevator.Setpoints.SHELF_INTAKE_CUBE), // squeeze intake isn't relevant to offseason bot - leaving here just in case /*"squeeze intake", new CommandBase() { @@ -522,82 +519,83 @@ public void end(boolean interrupted) { new ElevatorPositionCommand( elevatorSubsystem, Constants.Elevator.Setpoints.STOWED))); - autoSelector.setDefaultOption( - "N1 1Cone + 2Cube Low Mobility Engage", - new N1_1ConePlus2CubeHybridMobilityEngage( - 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N1 1Cone + 2Cube Low Mobility NO ENGAGE", - new N1_1ConePlus2CubeHybridMobility( - 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N9 1Cone + 1Cube + Grab Cube Mobility", - new N9_1ConePlus2CubeMobility( - 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem)); - - autoSelector.addOption( - "Near Substation Mobility [APRILTAG]", - new MobilityAuto( - manueverGenerator, - drivebaseSubsystem, - intakeSubsystem, - elevatorSubsystem, - rgbSubsystem, - new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); - - autoSelector.addOption( - "Far Substation Mobility [APRILTAG]", - new MobilityAuto( - manueverGenerator, - drivebaseSubsystem, - intakeSubsystem, - elevatorSubsystem, - rgbSubsystem, - new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); - - autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); - - autoSelector.addOption( - "N3 1Cone + Mobility Engage", - new N3_1ConePlusMobilityEngage( - 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N3 1Cone + Mobility", - new N3_1ConePlusMobility( - 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.setDefaultOption( - "N6 1Cone + Engage", - new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "N9 1Cone + Mobility Engage", - new N9_1ConePlusMobilityEngage( - 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "N9 1Cone + Mobility", - new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - autoSelector.addOption( - "Score High Cone [DOES NOT CALIBRATE]", - new SetZeroModeCommand( - elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check - // later - .raceWith( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false) - .andThen( - new ScoreCommand( - intakeSubsystem, - elevatorSubsystem, - Constants.SCORE_STEP_MAP.get( - NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH)))))); + // autoSelector.setDefaultOption( + // "N1 1Cone + 2Cube Low Mobility Engage", + // new N1_1ConePlus2CubeHybridMobilityEngage( + // 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N1 1Cone + 2Cube Low Mobility NO ENGAGE", + // new N1_1ConePlus2CubeHybridMobility( + // 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N9 1Cone + 1Cube + Grab Cube Mobility", + // new N9_1ConePlus2CubeMobility( + // 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem)); + + // autoSelector.addOption( + // "Near Substation Mobility [APRILTAG]", + // new MobilityAuto( + // manueverGenerator, + // drivebaseSubsystem, + // intakeSubsystem, + // elevatorSubsystem, + // rgbSubsystem, + // new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); + + // autoSelector.addOption( + // "Far Substation Mobility [APRILTAG]", + // new MobilityAuto( + // manueverGenerator, + // drivebaseSubsystem, + // intakeSubsystem, + // elevatorSubsystem, + // rgbSubsystem, + // new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); + + // autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N3 1Cone + Mobility Engage", + // new N3_1ConePlusMobilityEngage( + // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N3 1Cone + Mobility", + // new N3_1ConePlusMobility( + // 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.setDefaultOption( + // "N6 1Cone + Engage", + // new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N9 1Cone + Mobility Engage", + // new N9_1ConePlusMobilityEngage( + // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + // autoSelector.addOption( + // "N9 1Cone + Mobility", + // new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, + // drivebaseSubsystem)); + + // autoSelector.addOption( + // "Score High Cone [DOES NOT CALIBRATE]", + // new SetZeroModeCommand( + // elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check + // // later + // .raceWith( + // new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false) + // .andThen( + // new ScoreCommand( + // intakeSubsystem, + // elevatorSubsystem, + // Constants.SCORE_STEP_MAP.get( + // NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH)))))); driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0); diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 238a684..1300d0a 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; @@ -17,6 +18,10 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class GroundPickupCommand extends SequentialCommandGroup { /** Creates a new GroundPickupCommand. */ + private BooleanSupplier isCube; + + private ElevatorSubsystem elevatorSubsystem; + public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, @@ -24,9 +29,20 @@ public GroundPickupCommand( BooleanSupplier isCube) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); + this.isCube = isCube; + this.elevatorSubsystem = elevatorSubsystem; + addCommands( - new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE) + determineIntakeType() .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube)) .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))); } + + private Command determineIntakeType() { + if (isCube.getAsBoolean()) { + return new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE_CUBE); + } else { + return new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE_CONE); + } + } } diff --git a/src/main/java/frc/robot/commands/IntakeModeCommand.java b/src/main/java/frc/robot/commands/IntakeModeCommand.java index cc3c63f..a2d860f 100644 --- a/src/main/java/frc/robot/commands/IntakeModeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeModeCommand.java @@ -36,14 +36,15 @@ public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode) { @Override public void initialize() { intakeSubsystem.setMode(mode); - if (isCone != null) { - intakeSubsystem.setIsCube(isCone.getAsBoolean()); - } } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (isCone != null) { + intakeSubsystem.setIsCube(isCone.getAsBoolean()); + } + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 76a4f4b..6f44ab5 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -40,9 +40,9 @@ public IntakeSubsystem() { filterOutput = 0.0; - statorCurrentLimit = 30; + statorCurrentLimit = 60; - isCube = true; + isCube = false; // coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT); // cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT); From 8bf8e7fa9b4b66cd06a64973786ad3fc25e8339b Mon Sep 17 00:00:00 2001 From: audrey Date: Thu, 9 Nov 2023 19:45:50 -0800 Subject: [PATCH 31/63] make separate cone and cube setpoints --- src/main/java/frc/robot/Constants.java | 35 ++++++++++++------- src/main/java/frc/robot/RobotContainer.java | 28 +++++++-------- .../robot/commands/DefaultDriveCommand.java | 12 +++---- .../robot/commands/GroundPickupCommand.java | 3 -- .../commands/RotateVelocityDriveCommand.java | 10 +++--- .../robot/subsystems/DrivebaseSubsystem.java | 4 +-- 6 files changed, 47 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3ad5583..d03da23 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -208,9 +208,18 @@ public static final class Setpoints { new ElevatorState(MIN_EXTENSION_INCHES, 75); public static final ElevatorState GROUND_INTAKE_CUBE = new ElevatorState(MIN_EXTENSION_INCHES, 85); - public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 20); - public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); - public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 60); + public static final ElevatorState SCORE_HIGH_CONE = + new ElevatorState(MAX_EXTENSION_INCHES, 20); + public static final ElevatorState SCORE_HIGH_CUBE = + new ElevatorState(MAX_EXTENSION_INCHES, 20); + public static final ElevatorState SCORE_MID_CONE = + new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); + public static final ElevatorState SCORE_MID_CUBE = + new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); + public static final ElevatorState SCORE_LOW_CONE = + new ElevatorState(MIN_EXTENSION_INCHES, 60); + public static final ElevatorState SCORE_LOW_CUBE = + new ElevatorState(MIN_EXTENSION_INCHES, 60); } public static final double MAX_EXTENSION_INCHES = 54; @@ -261,27 +270,27 @@ public static final class Ports { Map.of( NodeType.CONE.atHeight(Height.HIGH), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_HIGH).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_HIGH, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE, IntakeSubsystem.Modes.OUTTAKE)), NodeType.CONE.atHeight(Height.MID), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_MID).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_MID, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE, IntakeSubsystem.Modes.OUTTAKE)), NodeType.CONE.atHeight(Height.LOW), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_LOW_CONE).canWaitHere(), new ScoreStep(IntakeSubsystem.Modes.OUTTAKE)), NodeType.CUBE.atHeight(Height.HIGH), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_HIGH).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_HIGH, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CUBE).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CUBE, IntakeSubsystem.Modes.OUTTAKE)), NodeType.CUBE.atHeight(Height.MID), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_MID).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_MID, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_MID_CUBE).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_MID_CUBE, IntakeSubsystem.Modes.OUTTAKE)), NodeType.CUBE.atHeight(Height.LOW), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_LOW_CUBE).canWaitHere(), new ScoreStep(IntakeSubsystem.Modes.OUTTAKE))); public static final class Vision { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2445c47..359b55b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -124,7 +124,7 @@ public RobotContainer() { drivebaseSubsystem, translationXSupplier, translationYSupplier, - anthony.rightBumper(), + // anthony.rightBumper(), anthony.leftBumper())); // // FIXME: This error is here to kind of guide you... @@ -304,8 +304,11 @@ private void configureButtonBindings() { .povUp() .onTrue( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) - .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightStick())); + elevatorSubsystem, + anthony.rightBumper().getAsBoolean() + ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE + : Constants.Elevator.Setpoints.GROUND_INTAKE_CONE)) + .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightBumper())); // reset jacobLayer @@ -323,21 +326,11 @@ private void configureButtonBindings() { anthony .povDown() - .onTrue( - new GroundPickupCommand( - intakeSubsystem, - elevatorSubsystem, - () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE, - jacob.rightStick())); + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.rightStick())); jacob .a() - .onTrue( - new GroundPickupCommand( - intakeSubsystem, - elevatorSubsystem, - () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE, - jacob.rightStick())); + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.rightStick())); jacobLayer .off(jacob.povUp()) @@ -355,7 +348,10 @@ private void configureButtonBindings() { new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.rightStick()) .alongWith( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))) + elevatorSubsystem, + jacob.rightStick().getAsBoolean() + ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE + : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE))) .onFalse( new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); diff --git a/src/main/java/frc/robot/commands/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java index 8977b85..0ef7a44 100644 --- a/src/main/java/frc/robot/commands/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/DefaultDriveCommand.java @@ -23,21 +23,21 @@ public class DefaultDriveCommand extends CommandBase { private final DoubleSupplier translationYSupplier; private final BooleanSupplier isRobotRelativeForwardSupplier; - private final BooleanSupplier isRobotRelativeBackwardSupplier; + // private final BooleanSupplier isRobotRelativeBackwardSupplier; /** Creates a new DefaultDriveCommand. */ public DefaultDriveCommand( DrivebaseSubsystem drivebaseSubsystem, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, - BooleanSupplier isRobotRelativeForwardSupplier, - BooleanSupplier isRobotRelativeBackwardSupplier) { + BooleanSupplier isRobotRelativeForwardSupplier) { + // BooleanSupplier isRobotRelativeBackwardSupplier) { this.drivebaseSubsystem = drivebaseSubsystem; this.translationXSupplier = translationXSupplier; this.translationYSupplier = translationYSupplier; this.isRobotRelativeForwardSupplier = isRobotRelativeForwardSupplier; - this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier; + // this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier; addRequirements(drivebaseSubsystem); } @@ -48,12 +48,12 @@ public void execute() { double x = translationXSupplier.getAsDouble(); double y = translationYSupplier.getAsDouble(); Boolean forwardRelative = isRobotRelativeForwardSupplier.getAsBoolean(); - Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean(); + // Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean(); drivebaseSubsystem.drive( DrivebaseSubsystem.produceChassisSpeeds( forwardRelative, - backwardRelative, + // backwardRelative, x, y, 0, diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 1300d0a..0315355 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -11,7 +11,6 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; import java.util.function.BooleanSupplier; -import java.util.function.Supplier; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -25,10 +24,8 @@ public class GroundPickupCommand extends SequentialCommandGroup { public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, - Supplier modeSupplier, BooleanSupplier isCube) { // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); this.isCube = isCube; this.elevatorSubsystem = elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/RotateVelocityDriveCommand.java b/src/main/java/frc/robot/commands/RotateVelocityDriveCommand.java index 762972f..5c1234b 100644 --- a/src/main/java/frc/robot/commands/RotateVelocityDriveCommand.java +++ b/src/main/java/frc/robot/commands/RotateVelocityDriveCommand.java @@ -22,7 +22,7 @@ public class RotateVelocityDriveCommand extends CommandBase { private final DoubleSupplier rotationSupplier; private final BooleanSupplier isRobotRelativeForwardSupplier; - private final BooleanSupplier isRobotRelativeBackwardSupplier; + // private final BooleanSupplier isRobotRelativeBackwardSupplier; /** Creates a new RotateVelocityDriveCommand. */ public RotateVelocityDriveCommand( @@ -30,15 +30,15 @@ public RotateVelocityDriveCommand( DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, DoubleSupplier rotationSupplier, - BooleanSupplier isRobotRelativeForwardSupplier, - BooleanSupplier isRobotRelativeBackwardSupplier) { + BooleanSupplier isRobotRelativeForwardSupplier) { + // BooleanSupplier isRobotRelativeBackwardSupplier) { this.drivebaseSubsystem = drivebaseSubsystem; this.translationXSupplier = translationXSupplier; this.translationYSupplier = translationYSupplier; this.rotationSupplier = rotationSupplier; this.isRobotRelativeForwardSupplier = isRobotRelativeForwardSupplier; - this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier; + // this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier; addRequirements(drivebaseSubsystem); } @@ -59,7 +59,7 @@ public void execute() { drivebaseSubsystem.drive( DrivebaseSubsystem.produceChassisSpeeds( isRobotRelativeForwardSupplier.getAsBoolean(), - isRobotRelativeBackwardSupplier.getAsBoolean(), + // isRobotRelativeBackwardSupplier.getAsBoolean(), x, y, rot, diff --git a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java index 66f917c..f21e08c 100644 --- a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java @@ -557,13 +557,13 @@ public void periodic() { public static ChassisSpeeds produceChassisSpeeds( boolean isRobotRelativeForward, - boolean isRobotRelativeBackward, + // boolean isRobotRelativeBackward, double x, double y, double rotationVelocity, Rotation2d currentGyroAngle) { if (isRobotRelativeForward) return new ChassisSpeeds(x, y, rotationVelocity); - if (isRobotRelativeBackward) return new ChassisSpeeds(-x, -y, rotationVelocity); + // if (isRobotRelativeBackward) return new ChassisSpeeds(-x, -y, rotationVelocity); return ChassisSpeeds.fromFieldRelativeSpeeds(x, y, rotationVelocity, currentGyroAngle); } } From e14359c8fa448208d20552290afb1913d45c6543 Mon Sep 17 00:00:00 2001 From: audrey Date: Thu, 9 Nov 2023 19:46:23 -0800 Subject: [PATCH 32/63] extra button changes --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 359b55b..d543361 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -282,7 +282,7 @@ private void configureButtonBindings() { // outtake states jacobLayer .off(jacob.leftTrigger()) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.rightStick())); + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.leftBumper())); jacobLayer .off(jacob.rightTrigger()) @@ -307,7 +307,7 @@ private void configureButtonBindings() { elevatorSubsystem, anthony.rightBumper().getAsBoolean() ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE - : Constants.Elevator.Setpoints.GROUND_INTAKE_CONE)) + : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE)) .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightBumper())); // reset @@ -326,11 +326,11 @@ private void configureButtonBindings() { anthony .povDown() - .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.rightStick())); + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, anthony.rightBumper())); jacob .a() - .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.rightStick())); + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.leftBumper())); jacobLayer .off(jacob.povUp()) @@ -345,11 +345,11 @@ private void configureButtonBindings() { jacobLayer .off(jacob.back()) .whileTrue( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.rightStick()) + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.leftBumper()) .alongWith( new ElevatorPositionCommand( elevatorSubsystem, - jacob.rightStick().getAsBoolean() + jacob.leftBumper().getAsBoolean() ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE))) .onFalse( @@ -392,11 +392,11 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), - jacob.leftBumper())); + jacob.b())); // anthony.povRight())); jacob - .leftBumper() + .b() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); From 3c0aeff4ba607275aa1b202ee74b165da5ded90c Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 10 Nov 2023 10:43:43 -0800 Subject: [PATCH 33/63] change static ip to match team 9992 --- scripts/SetStaticEthernetIP.bat | 2 +- scripts/SetStaticIP.bat | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/SetStaticEthernetIP.bat b/scripts/SetStaticEthernetIP.bat index adc38ce..e642648 100644 --- a/scripts/SetStaticEthernetIP.bat +++ b/scripts/SetStaticEthernetIP.bat @@ -3,7 +3,7 @@ echo off set "params=%*" cd /d "%~dp0" && ( if exist "%temp%\getadmin.vbs" del "%temp%\getadmin.vbs" ) && fsutil dirty query %systemdrive% 1>nul 2>nul || ( echo Set UAC = CreateObject^("Shell.Application"^) : UAC.ShellExecute "cmd.exe", "/k cd ""%~sdp0"" && %~s0 %params%", "", "runas", 1 >> "%temp%\getadmin.vbs" && "%temp%\getadmin.vbs" && exit /B ) -netsh interface ip set address name="Ethernet 5" source=static ^ address=10.50.26.5 mask=255.255.255.0 gate=10.10.0.1 +netsh interface ip set address name="Ethernet 5" source=static ^ address=10.99.92.5 mask=255.255.255.0 gate=10.10.0.1 exit \ No newline at end of file diff --git a/scripts/SetStaticIP.bat b/scripts/SetStaticIP.bat index edfc267..26cc396 100644 --- a/scripts/SetStaticIP.bat +++ b/scripts/SetStaticIP.bat @@ -3,7 +3,7 @@ echo off set "params=%*" cd /d "%~dp0" && ( if exist "%temp%\getadmin.vbs" del "%temp%\getadmin.vbs" ) && fsutil dirty query %systemdrive% 1>nul 2>nul || ( echo Set UAC = CreateObject^("Shell.Application"^) : UAC.ShellExecute "cmd.exe", "/k cd ""%~sdp0"" && %~s0 %params%", "", "runas", 1 >> "%temp%\getadmin.vbs" && "%temp%\getadmin.vbs" && exit /B ) -netsh interface ip set address name="Wi-Fi" source=static ^ address=10.50.26.5 mask=255.255.255.0 gate=10.10.0.1 +netsh interface ip set address name="Wi-Fi" source=static ^ address=10.99.92.5 mask=255.255.255.0 gate=10.10.0.1 netsh interface ip set dnsservers "Wi-Fi" static 8.8.8.8 validate=no From 8d00413718a76e2b50843e4473c2f024ba1609f2 Mon Sep 17 00:00:00 2001 From: Derek Date: Fri, 10 Nov 2023 10:44:44 -0800 Subject: [PATCH 34/63] Error fix --- src/main/java/frc/robot/RobotContainer.java | 4 +--- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d543361..b95baae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -316,8 +316,6 @@ private void configureButtonBindings() { .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); - jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); - anthony .povLeft() .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) @@ -459,7 +457,7 @@ private void setupAutonomousCommands() { intakeLow[0] ? Elevator.Setpoints.GROUND_INTAKE_CONE : Elevator.Setpoints.SHELF_INTAKE_CONE), - "intake cone", + "intake cube", new ElevatorPositionCommand( // edited so that it works with elevator - chooses between // ground or shelf intake elevatorSubsystem, diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 6f44ab5..a5f4d29 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -40,7 +40,7 @@ public IntakeSubsystem() { filterOutput = 0.0; - statorCurrentLimit = 60; + statorCurrentLimit = 75; isCube = false; From 84a404cac885ff678aa5c9b326d34ad613fabeae Mon Sep 17 00:00:00 2001 From: audrey Date: Fri, 10 Nov 2023 10:51:14 -0800 Subject: [PATCH 35/63] make score commands use isCube --- src/main/java/frc/robot/Constants.java | 16 ++++++++++------ src/main/java/frc/robot/RobotContainer.java | 9 ++++----- .../java/frc/robot/commands/ScoreCommand.java | 19 +++++++++++-------- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- 4 files changed, 26 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d03da23..dbef31f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -271,27 +271,31 @@ public static final class Ports { NodeType.CONE.atHeight(Height.HIGH), List.of( new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep( + Elevator.Setpoints.SCORE_HIGH_CONE, IntakeSubsystem.Modes.OUTTAKE, false)), NodeType.CONE.atHeight(Height.MID), List.of( new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep( + Elevator.Setpoints.SCORE_MID_CONE, IntakeSubsystem.Modes.OUTTAKE, false)), NodeType.CONE.atHeight(Height.LOW), List.of( new ScoreStep(Elevator.Setpoints.SCORE_LOW_CONE).canWaitHere(), - new ScoreStep(IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(IntakeSubsystem.Modes.OUTTAKE, false)), NodeType.CUBE.atHeight(Height.HIGH), List.of( new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CUBE).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CUBE, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep( + Elevator.Setpoints.SCORE_HIGH_CUBE, IntakeSubsystem.Modes.OUTTAKE, true)), NodeType.CUBE.atHeight(Height.MID), List.of( new ScoreStep(Elevator.Setpoints.SCORE_MID_CUBE).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_MID_CUBE, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep( + Elevator.Setpoints.SCORE_MID_CUBE, IntakeSubsystem.Modes.OUTTAKE, true)), NodeType.CUBE.atHeight(Height.LOW), List.of( new ScoreStep(Elevator.Setpoints.SCORE_LOW_CUBE).canWaitHere(), - new ScoreStep(IntakeSubsystem.Modes.OUTTAKE))); + new ScoreStep(IntakeSubsystem.Modes.OUTTAKE, true))); public static final class Vision { public static record VisionSource(String name, Transform3d robotToCamera) {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b95baae..5217f9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -390,11 +390,10 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), - jacob.b())); - // anthony.povRight())); + anthony.povRight())); - jacob - .b() + anthony + .povRight() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); @@ -441,7 +440,7 @@ private void setupAutonomousCommands() { List.of( new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_EXTENSION_INCHES)) .canWaitHere(), - new ScoreStep(Modes.OUTTAKE)); + new ScoreStep(Modes.OUTTAKE, true)); final boolean[] intakeLow = {false}; // FIXME go through each auto and make sure that we dont use a leftover event marker from Simba final Map eventMap = diff --git a/src/main/java/frc/robot/commands/ScoreCommand.java b/src/main/java/frc/robot/commands/ScoreCommand.java index e51a883..8745864 100644 --- a/src/main/java/frc/robot/commands/ScoreCommand.java +++ b/src/main/java/frc/robot/commands/ScoreCommand.java @@ -22,21 +22,23 @@ public class ScoreCommand extends SequentialCommandGroup { public static record ScoreStep( Optional elevatorState, Optional intakeState, + boolean isCube, boolean isPausePoint) { - public ScoreStep(ElevatorState elevatorState, IntakeSubsystem.Modes intakeState) { - this(Optional.of(elevatorState), Optional.of(intakeState), false); + public ScoreStep( + ElevatorState elevatorState, IntakeSubsystem.Modes intakeState, boolean isCube) { + this(Optional.of(elevatorState), Optional.of(intakeState), isCube, false); } public ScoreStep(ElevatorState elevatorState) { - this(Optional.of(elevatorState), Optional.empty(), false); + this(Optional.of(elevatorState), Optional.empty(), false, false); } - public ScoreStep(IntakeSubsystem.Modes intakeState) { - this(Optional.empty(), Optional.of(intakeState), false); + public ScoreStep(IntakeSubsystem.Modes intakeState, boolean isCube) { + this(Optional.empty(), Optional.of(intakeState), isCube, false); } public ScoreStep canWaitHere() { - return new ScoreStep(elevatorState, intakeState, true); + return new ScoreStep(elevatorState, intakeState, false, true); } } @@ -69,13 +71,14 @@ public boolean isFinished() { private Command createStep(ScoreStep scoreStep) { var elevatorState = scoreStep.elevatorState(); var intakeState = scoreStep.intakeState(); + var isCube = scoreStep.isCube(); if (elevatorState.isPresent() && intakeState.isPresent()) { return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, intakeState.get())); + .deadlineWith(new IntakeModeCommand(intakeSubsystem, intakeState.get(), () -> isCube)); } else if (elevatorState.isPresent()) { return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()); } else if (intakeState.isPresent()) { - return new IntakeModeCommand(intakeSubsystem, intakeState.get()); + return new IntakeModeCommand(intakeSubsystem, intakeState.get(), () -> isCube); } else { throw new IllegalArgumentException("ScoreStep must have at least one state"); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a5f4d29..0f7d569 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -123,7 +123,7 @@ private double getFilterCalculatedValue() { public void periodic() { filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); if (filterOutput >= statorCurrentLimit) { - currentIntakeMode = Modes.OFF; + currentIntakeMode = Modes.HOLD; } intakePeriodic(currentIntakeMode); From 08a531049080d6634bb596543a0b521efed86def Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 10 Nov 2023 11:15:21 -0800 Subject: [PATCH 36/63] test --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 +++++--- src/main/java/frc/robot/commands/GroundPickupCommand.java | 2 ++ 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dbef31f..d3473ff 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -207,7 +207,7 @@ public static final class Setpoints { public static final ElevatorState GROUND_INTAKE_CONE = new ElevatorState(MIN_EXTENSION_INCHES, 75); public static final ElevatorState GROUND_INTAKE_CUBE = - new ElevatorState(MIN_EXTENSION_INCHES, 85); + new ElevatorState(MIN_EXTENSION_INCHES, 90); public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(MAX_EXTENSION_INCHES, 20); public static final ElevatorState SCORE_HIGH_CUBE = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5217f9e..fc81f49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -390,10 +390,12 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), - anthony.povRight())); + jacob.b())); + // anthony.povRight())); - anthony - .povRight() + // anthony + // .povRight() + jacob.b() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 0315355..6b6d236 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -9,8 +9,10 @@ import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.IntakeSubsystem.Modes; import java.util.function.BooleanSupplier; +import java.util.function.Supplier; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: From adb71f7f946f381940ea40a2e1b41c6365223173 Mon Sep 17 00:00:00 2001 From: audrey Date: Fri, 10 Nov 2023 11:25:25 -0800 Subject: [PATCH 37/63] add camera measurements --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dbef31f..1044362 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -306,9 +306,9 @@ public static record VisionSource(String name, Transform3d robotToCamera) {} "frontCam", new Transform3d( new Translation3d( - 0.228110, // front/back - 0.253802, // left/right - 0.443955 // up/down + 0.23749, // front/back + 0.2403348, // left/right + 0.7973822 // up/down ), new Rotation3d( 0, From 6d30e4b278dbb30bf6fbedb6afbf5021588d4379 Mon Sep 17 00:00:00 2001 From: audrey Date: Fri, 10 Nov 2023 11:48:50 -0800 Subject: [PATCH 38/63] second camera measurements --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a6eb4f1..3b0975f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -318,9 +318,9 @@ public static record VisionSource(String name, Transform3d robotToCamera) {} "backCam", new Transform3d( new Translation3d( - 0.102078, // front/back - -0.253802, // left/right - 1.222387 // up/down + 0, // front/back + -0.212725, // left/right + 0.6470142 // up/down ), new Rotation3d(0, Math.toRadians(17), Math.PI)))); From c57326fa3cf96d28e7a78d0c62a7b0e6dfd88236 Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 10 Nov 2023 12:13:04 -0800 Subject: [PATCH 39/63] calibrate --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d03da23..edeed84 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -152,7 +152,7 @@ public static final class Module1 { // historically front right public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(8.07400 + 180) // comp bot offset + ? -Math.toRadians(343.432) // comp bot offset : -Math.toRadians(184.833984); // practice bot offset } @@ -163,7 +163,7 @@ public static final class Module2 { // historically front left public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(274.562 + 180) // comp bot offset + ? -Math.toRadians(221.388) // comp bot offset : -Math.toRadians(307.968750); // practice bot offset } @@ -174,7 +174,7 @@ public static final class Module3 { // historically back left public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(225.082 + 180) // comp bot offset + ? -Math.toRadians(134.115) // comp bot offset : -Math.toRadians(306.738281 + 180); // practice bot offset } @@ -185,7 +185,7 @@ public static final class Module4 { // historically back right public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(335.124 + 180) // comp bot offset + ? -Math.toRadians(20.4750) // comp bot offset : -Math.toRadians(60.908203); // practice bot offset } } From bd079c162393d7f8df43a5a25e622622f458f766 Mon Sep 17 00:00:00 2001 From: Derek Date: Fri, 10 Nov 2023 12:34:31 -0800 Subject: [PATCH 40/63] Calibrated setpoints --- src/main/java/frc/robot/Constants.java | 18 +++++++---------- src/main/java/frc/robot/RobotContainer.java | 20 +++++++++++++++++-- .../robot/commands/GroundPickupCommand.java | 7 ++++++- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- 4 files changed, 32 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index edeed84..56b17ca 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -207,19 +207,15 @@ public static final class Setpoints { public static final ElevatorState GROUND_INTAKE_CONE = new ElevatorState(MIN_EXTENSION_INCHES, 75); public static final ElevatorState GROUND_INTAKE_CUBE = - new ElevatorState(MIN_EXTENSION_INCHES, 85); - public static final ElevatorState SCORE_HIGH_CONE = - new ElevatorState(MAX_EXTENSION_INCHES, 20); - public static final ElevatorState SCORE_HIGH_CUBE = - new ElevatorState(MAX_EXTENSION_INCHES, 20); - public static final ElevatorState SCORE_MID_CONE = - new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); - public static final ElevatorState SCORE_MID_CUBE = - new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); + new ElevatorState(MIN_EXTENSION_INCHES, 90); + public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5); + public static final ElevatorState SCORE_HIGH_CUBE = new ElevatorState(53.8, 74.5); + public static final ElevatorState SCORE_MID_CONE = new ElevatorState(22.5, 35.5); + public static final ElevatorState SCORE_MID_CUBE = new ElevatorState(30, 74.5); public static final ElevatorState SCORE_LOW_CONE = - new ElevatorState(MIN_EXTENSION_INCHES, 60); + new ElevatorState(MIN_EXTENSION_INCHES, 55); public static final ElevatorState SCORE_LOW_CUBE = - new ElevatorState(MIN_EXTENSION_INCHES, 60); + new ElevatorState(MIN_EXTENSION_INCHES, 48); } public static final double MAX_EXTENSION_INCHES = 54; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b95baae..0f299e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -324,11 +324,27 @@ private void configureButtonBindings() { anthony .povDown() - .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, anthony.rightBumper())); + .onTrue( + new GroundPickupCommand( + intakeSubsystem, + elevatorSubsystem, + () -> + anthony.rightBumper().getAsBoolean() + ? Elevator.Setpoints.GROUND_INTAKE_CUBE + : Elevator.Setpoints.GROUND_INTAKE_CONE, + anthony.rightBumper())); jacob .a() - .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.leftBumper())); + .onTrue( + new GroundPickupCommand( + intakeSubsystem, + elevatorSubsystem, + () -> + jacob.leftBumper().getAsBoolean() + ? Elevator.Setpoints.GROUND_INTAKE_CUBE + : Elevator.Setpoints.GROUND_INTAKE_CONE, + jacob.leftBumper())); jacobLayer .off(jacob.povUp()) diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 0315355..c9bfd59 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; import java.util.function.BooleanSupplier; @@ -21,16 +22,20 @@ public class GroundPickupCommand extends SequentialCommandGroup { private ElevatorSubsystem elevatorSubsystem; + private Supplier setpoint; + public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, + Supplier setpoint, BooleanSupplier isCube) { // Add your commands in the addCommands() call, e.g. this.isCube = isCube; this.elevatorSubsystem = elevatorSubsystem; + this.setpoint = setpoint; addCommands( - determineIntakeType() + new ElevatorPositionCommand(elevatorSubsystem, setpoint.get()) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube)) .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a5f4d29..5911c64 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -52,7 +52,7 @@ public IntakeSubsystem() { shuffleboard.addString("Current mode", () -> currentIntakeMode.toString()); shuffleboard.addDouble("filter output", () -> filterOutput); shuffleboard.addDouble("motor output", intakeMotor::getMotorOutputPercent); - shuffleboard.addBoolean("is cone", () -> isCube); + shuffleboard.addBoolean("is cube intake", () -> isCube); // shuffleboard.addDouble("coneToFInches", this::getConeToFInches); // shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches); } From ad8b70f9f797e09aff04ddb021fcb49933a207b2 Mon Sep 17 00:00:00 2001 From: Derek Date: Fri, 10 Nov 2023 12:48:25 -0800 Subject: [PATCH 41/63] Intake setpoint calibration --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/commands/GroundPickupCommand.java | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 56b17ca..30fc246 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -202,12 +202,12 @@ public static final class Ports { public static final class Setpoints { public static final ElevatorState STOWED = new ElevatorState(MIN_EXTENSION_INCHES, MIN_ANGLE_DEGREES); - public static final ElevatorState SHELF_INTAKE_CONE = new ElevatorState(20, 20); - public static final ElevatorState SHELF_INTAKE_CUBE = new ElevatorState(20, 20); + public static final ElevatorState SHELF_INTAKE_CONE = new ElevatorState(47.3, 68.5); + public static final ElevatorState SHELF_INTAKE_CUBE = new ElevatorState(43.2, 68.5); public static final ElevatorState GROUND_INTAKE_CONE = new ElevatorState(MIN_EXTENSION_INCHES, 75); public static final ElevatorState GROUND_INTAKE_CUBE = - new ElevatorState(MIN_EXTENSION_INCHES, 90); + new ElevatorState(MIN_EXTENSION_INCHES, 81.4); public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5); public static final ElevatorState SCORE_HIGH_CUBE = new ElevatorState(53.8, 74.5); public static final ElevatorState SCORE_MID_CONE = new ElevatorState(22.5, 35.5); @@ -267,7 +267,7 @@ public static final class Ports { NodeType.CONE.atHeight(Height.HIGH), List.of( new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(new ElevatorState(22.5, 0), IntakeSubsystem.Modes.OUTTAKE)), NodeType.CONE.atHeight(Height.MID), List.of( new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE).canWaitHere(), diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index c9bfd59..60df2d5 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -12,6 +12,7 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; import java.util.function.BooleanSupplier; +import java.util.function.Supplier; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: From 6e30f1f5b72c16378ada6d9ccdf5b5b8d901a77b Mon Sep 17 00:00:00 2001 From: Derek Date: Fri, 10 Nov 2023 12:53:15 -0800 Subject: [PATCH 42/63] trying to make ground pickup work? --- src/main/java/frc/robot/Constants.java | 16 +++++------- src/main/java/frc/robot/RobotContainer.java | 25 ++++++++++++++++--- .../robot/commands/GroundPickupCommand.java | 8 ++++-- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- 4 files changed, 34 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b0975f..5d1de8d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -208,18 +208,14 @@ public static final class Setpoints { new ElevatorState(MIN_EXTENSION_INCHES, 75); public static final ElevatorState GROUND_INTAKE_CUBE = new ElevatorState(MIN_EXTENSION_INCHES, 90); - public static final ElevatorState SCORE_HIGH_CONE = - new ElevatorState(MAX_EXTENSION_INCHES, 20); - public static final ElevatorState SCORE_HIGH_CUBE = - new ElevatorState(MAX_EXTENSION_INCHES, 20); - public static final ElevatorState SCORE_MID_CONE = - new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); - public static final ElevatorState SCORE_MID_CUBE = - new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); + public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5); + public static final ElevatorState SCORE_HIGH_CUBE = new ElevatorState(53.8, 74.5); + public static final ElevatorState SCORE_MID_CONE = new ElevatorState(22.5, 35.5); + public static final ElevatorState SCORE_MID_CUBE = new ElevatorState(30, 74.5); public static final ElevatorState SCORE_LOW_CONE = - new ElevatorState(MIN_EXTENSION_INCHES, 60); + new ElevatorState(MIN_EXTENSION_INCHES, 55); public static final ElevatorState SCORE_LOW_CUBE = - new ElevatorState(MIN_EXTENSION_INCHES, 60); + new ElevatorState(MIN_EXTENSION_INCHES, 48); } public static final double MAX_EXTENSION_INCHES = 54; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc81f49..1a150c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -324,11 +324,27 @@ private void configureButtonBindings() { anthony .povDown() - .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, anthony.rightBumper())); + .onTrue( + new GroundPickupCommand( + intakeSubsystem, + elevatorSubsystem, + () -> + anthony.rightBumper().getAsBoolean() + ? Elevator.Setpoints.GROUND_INTAKE_CUBE + : Elevator.Setpoints.GROUND_INTAKE_CONE, + anthony.rightBumper())); jacob .a() - .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.leftBumper())); + .onTrue( + new GroundPickupCommand( + intakeSubsystem, + elevatorSubsystem, + () -> + jacob.leftBumper().getAsBoolean() + ? Elevator.Setpoints.GROUND_INTAKE_CUBE + : Elevator.Setpoints.GROUND_INTAKE_CONE, + jacob.leftBumper())); jacobLayer .off(jacob.povUp()) @@ -391,11 +407,12 @@ private void configureButtonBindings() { elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), jacob.b())); - // anthony.povRight())); + // anthony.povRight())); // anthony // .povRight() - jacob.b() + jacob + .b() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 6b6d236..60df2d5 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; -import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -23,16 +23,20 @@ public class GroundPickupCommand extends SequentialCommandGroup { private ElevatorSubsystem elevatorSubsystem; + private Supplier setpoint; + public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, + Supplier setpoint, BooleanSupplier isCube) { // Add your commands in the addCommands() call, e.g. this.isCube = isCube; this.elevatorSubsystem = elevatorSubsystem; + this.setpoint = setpoint; addCommands( - determineIntakeType() + new ElevatorPositionCommand(elevatorSubsystem, setpoint.get()) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube)) .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 0f7d569..ad7e19b 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -52,7 +52,7 @@ public IntakeSubsystem() { shuffleboard.addString("Current mode", () -> currentIntakeMode.toString()); shuffleboard.addDouble("filter output", () -> filterOutput); shuffleboard.addDouble("motor output", intakeMotor::getMotorOutputPercent); - shuffleboard.addBoolean("is cone", () -> isCube); + shuffleboard.addBoolean("is cube intake", () -> isCube); // shuffleboard.addDouble("coneToFInches", this::getConeToFInches); // shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches); } From 67b725e7629d1af384c78508d098501bbe8fad6a Mon Sep 17 00:00:00 2001 From: Derek Date: Fri, 10 Nov 2023 13:56:23 -0800 Subject: [PATCH 43/63] os work kewl --- src/main/java/frc/robot/RobotContainer.java | 54 +++++++++++-------- .../commands/N3_1ConePlusMobility.java | 2 +- .../commands/N3_1ConePlusMobilityEngage.java | 3 +- .../commands/N6_1ConePlusEngage.java | 3 +- .../commands/N9_1ConePlusMobility.java | 2 +- .../commands/N9_1ConePlusMobilityEngage.java | 3 +- .../commands/ElevatorPositionCommand.java | 46 ++++++++++------ .../robot/commands/GroundPickupCommand.java | 22 ++++---- .../java/frc/robot/commands/ScoreCommand.java | 4 +- 9 files changed, 82 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a150c5..d2c8d94 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -305,20 +305,25 @@ private void configureButtonBindings() { .onTrue( new ElevatorPositionCommand( elevatorSubsystem, - anthony.rightBumper().getAsBoolean() - ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE - : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE)) + () -> + anthony.rightBumper().getAsBoolean() + ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE + : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE)) .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightBumper())); // reset jacobLayer .off(jacob.y()) - .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) + .onTrue( + new ElevatorPositionCommand( + elevatorSubsystem, () -> Constants.Elevator.Setpoints.STOWED)) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)); anthony .povLeft() - .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)) + .onTrue( + new ElevatorPositionCommand( + elevatorSubsystem, () -> Constants.Elevator.Setpoints.STOWED)) .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) .onTrue(new SetZeroModeCommand(elevatorSubsystem)); @@ -329,7 +334,7 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, () -> - anthony.rightBumper().getAsBoolean() + anthony.getHID().getRightBumper() ? Elevator.Setpoints.GROUND_INTAKE_CUBE : Elevator.Setpoints.GROUND_INTAKE_CONE, anthony.rightBumper())); @@ -341,18 +346,17 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, () -> - jacob.leftBumper().getAsBoolean() + jacob.getHID().getLeftBumper() ? Elevator.Setpoints.GROUND_INTAKE_CUBE : Elevator.Setpoints.GROUND_INTAKE_CONE, jacob.leftBumper())); - jacobLayer .off(jacob.povUp()) .onTrue( new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) .alongWith( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.GROUND_INTAKE_CONE))); + elevatorSubsystem, () -> Constants.Elevator.Setpoints.GROUND_INTAKE_CONE))); // jacob.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem)); @@ -363,11 +367,13 @@ private void configureButtonBindings() { .alongWith( new ElevatorPositionCommand( elevatorSubsystem, - jacob.leftBumper().getAsBoolean() - ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE - : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE))) + () -> + jacob.leftBumper().getAsBoolean() + ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE + : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE))) .onFalse( - new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) + new ElevatorPositionCommand( + elevatorSubsystem, () -> Constants.Elevator.Setpoints.STOWED) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); // scoring @@ -465,23 +471,25 @@ private void setupAutonomousCommands() { final Map eventMap = Map.of( "stow elevator", - new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED), + new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED), "zero everything", new SetZeroModeCommand(elevatorSubsystem), "intake cone", new ElevatorPositionCommand( // edited so that it works with elevator - chooses between // ground or shelf intake elevatorSubsystem, - intakeLow[0] - ? Elevator.Setpoints.GROUND_INTAKE_CONE - : Elevator.Setpoints.SHELF_INTAKE_CONE), + () -> + intakeLow[0] + ? Elevator.Setpoints.GROUND_INTAKE_CONE + : Elevator.Setpoints.SHELF_INTAKE_CONE), "intake cube", new ElevatorPositionCommand( // edited so that it works with elevator - chooses between // ground or shelf intake elevatorSubsystem, - intakeLow[0] - ? Elevator.Setpoints.GROUND_INTAKE_CUBE - : Elevator.Setpoints.SHELF_INTAKE_CUBE), + () -> + intakeLow[0] + ? Elevator.Setpoints.GROUND_INTAKE_CUBE + : Elevator.Setpoints.SHELF_INTAKE_CUBE), // squeeze intake isn't relevant to offseason bot - leaving here just in case /*"squeeze intake", new CommandBase() { @@ -523,13 +531,13 @@ public void end(boolean interrupted) { intakeSubsystem, elevatorSubsystem, drivingCubeOuttake.subList(1, 2), 1) .andThen( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) + elevatorSubsystem, () -> Constants.Elevator.Setpoints.STOWED) .andThen(new IntakeModeCommand(intakeSubsystem, Modes.OFF))), "armbat preload", - new ElevatorPositionCommand(elevatorSubsystem, 30, 0) + new ElevatorPositionCommand(elevatorSubsystem, () -> new ElevatorState(22.5, 0)) .andThen( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.STOWED))); + elevatorSubsystem, () -> Constants.Elevator.Setpoints.STOWED))); // autoSelector.setDefaultOption( // "N1 1Cone + 2Cube Low Mobility Engage", diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java index 7ad7100..45380d0 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java @@ -48,6 +48,6 @@ public N3_1ConePlusMobility( (new WaitCommand(1)) .andThen( new ElevatorPositionCommand( - elevatorSubsystem, Elevator.Setpoints.STOWED)))); + elevatorSubsystem, () -> Elevator.Setpoints.STOWED)))); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java index 8bbe390..9c4da8a 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java @@ -51,7 +51,8 @@ public N3_1ConePlusMobilityEngage( .alongWith( (new WaitCommand(1)) .andThen( - new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))), + new ElevatorPositionCommand( + elevatorSubsystem, () -> Elevator.Setpoints.STOWED))), new EngageCommand(drivebaseSubsystem, EngageCommand.EngageDirection.GO_FORWARD)); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java b/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java index edf4932..930be55 100644 --- a/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java @@ -49,7 +49,8 @@ public N6_1ConePlusEngage( .alongWith( (new WaitCommand(1)) .andThen( - new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))), + new ElevatorPositionCommand( + elevatorSubsystem, () -> Elevator.Setpoints.STOWED))), new EngageCommand(drivebaseSubsystem, EngageCommand.EngageDirection.GO_BACKWARD)); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java index c08f5dd..4f91b71 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java @@ -49,6 +49,6 @@ public N9_1ConePlusMobility( (new WaitCommand(1)) .andThen( new ElevatorPositionCommand( - elevatorSubsystem, Elevator.Setpoints.STOWED)))); + elevatorSubsystem, () -> Elevator.Setpoints.STOWED)))); } } diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java index 91b4f3b..a60f2dd 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java @@ -51,7 +51,8 @@ public N9_1ConePlusMobilityEngage( .alongWith( (new WaitCommand(1)) .andThen( - new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))), + new ElevatorPositionCommand( + elevatorSubsystem, () -> Elevator.Setpoints.STOWED))), new EngageCommand(drivebaseSubsystem, EngageCommand.EngageDirection.GO_FORWARD)); } } diff --git a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java index 87d3106..9a48693 100644 --- a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java @@ -10,42 +10,54 @@ import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.ElevatorSubsystem.Modes; import frc.util.Util; +import java.util.function.Supplier; public class ElevatorPositionCommand extends CommandBase { private final ElevatorSubsystem elevatorSubsystem; private double targetExtension; private double targetAngle; + private Supplier elevatorStateSupplier; /** Creates a new ElevatorPositionCommand. */ - public ElevatorPositionCommand( - ElevatorSubsystem subsystem, double targetExtension, double targetAngle) { - this.elevatorSubsystem = subsystem; - this.targetExtension = targetExtension; - this.targetAngle = targetAngle; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(elevatorSubsystem); - } + // public ElevatorPositionCommand( + // ElevatorSubsystem subsystem, double targetExtension, double targetAngle) { + // this.elevatorSubsystem = subsystem; + // this.targetExtension = targetExtension; + // this.targetAngle = targetAngle; + // // Use addRequirements() here to declare subsystem dependencies. + // addRequirements(elevatorSubsystem); + // } + + // public ElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, ElevatorState + // elevatorState) { + // // height and angle + // this.elevatorSubsystem = elevatorSubsystem; + // targetExtension = elevatorState.extension(); + // targetAngle = elevatorState.angle(); + // // Use addRequirements() here to declare subsystem dependencies. + // addRequirements(elevatorSubsystem); + // } - public ElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, ElevatorState elevatorState) { + public ElevatorPositionCommand( + ElevatorSubsystem elevatorSubsystem, Supplier elevatorStateSupplier) { // height and angle this.elevatorSubsystem = elevatorSubsystem; - targetExtension = elevatorState.extension(); - targetAngle = elevatorState.angle(); + this.elevatorStateSupplier = elevatorStateSupplier; // Use addRequirements() here to declare subsystem dependencies. addRequirements(elevatorSubsystem); } // Called when the command is initially scheduled. @Override - public void initialize() { - elevatorSubsystem.setTargetExtensionInches(targetExtension); - elevatorSubsystem.setTargetAngle(targetAngle); - elevatorSubsystem.setMode(Modes.POSITION_CONTROL); - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + elevatorSubsystem.setTargetExtensionInches(elevatorStateSupplier.get().extension()); + elevatorSubsystem.setTargetAngle(elevatorStateSupplier.get().angle()); + elevatorSubsystem.setMode(Modes.POSITION_CONTROL); + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 60df2d5..6d4e4fc 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -4,7 +4,6 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; @@ -36,16 +35,19 @@ public GroundPickupCommand( this.setpoint = setpoint; addCommands( - new ElevatorPositionCommand(elevatorSubsystem, setpoint.get()) + new ElevatorPositionCommand(elevatorSubsystem, setpoint) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube)) - .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))); + .andThen( + new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED))); } - private Command determineIntakeType() { - if (isCube.getAsBoolean()) { - return new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE_CUBE); - } else { - return new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE_CONE); - } - } + // private Command determineIntakeType() { + // if (isCube.getAsBoolean()) { + // return new ElevatorPositionCommand(elevatorSubsystem, + // Elevator.Setpoints.GROUND_INTAKE_CUBE); + // } else { + // return new ElevatorPositionCommand(elevatorSubsystem, + // Elevator.Setpoints.GROUND_INTAKE_CONE); + // } + // } } diff --git a/src/main/java/frc/robot/commands/ScoreCommand.java b/src/main/java/frc/robot/commands/ScoreCommand.java index 8745864..4683f8f 100644 --- a/src/main/java/frc/robot/commands/ScoreCommand.java +++ b/src/main/java/frc/robot/commands/ScoreCommand.java @@ -73,10 +73,10 @@ private Command createStep(ScoreStep scoreStep) { var intakeState = scoreStep.intakeState(); var isCube = scoreStep.isCube(); if (elevatorState.isPresent() && intakeState.isPresent()) { - return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()) + return new ElevatorPositionCommand(elevatorSubsystem, () -> elevatorState.get()) .deadlineWith(new IntakeModeCommand(intakeSubsystem, intakeState.get(), () -> isCube)); } else if (elevatorState.isPresent()) { - return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()); + return new ElevatorPositionCommand(elevatorSubsystem, () -> elevatorState.get()); } else if (intakeState.isPresent()) { return new IntakeModeCommand(intakeSubsystem, intakeState.get(), () -> isCube); } else { From f00e61a84ba532733215d786653ebb8b893d5550 Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 10 Nov 2023 20:13:05 -0800 Subject: [PATCH 44/63] fix ground intake selection --- src/main/java/frc/robot/RobotContainer.java | 20 ++++--------- .../commands/GroundIntakeElevatorCommand.java | 29 +++++++++++++++++++ .../robot/commands/GroundPickupCommand.java | 20 +------------ 3 files changed, 36 insertions(+), 33 deletions(-) create mode 100644 src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a150c5..2cad0d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -328,10 +328,6 @@ private void configureButtonBindings() { new GroundPickupCommand( intakeSubsystem, elevatorSubsystem, - () -> - anthony.rightBumper().getAsBoolean() - ? Elevator.Setpoints.GROUND_INTAKE_CUBE - : Elevator.Setpoints.GROUND_INTAKE_CONE, anthony.rightBumper())); jacob @@ -340,10 +336,6 @@ private void configureButtonBindings() { new GroundPickupCommand( intakeSubsystem, elevatorSubsystem, - () -> - jacob.leftBumper().getAsBoolean() - ? Elevator.Setpoints.GROUND_INTAKE_CUBE - : Elevator.Setpoints.GROUND_INTAKE_CONE, jacob.leftBumper())); jacobLayer @@ -406,13 +398,13 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), - jacob.b())); - // anthony.povRight())); + // jacob.b())); + anthony.povRight())); - // anthony - // .povRight() - jacob - .b() + anthony + .povRight() + // jacob + // .b() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java new file mode 100644 index 0000000..f281bd4 --- /dev/null +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -0,0 +1,29 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.ElevatorSubsystem; + +public class GroundIntakeElevatorCommand extends CommandBase { + + private ElevatorSubsystem elevatorSubsystem; + private BooleanSupplier isCube; + + public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanSupplier isCube) { + this.elevatorSubsystem = elevatorSubsystem; + this.isCube = isCube; + + addRequirements(this.elevatorSubsystem); + } + + @Override + public void execute() { + if(isCube.getAsBoolean()) { + elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CUBE); + } else { + elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); + } + } +} diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 60df2d5..42f3ef7 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -19,33 +19,15 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class GroundPickupCommand extends SequentialCommandGroup { /** Creates a new GroundPickupCommand. */ - private BooleanSupplier isCube; - - private ElevatorSubsystem elevatorSubsystem; - - private Supplier setpoint; - public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, - Supplier setpoint, BooleanSupplier isCube) { // Add your commands in the addCommands() call, e.g. - this.isCube = isCube; - this.elevatorSubsystem = elevatorSubsystem; - this.setpoint = setpoint; addCommands( - new ElevatorPositionCommand(elevatorSubsystem, setpoint.get()) + new GroundIntakeElevatorCommand(elevatorSubsystem, isCube) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube)) .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))); } - - private Command determineIntakeType() { - if (isCube.getAsBoolean()) { - return new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE_CUBE); - } else { - return new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE_CONE); - } - } } From 8123b57a5f6b2e5b86b0c80cf22eceab20526a0b Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 10 Nov 2023 20:36:37 -0800 Subject: [PATCH 45/63] fix constructor error --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5d6ef6f..f8d0617 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -267,7 +267,7 @@ public static final class Ports { NodeType.CONE.atHeight(Height.HIGH), List.of( new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE).canWaitHere(), - new ScoreStep(new ElevatorState(22.5, 0), IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(new ElevatorState(22.5, 0), IntakeSubsystem.Modes.OUTTAKE, false)), NodeType.CONE.atHeight(Height.MID), List.of( new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE).canWaitHere(), From 2316772828db0cc6fa2221c7c4bd4fcd189817de Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 10 Nov 2023 20:37:31 -0800 Subject: [PATCH 46/63] spotless --- src/main/java/frc/robot/RobotContainer.java | 20 ++++-------- .../commands/GroundIntakeElevatorCommand.java | 31 +++++++++---------- .../robot/commands/GroundPickupCommand.java | 3 -- 3 files changed, 21 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2cad0d8..b5213a9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -324,19 +324,11 @@ private void configureButtonBindings() { anthony .povDown() - .onTrue( - new GroundPickupCommand( - intakeSubsystem, - elevatorSubsystem, - anthony.rightBumper())); + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, anthony.rightBumper())); jacob .a() - .onTrue( - new GroundPickupCommand( - intakeSubsystem, - elevatorSubsystem, - jacob.leftBumper())); + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.leftBumper())); jacobLayer .off(jacob.povUp()) @@ -398,13 +390,13 @@ private void configureButtonBindings() { intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(scoreType), - // jacob.b())); - anthony.povRight())); + // jacob.b())); + anthony.povRight())); anthony .povRight() - // jacob - // .b() + // jacob + // .b() .onTrue( new HashMapCommand<>( scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java index f281bd4..f84e59f 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -1,29 +1,28 @@ package frc.robot.commands; -import java.util.function.BooleanSupplier; - import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; import frc.robot.subsystems.ElevatorSubsystem; +import java.util.function.BooleanSupplier; public class GroundIntakeElevatorCommand extends CommandBase { - private ElevatorSubsystem elevatorSubsystem; - private BooleanSupplier isCube; + private ElevatorSubsystem elevatorSubsystem; + private BooleanSupplier isCube; - public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanSupplier isCube) { - this.elevatorSubsystem = elevatorSubsystem; - this.isCube = isCube; + public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanSupplier isCube) { + this.elevatorSubsystem = elevatorSubsystem; + this.isCube = isCube; - addRequirements(this.elevatorSubsystem); - } + addRequirements(this.elevatorSubsystem); + } - @Override - public void execute() { - if(isCube.getAsBoolean()) { - elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CUBE); - } else { - elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); - } + @Override + public void execute() { + if (isCube.getAsBoolean()) { + elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CUBE); + } else { + elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); } + } } diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 42f3ef7..873782b 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -4,15 +4,12 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; -import frc.robot.subsystems.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; import java.util.function.BooleanSupplier; -import java.util.function.Supplier; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: From 527f8397a2d2db3d409731f827971a970d8b08b0 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 08:20:42 -0800 Subject: [PATCH 47/63] calibrate swerve --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f8d0617..3f7792b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -152,7 +152,7 @@ public static final class Module1 { // historically front right public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(343.432) // comp bot offset + ? -Math.toRadians(162.685546875 + 180) // comp bot offset : -Math.toRadians(184.833984); // practice bot offset } @@ -163,7 +163,7 @@ public static final class Module2 { // historically front left public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(221.388) // comp bot offset + ? -Math.toRadians(220.517578125) // comp bot offset : -Math.toRadians(307.968750); // practice bot offset } @@ -174,7 +174,7 @@ public static final class Module3 { // historically back left public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(134.115) // comp bot offset + ? -Math.toRadians(134.82421875) // comp bot offset : -Math.toRadians(306.738281 + 180); // practice bot offset } @@ -185,7 +185,7 @@ public static final class Module4 { // historically back right public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(20.4750) // comp bot offset + ? -Math.toRadians(201.09375 + 180) // comp bot offset : -Math.toRadians(60.908203); // practice bot offset } } From 38d200a2adf217ac2590f4a03e733629ce521222 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 08:58:02 -0800 Subject: [PATCH 48/63] fixing thigns? --- src/main/java/frc/robot/RobotContainer.java | 148 +++++++++++--------- 1 file changed, 80 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e482dd9..9de9900 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import com.pathplanner.lib.server.PathPlannerServer; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -23,6 +24,16 @@ import frc.robot.Constants.Config; import frc.robot.Constants.Drive; import frc.robot.Constants.Elevator; +import frc.robot.autonomous.commands.MobilityAuto; +import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobility; +import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobilityEngage; +import frc.robot.autonomous.commands.N2_Engage; +import frc.robot.autonomous.commands.N3_1ConePlusMobility; +import frc.robot.autonomous.commands.N3_1ConePlusMobilityEngage; +import frc.robot.autonomous.commands.N6_1ConePlusEngage; +import frc.robot.autonomous.commands.N9_1ConePlus2CubeMobility; +import frc.robot.autonomous.commands.N9_1ConePlusMobility; +import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.DriveToPlaceCommand; @@ -34,6 +45,7 @@ import frc.robot.commands.HashMapCommand; import frc.robot.commands.IntakeModeCommand; import frc.robot.commands.RotateVectorDriveCommand; +import frc.robot.commands.RotateVelocityDriveCommand; import frc.robot.commands.ScoreCommand; import frc.robot.commands.ScoreCommand.ScoreStep; import frc.robot.commands.SetZeroModeCommand; @@ -56,6 +68,7 @@ import frc.util.NodeSelectorUtility.NodeType; import frc.util.SharedReference; import frc.util.Util; +import frc.util.pathing.AlliancePose2d; import frc.util.pathing.RubenManueverGenerator; import java.util.HashMap; import java.util.List; @@ -225,14 +238,14 @@ private void configureButtonBindings() { /** percent of fraction power */ (anthony.getHID().getAButton() ? .3 : .8); - // new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0) - // .whileTrue( - // new RotateVelocityDriveCommand( - // drivebaseSubsystem, - // translationXSupplier, - // translationYSupplier, - // rotationVelocity, - // anthony.rightBumper())); + new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0) + .whileTrue( + new RotateVelocityDriveCommand( + drivebaseSubsystem, + translationXSupplier, + translationYSupplier, + rotationVelocity, + anthony.rightBumper())); new Trigger( () -> @@ -539,68 +552,67 @@ public void end(boolean interrupted) { // new N9_1ConePlus2CubeMobility( // 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - // autoSelector.addOption( - // "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem)); - - // autoSelector.addOption( - // "Near Substation Mobility [APRILTAG]", - // new MobilityAuto( - // manueverGenerator, - // drivebaseSubsystem, - // intakeSubsystem, - // elevatorSubsystem, - // rgbSubsystem, - // new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); - - // autoSelector.addOption( - // "Far Substation Mobility [APRILTAG]", - // new MobilityAuto( - // manueverGenerator, - // drivebaseSubsystem, - // intakeSubsystem, - // elevatorSubsystem, - // rgbSubsystem, - // new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); - - // autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); + autoSelector.addOption( + "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem)); - // autoSelector.addOption( - // "N3 1Cone + Mobility Engage", - // new N3_1ConePlusMobilityEngage( - // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.setDefaultOption( - // "N3 1Cone + Mobility", - // new N3_1ConePlusMobility( - // 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + autoSelector.addOption( + "Near Substation Mobility [APRILTAG]", + new MobilityAuto( + manueverGenerator, + drivebaseSubsystem, + intakeSubsystem, + elevatorSubsystem, + rgbSubsystem, + new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); - // autoSelector.setDefaultOption( - // "N6 1Cone + Engage", - // new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.addOption( - // "N9 1Cone + Mobility Engage", - // new N9_1ConePlusMobilityEngage( - // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - - // autoSelector.addOption( - // "N9 1Cone + Mobility", - // new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, - // drivebaseSubsystem)); - - // autoSelector.addOption( - // "Score High Cone [DOES NOT CALIBRATE]", - // new SetZeroModeCommand( - // elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check - // // later - // .raceWith( - // new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false) - // .andThen( - // new ScoreCommand( - // intakeSubsystem, - // elevatorSubsystem, - // Constants.SCORE_STEP_MAP.get( - // NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH)))))); + autoSelector.addOption( + "Far Substation Mobility [APRILTAG]", + new MobilityAuto( + manueverGenerator, + drivebaseSubsystem, + intakeSubsystem, + elevatorSubsystem, + rgbSubsystem, + new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); + + autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); + + autoSelector.addOption( + "N3 1Cone + Mobility Engage", + new N3_1ConePlusMobilityEngage( + 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.setDefaultOption( + "N3 1Cone + Mobility", + new N3_1ConePlusMobility( + 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.setDefaultOption( + "N6 1Cone + Engage", + new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.addOption( + "N9 1Cone + Mobility Engage", + new N9_1ConePlusMobilityEngage( + 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.addOption( + "N9 1Cone + Mobility", + new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + + autoSelector.addOption( + "Score High Cone [DOES NOT CALIBRATE]", + new SetZeroModeCommand( + elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check + // later + .raceWith( + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false) + .andThen( + new ScoreCommand( + intakeSubsystem, + elevatorSubsystem, + Constants.SCORE_STEP_MAP.get( + NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH)))))); driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0); From 8e417cdf856e4570dd5d0be5d0011a87c686014b Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 09:08:55 -0800 Subject: [PATCH 49/63] fix ground pickup issue --- src/main/java/frc/robot/RobotContainer.java | 26 +++++++++---------- .../commands/GroundIntakeElevatorCommand.java | 15 +++++++++++ 2 files changed, 28 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9de9900..405aa3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -600,19 +600,19 @@ public void end(boolean interrupted) { "N9 1Cone + Mobility", new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - autoSelector.addOption( - "Score High Cone [DOES NOT CALIBRATE]", - new SetZeroModeCommand( - elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check - // later - .raceWith( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false) - .andThen( - new ScoreCommand( - intakeSubsystem, - elevatorSubsystem, - Constants.SCORE_STEP_MAP.get( - NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH)))))); + // autoSelector.addOption( + // "Score High Cone [DOES NOT CALIBRATE]", + // new SetZeroModeCommand( + // elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check + // // later + // .raceWith( + // new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false) + // .andThen( + // new ScoreCommand( + // intakeSubsystem, + // elevatorSubsystem, + // Constants.SCORE_STEP_MAP.get( + // NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH)))))); driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0); diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java index f84e59f..5261718 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -2,7 +2,10 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; +import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; +import frc.util.Util; + import java.util.function.BooleanSupplier; public class GroundIntakeElevatorCommand extends CommandBase { @@ -25,4 +28,16 @@ public void execute() { elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); } } + + @Override + public boolean isFinished() { + return Util.epsilonEquals( + elevatorSubsystem.getCurrentAngleDegrees(), + elevatorSubsystem.getTargetAngle(), + Elevator.ANGLE_EPSILON) + && Util.epsilonEquals( + elevatorSubsystem.getCurrentExtensionInches(), + elevatorSubsystem.getTargetExtension(), + Elevator.EXTENSION_EPSILON); + } } From e609e7b74b64bbf2f3f6180d3116d75ced658310 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 10:15:08 -0800 Subject: [PATCH 50/63] differentiate cube and cone stator limit, haltdrivecommand bind --- src/main/java/frc/robot/Constants.java | 5 ++++- src/main/java/frc/robot/RobotContainer.java | 2 ++ .../commands/GroundIntakeElevatorCommand.java | 18 ++++++------------ .../frc/robot/subsystems/IntakeSubsystem.java | 7 +++---- 4 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3f7792b..9302645 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -207,7 +207,7 @@ public static final class Setpoints { public static final ElevatorState GROUND_INTAKE_CONE = new ElevatorState(MIN_EXTENSION_INCHES, 75); public static final ElevatorState GROUND_INTAKE_CUBE = - new ElevatorState(MIN_EXTENSION_INCHES, 81.4); + new ElevatorState(MIN_EXTENSION_INCHES, 85); public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5); public static final ElevatorState SCORE_HIGH_CUBE = new ElevatorState(53.8, 74.5); public static final ElevatorState SCORE_MID_CONE = new ElevatorState(22.5, 35.5); @@ -255,6 +255,9 @@ public static final class Intake { public static final double HOLD_CONE_PERCENT = 0.1; public static final double HOLD_CUBE_PERCENT = -0.1; + public static final double CONE_STATOR_LIMIT = 75; + public static final double CUBE_STATOR_LIMIT = 65; + public static final class Ports { public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor"); public static final int CONE_TOF_PORT = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 405aa3b..757d269 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -219,6 +219,8 @@ private void configureButtonBindings() { anthony.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + jacob.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java index 5261718..5cff800 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -4,6 +4,7 @@ import frc.robot.Constants; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.Modes; import frc.util.Util; import java.util.function.BooleanSupplier; @@ -20,6 +21,11 @@ public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanS addRequirements(this.elevatorSubsystem); } + @Override + public void initialize() { + elevatorSubsystem.setMode(Modes.POSITION_CONTROL); + } + @Override public void execute() { if (isCube.getAsBoolean()) { @@ -28,16 +34,4 @@ public void execute() { elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); } } - - @Override - public boolean isFinished() { - return Util.epsilonEquals( - elevatorSubsystem.getCurrentAngleDegrees(), - elevatorSubsystem.getTargetAngle(), - Elevator.ANGLE_EPSILON) - && Util.epsilonEquals( - elevatorSubsystem.getCurrentExtensionInches(), - elevatorSubsystem.getTargetExtension(), - Elevator.EXTENSION_EPSILON); - } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ad7e19b..0e2e68d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -20,7 +20,6 @@ public class IntakeSubsystem extends SubsystemBase { private Modes currentIntakeMode; private LinearFilter filter; private double filterOutput; - private double statorCurrentLimit; private boolean isCube; // private final TimeOfFlight coneToF, cubeToF; @@ -40,8 +39,6 @@ public IntakeSubsystem() { filterOutput = 0.0; - statorCurrentLimit = 75; - isCube = false; // coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT); @@ -122,7 +119,9 @@ private double getFilterCalculatedValue() { @Override public void periodic() { filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); - if (filterOutput >= statorCurrentLimit) { + if(isCube && filterOutput >= Intake.CUBE_STATOR_LIMIT) { + currentIntakeMode = Modes.HOLD; + } else if(filterOutput >= Intake.CUBE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; } From b2ace8ca7037796e29367cd64ca119ff38fa94de Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 10:19:34 -0800 Subject: [PATCH 51/63] spotless + softer elevator zero --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 3 --- .../java/frc/robot/commands/GroundIntakeElevatorCommand.java | 3 --- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 4 ++-- 4 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9302645..417df16 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -237,8 +237,8 @@ public static final class Setpoints { public static final double EXTENSION_EPSILON = 5; public static final double ANGULAR_OFFSET = 0; - public static final double ZERO_MOTOR_POWER = -0.2; - public static final double ZERO_STATOR_LIMIT = 10; + public static final double ZERO_MOTOR_POWER = -0.12; + public static final double ZERO_STATOR_LIMIT = 5; public static final double STATOR_LIMIT = 25; public static final double GRAVITY_OFFSET_PERCENT = .2; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 757d269..413b113 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,13 +25,10 @@ import frc.robot.Constants.Drive; import frc.robot.Constants.Elevator; import frc.robot.autonomous.commands.MobilityAuto; -import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobility; -import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobilityEngage; import frc.robot.autonomous.commands.N2_Engage; import frc.robot.autonomous.commands.N3_1ConePlusMobility; import frc.robot.autonomous.commands.N3_1ConePlusMobilityEngage; import frc.robot.autonomous.commands.N6_1ConePlusEngage; -import frc.robot.autonomous.commands.N9_1ConePlus2CubeMobility; import frc.robot.autonomous.commands.N9_1ConePlusMobility; import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage; import frc.robot.commands.DefaultDriveCommand; diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java index 5cff800..a9492f3 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -2,11 +2,8 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; -import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.ElevatorSubsystem.Modes; -import frc.util.Util; - import java.util.function.BooleanSupplier; public class GroundIntakeElevatorCommand extends CommandBase { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 0e2e68d..a6ab50c 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -119,9 +119,9 @@ private double getFilterCalculatedValue() { @Override public void periodic() { filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); - if(isCube && filterOutput >= Intake.CUBE_STATOR_LIMIT) { + if (isCube && filterOutput >= Intake.CUBE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; - } else if(filterOutput >= Intake.CUBE_STATOR_LIMIT) { + } else if (filterOutput >= Intake.CUBE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; } From 814fd1fae793ba69d8f28ebc85e8e3a40d7074ed Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 11:13:48 -0800 Subject: [PATCH 52/63] better elevator zero, better cone ground angle --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/commands/GroundPickupCommand.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 417df16..41bdeec 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -205,7 +205,7 @@ public static final class Setpoints { public static final ElevatorState SHELF_INTAKE_CONE = new ElevatorState(47.3, 68.5); public static final ElevatorState SHELF_INTAKE_CUBE = new ElevatorState(43.2, 68.5); public static final ElevatorState GROUND_INTAKE_CONE = - new ElevatorState(MIN_EXTENSION_INCHES, 75); + new ElevatorState(MIN_EXTENSION_INCHES, 72.5); public static final ElevatorState GROUND_INTAKE_CUBE = new ElevatorState(MIN_EXTENSION_INCHES, 85); public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5); @@ -238,7 +238,7 @@ public static final class Setpoints { public static final double ANGULAR_OFFSET = 0; public static final double ZERO_MOTOR_POWER = -0.12; - public static final double ZERO_STATOR_LIMIT = 5; + public static final double ZERO_STATOR_LIMIT = 7; public static final double STATOR_LIMIT = 25; public static final double GRAVITY_OFFSET_PERCENT = .2; diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 68802d6..3e073f8 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -23,8 +23,8 @@ public GroundPickupCommand( // Add your commands in the addCommands() call, e.g. addCommands( - new GroundIntakeElevatorCommand(elevatorSubsystem, isCube) - .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube)) + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube) + .alongWith(new GroundIntakeElevatorCommand(elevatorSubsystem, isCube)) .andThen( new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED))); } From dbc075cb509e44ba520aaa272454d33a82c25f8c Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 14:29:48 -0800 Subject: [PATCH 53/63] add auto --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++-- .../commands/N3_1ConePlusMobility.java | 2 +- .../robot/autonomous/commands/N6_1Cone.java | 40 +++++++++++++++++++ .../commands/N9_1ConePlusMobility.java | 2 +- .../robot/commands/DefaultDriveCommand.java | 5 ++- 5 files changed, 60 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/commands/N6_1Cone.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e482dd9..cd3a7cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,14 @@ import frc.robot.Constants.Config; import frc.robot.Constants.Drive; import frc.robot.Constants.Elevator; +import frc.robot.autonomous.commands.MobilityAuto; +import frc.robot.autonomous.commands.N2_Engage; +import frc.robot.autonomous.commands.N3_1ConePlusMobility; +import frc.robot.autonomous.commands.N3_1ConePlusMobilityEngage; +import frc.robot.autonomous.commands.N6_1Cone; +import frc.robot.autonomous.commands.N6_1ConePlusEngage; +import frc.robot.autonomous.commands.N9_1ConePlusMobility; +import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.DriveToPlaceCommand; @@ -574,9 +582,13 @@ public void end(boolean interrupted) { // new N3_1ConePlusMobility( // 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - // autoSelector.setDefaultOption( - // "N6 1Cone + Engage", - // new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + autoSelector.setDefaultOption( + "N6 1Cone", + new N6_1Cone(intakeSubsystem, elevatorSubsystem)); + + autoSelector.setDefaultOption( + "N6 1Cone + Engage", + new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); // autoSelector.addOption( // "N9 1Cone + Mobility Engage", diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java index 45380d0..b158d80 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java @@ -42,7 +42,7 @@ public N3_1ConePlusMobility( intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(NodeType.CONE.atHeight(Height.HIGH)), - 1), + 2.5), (new FollowTrajectoryCommand(path, true, drivebaseSubsystem)) .alongWith( (new WaitCommand(1)) diff --git a/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java b/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java new file mode 100644 index 0000000..af267fd --- /dev/null +++ b/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java @@ -0,0 +1,40 @@ +// 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 frc.robot.autonomous.commands; + +import com.pathplanner.lib.PathPlannerTrajectory; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; +import frc.robot.Constants.Elevator; +import frc.robot.commands.ElevatorPositionCommand; +import frc.robot.commands.EngageCommand; +import frc.robot.commands.FollowTrajectoryCommand; +import frc.robot.commands.IntakeModeCommand; +import frc.robot.commands.ScoreCommand; +import frc.robot.commands.SetZeroModeCommand; +import frc.robot.subsystems.DrivebaseSubsystem; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.IntakeSubsystem.Modes; +import frc.util.NodeSelectorUtility.Height; +import frc.util.NodeSelectorUtility.NodeType; + +public class N6_1Cone extends SequentialCommandGroup { + /** Creates a new N2MobilityEngage. */ + public N6_1Cone( + IntakeSubsystem intakeSubsystem, + ElevatorSubsystem elevatorSubsystem) { + + addCommands( + new SetZeroModeCommand(elevatorSubsystem) + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)), + new ScoreCommand( + intakeSubsystem, + elevatorSubsystem, + Constants.SCORE_STEP_MAP.get(NodeType.CONE.atHeight(Height.HIGH)), + 1)); + } +} diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java index 4f91b71..18e47de 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java @@ -43,7 +43,7 @@ public N9_1ConePlusMobility( intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(NodeType.CONE.atHeight(Height.HIGH)), - 1), + 2.5), (new FollowTrajectoryCommand(path, true, drivebaseSubsystem)) .alongWith( (new WaitCommand(1)) diff --git a/src/main/java/frc/robot/commands/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java index 0ef7a44..8e9786d 100644 --- a/src/main/java/frc/robot/commands/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/DefaultDriveCommand.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DrivebaseSubsystem; @@ -45,8 +46,8 @@ public DefaultDriveCommand( // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double x = translationXSupplier.getAsDouble(); - double y = translationYSupplier.getAsDouble(); + double x = MathUtil.clamp(translationXSupplier.getAsDouble(), -0.8, 0.8); + double y = MathUtil.clamp(translationYSupplier.getAsDouble(), -0.8, 0.8); Boolean forwardRelative = isRobotRelativeForwardSupplier.getAsBoolean(); // Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean(); From e679ac409aa5b75225fd71eedb38b658bd188153 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 16:05:18 -0800 Subject: [PATCH 54/63] stuff --- src/main/java/frc/robot/Constants.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 94 ++++++++++--------- .../robot/autonomous/commands/N6_1Cone.java | 4 +- 3 files changed, 56 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3f7792b..b60a9c5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -152,7 +152,7 @@ public static final class Module1 { // historically front right public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(162.685546875 + 180) // comp bot offset + ? -Math.toRadians(162.59765625 + 180) // comp bot offset : -Math.toRadians(184.833984); // practice bot offset } @@ -163,7 +163,7 @@ public static final class Module2 { // historically front left public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(220.517578125) // comp bot offset + ? -Math.toRadians(219.7265625) // comp bot offset : -Math.toRadians(307.968750); // practice bot offset } @@ -174,7 +174,7 @@ public static final class Module3 { // historically back left public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(134.82421875) // comp bot offset + ? -Math.toRadians(135.966796875) // comp bot offset : -Math.toRadians(306.738281 + 180); // practice bot offset } @@ -185,7 +185,7 @@ public static final class Module4 { // historically back right public static final double STEER_OFFSET = IS_COMP_BOT - ? -Math.toRadians(201.09375 + 180) // comp bot offset + ? -Math.toRadians(200.0390625 + 180) // comp bot offset : -Math.toRadians(60.908203); // practice bot offset } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd3a7cf..01e2d02 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import com.pathplanner.lib.server.PathPlannerServer; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -42,6 +43,7 @@ import frc.robot.commands.HashMapCommand; import frc.robot.commands.IntakeModeCommand; import frc.robot.commands.RotateVectorDriveCommand; +import frc.robot.commands.RotateVelocityDriveCommand; import frc.robot.commands.ScoreCommand; import frc.robot.commands.ScoreCommand.ScoreStep; import frc.robot.commands.SetZeroModeCommand; @@ -64,6 +66,7 @@ import frc.util.NodeSelectorUtility.NodeType; import frc.util.SharedReference; import frc.util.Util; +import frc.util.pathing.AlliancePose2d; import frc.util.pathing.RubenManueverGenerator; import java.util.HashMap; import java.util.List; @@ -214,6 +217,8 @@ private void configureButtonBindings() { anthony.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + jacob.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); @@ -233,14 +238,14 @@ private void configureButtonBindings() { /** percent of fraction power */ (anthony.getHID().getAButton() ? .3 : .8); - // new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0) - // .whileTrue( - // new RotateVelocityDriveCommand( - // drivebaseSubsystem, - // translationXSupplier, - // translationYSupplier, - // rotationVelocity, - // anthony.rightBumper())); + new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0) + .whileTrue( + new RotateVelocityDriveCommand( + drivebaseSubsystem, + translationXSupplier, + translationYSupplier, + rotationVelocity, + anthony.rightBumper())); new Trigger( () -> @@ -547,40 +552,40 @@ public void end(boolean interrupted) { // new N9_1ConePlus2CubeMobility( // 4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - // autoSelector.addOption( - // "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem)); + autoSelector.addOption( + "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem)); - // autoSelector.addOption( - // "Near Substation Mobility [APRILTAG]", - // new MobilityAuto( - // manueverGenerator, - // drivebaseSubsystem, - // intakeSubsystem, - // elevatorSubsystem, - // rgbSubsystem, - // new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); + autoSelector.addOption( + "Near Substation Mobility [APRILTAG]", + new MobilityAuto( + manueverGenerator, + drivebaseSubsystem, + intakeSubsystem, + elevatorSubsystem, + rgbSubsystem, + new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0)))); - // autoSelector.addOption( - // "Far Substation Mobility [APRILTAG]", - // new MobilityAuto( - // manueverGenerator, - // drivebaseSubsystem, - // intakeSubsystem, - // elevatorSubsystem, - // rgbSubsystem, - // new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); + autoSelector.addOption( + "Far Substation Mobility [APRILTAG]", + new MobilityAuto( + manueverGenerator, + drivebaseSubsystem, + intakeSubsystem, + elevatorSubsystem, + rgbSubsystem, + new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0)))); - // autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); + autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem)); - // autoSelector.addOption( - // "N3 1Cone + Mobility Engage", - // new N3_1ConePlusMobilityEngage( - // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + autoSelector.addOption( + "N3 1Cone + Mobility Engage", + new N3_1ConePlusMobilityEngage( + 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - // autoSelector.setDefaultOption( - // "N3 1Cone + Mobility", - // new N3_1ConePlusMobility( - // 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + autoSelector.setDefaultOption( + "N3 1Cone + Mobility", + new N3_1ConePlusMobility( + 4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); autoSelector.setDefaultOption( "N6 1Cone", @@ -590,15 +595,14 @@ public void end(boolean interrupted) { "N6 1Cone + Engage", new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - // autoSelector.addOption( - // "N9 1Cone + Mobility Engage", - // new N9_1ConePlusMobilityEngage( - // 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + autoSelector.addOption( + "N9 1Cone + Mobility Engage", + new N9_1ConePlusMobilityEngage( + 5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); - // autoSelector.addOption( - // "N9 1Cone + Mobility", - // new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, - // drivebaseSubsystem)); + autoSelector.addOption( + "N9 1Cone + Mobility", + new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); // autoSelector.addOption( // "Score High Cone [DOES NOT CALIBRATE]", diff --git a/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java b/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java index af267fd..354bbb8 100644 --- a/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java +++ b/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java @@ -35,6 +35,8 @@ public N6_1Cone( intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(NodeType.CONE.atHeight(Height.HIGH)), - 1)); + 2), + new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED) + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); } } From 93505d2df2cffc25f3e95846396496f08317d532 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 16:09:03 -0800 Subject: [PATCH 55/63] fix groundintake again --- .../frc/robot/commands/GroundIntakeElevatorCommand.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java index f84e59f..cdbd54f 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -3,6 +3,8 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.Modes; + import java.util.function.BooleanSupplier; public class GroundIntakeElevatorCommand extends CommandBase { @@ -17,6 +19,11 @@ public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanS addRequirements(this.elevatorSubsystem); } + @Override + public void initialize() { + elevatorSubsystem.setMode(Modes.POSITION_CONTROL); + } + @Override public void execute() { if (isCube.getAsBoolean()) { @@ -24,5 +31,6 @@ public void execute() { } else { elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); } + elevatorSubsystem.setMode(Modes.POSITION_CONTROL); } } From e72968ee9a69e31cd33d09b1613a35bd4a057be6 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 16:17:24 -0800 Subject: [PATCH 56/63] fasster drive --- src/main/java/frc/robot/commands/DefaultDriveCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java index 8e9786d..540e1e5 100644 --- a/src/main/java/frc/robot/commands/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/DefaultDriveCommand.java @@ -46,8 +46,8 @@ public DefaultDriveCommand( // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double x = MathUtil.clamp(translationXSupplier.getAsDouble(), -0.8, 0.8); - double y = MathUtil.clamp(translationYSupplier.getAsDouble(), -0.8, 0.8); + double x = MathUtil.clamp(translationXSupplier.getAsDouble(), -0.9, 0.9); + double y = MathUtil.clamp(translationYSupplier.getAsDouble(), -0.9, 0.9); Boolean forwardRelative = isRobotRelativeForwardSupplier.getAsBoolean(); // Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean(); From 970f36c2690ba63978b556e27ac49ed5105dfe23 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 16:20:24 -0800 Subject: [PATCH 57/63] make wrist stow further away --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5ca5a9c..98414c9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -221,7 +221,7 @@ public static final class Setpoints { public static final double MAX_EXTENSION_INCHES = 54; public static final double MIN_EXTENSION_INCHES = 1; - public static final double MIN_ANGLE_DEGREES = 3; + public static final double MIN_ANGLE_DEGREES = 5; public static final double MAX_ANGLE_DEGREES = 109.25; public static final int FALCON_CPR = 2048; From 6f8e14997fdad7231595b26e87b146ad142e5e90 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 16:46:35 -0800 Subject: [PATCH 58/63] FIXME make cube default ground intake my solution is very jank. make it so isCone instead of isCube later --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/DefaultDriveCommand.java | 4 ++-- .../java/frc/robot/commands/GroundIntakeElevatorCommand.java | 2 +- src/main/java/frc/robot/commands/IntakeModeCommand.java | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 98414c9..266081b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -207,7 +207,7 @@ public static final class Setpoints { public static final ElevatorState GROUND_INTAKE_CONE = new ElevatorState(MIN_EXTENSION_INCHES, 72.5); public static final ElevatorState GROUND_INTAKE_CUBE = - new ElevatorState(MIN_EXTENSION_INCHES, 85); + new ElevatorState(MIN_EXTENSION_INCHES, 83); public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5); public static final ElevatorState SCORE_HIGH_CUBE = new ElevatorState(53.8, 74.5); public static final ElevatorState SCORE_MID_CONE = new ElevatorState(22.5, 35.5); diff --git a/src/main/java/frc/robot/commands/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java index 540e1e5..5283e96 100644 --- a/src/main/java/frc/robot/commands/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/DefaultDriveCommand.java @@ -46,8 +46,8 @@ public DefaultDriveCommand( // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double x = MathUtil.clamp(translationXSupplier.getAsDouble(), -0.9, 0.9); - double y = MathUtil.clamp(translationYSupplier.getAsDouble(), -0.9, 0.9); + double x = translationXSupplier.getAsDouble(); + double y = translationYSupplier.getAsDouble(); Boolean forwardRelative = isRobotRelativeForwardSupplier.getAsBoolean(); // Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean(); diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java index bd04d94..1cea6fb 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -25,7 +25,7 @@ public void initialize() { @Override public void execute() { - if (isCube.getAsBoolean()) { + if (!isCube.getAsBoolean()) { elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CUBE); } else { elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); diff --git a/src/main/java/frc/robot/commands/IntakeModeCommand.java b/src/main/java/frc/robot/commands/IntakeModeCommand.java index a2d860f..578e0ba 100644 --- a/src/main/java/frc/robot/commands/IntakeModeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeModeCommand.java @@ -42,7 +42,7 @@ public void initialize() { @Override public void execute() { if (isCone != null) { - intakeSubsystem.setIsCube(isCone.getAsBoolean()); + intakeSubsystem.setIsCube(!isCone.getAsBoolean()); } } From df9f51ca384663959d84e672735b08bf46830042 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 17:56:42 -0800 Subject: [PATCH 59/63] more --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 266081b..2f1bff5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -253,7 +253,7 @@ public static final class Intake { public static final double OUTTAKE_CUBE_PERCENT = 0.7; public static final double HOLD_CONE_PERCENT = 0.1; - public static final double HOLD_CUBE_PERCENT = -0.1; + public static final double HOLD_CUBE_PERCENT = -0.2; public static final double CONE_STATOR_LIMIT = 75; public static final double CUBE_STATOR_LIMIT = 65; diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index e35ffa5..de67087 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -292,7 +292,7 @@ private void positionDrivePeriodic() { } else { wristMotor.set( TalonFXControlMode.PercentOutput, - MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25)); + MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.5, 0.5)); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a6ab50c..6954a63 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -121,7 +121,7 @@ public void periodic() { filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); if (isCube && filterOutput >= Intake.CUBE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; - } else if (filterOutput >= Intake.CUBE_STATOR_LIMIT) { + } else if (filterOutput >= Intake.CONE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; } From 75024b1c0e7fac88e87b91247b9a23a890c986da Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 18:46:36 -0800 Subject: [PATCH 60/63] auto stow --- src/main/java/frc/robot/commands/GroundPickupCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 3e073f8..51a5169 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -24,7 +24,7 @@ public GroundPickupCommand( addCommands( new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube) - .alongWith(new GroundIntakeElevatorCommand(elevatorSubsystem, isCube)) + .deadlineWith(new GroundIntakeElevatorCommand(elevatorSubsystem, isCube)) .andThen( new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED))); } From f10a25e6d12cf15c4e0e064f0ad3d70561d12279 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 11 Nov 2023 19:14:41 -0800 Subject: [PATCH 61/63] refactor to iscone --- .../commands/GroundIntakeElevatorCommand.java | 12 +++---- .../robot/commands/GroundPickupCommand.java | 6 ++-- .../frc/robot/commands/IntakeModeCommand.java | 2 +- .../frc/robot/subsystems/IntakeSubsystem.java | 32 +++++++++---------- 4 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java index 1cea6fb..43d4512 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -9,11 +9,11 @@ public class GroundIntakeElevatorCommand extends CommandBase { private ElevatorSubsystem elevatorSubsystem; - private BooleanSupplier isCube; + private BooleanSupplier isCone; - public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanSupplier isCube) { + public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanSupplier isCone) { this.elevatorSubsystem = elevatorSubsystem; - this.isCube = isCube; + this.isCone = isCone; addRequirements(this.elevatorSubsystem); } @@ -25,10 +25,10 @@ public void initialize() { @Override public void execute() { - if (!isCube.getAsBoolean()) { - elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CUBE); - } else { + if (isCone.getAsBoolean()) { elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); + } else { + elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CUBE); } elevatorSubsystem.setMode(Modes.POSITION_CONTROL); } diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 51a5169..5f88ac1 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -19,12 +19,12 @@ public class GroundPickupCommand extends SequentialCommandGroup { public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, - BooleanSupplier isCube) { + BooleanSupplier isCone) { // Add your commands in the addCommands() call, e.g. addCommands( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube) - .deadlineWith(new GroundIntakeElevatorCommand(elevatorSubsystem, isCube)) + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCone) + .deadlineWith(new GroundIntakeElevatorCommand(elevatorSubsystem, isCone)) .andThen( new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED))); } diff --git a/src/main/java/frc/robot/commands/IntakeModeCommand.java b/src/main/java/frc/robot/commands/IntakeModeCommand.java index 578e0ba..1cd4499 100644 --- a/src/main/java/frc/robot/commands/IntakeModeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeModeCommand.java @@ -42,7 +42,7 @@ public void initialize() { @Override public void execute() { if (isCone != null) { - intakeSubsystem.setIsCube(!isCone.getAsBoolean()); + intakeSubsystem.setIsCone(isCone.getAsBoolean()); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 6954a63..4d52b6d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -20,7 +20,7 @@ public class IntakeSubsystem extends SubsystemBase { private Modes currentIntakeMode; private LinearFilter filter; private double filterOutput; - private boolean isCube; + private boolean isCone; // private final TimeOfFlight coneToF, cubeToF; /** Creates a new IntakeSubsystem. */ @@ -39,7 +39,7 @@ public IntakeSubsystem() { filterOutput = 0.0; - isCube = false; + isCone = false; // coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT); // cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT); @@ -49,7 +49,7 @@ public IntakeSubsystem() { shuffleboard.addString("Current mode", () -> currentIntakeMode.toString()); shuffleboard.addDouble("filter output", () -> filterOutput); shuffleboard.addDouble("motor output", intakeMotor::getMotorOutputPercent); - shuffleboard.addBoolean("is cube intake", () -> isCube); + shuffleboard.addBoolean("is cube intake", () -> isCone); // shuffleboard.addDouble("coneToFInches", this::getConeToFInches); // shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches); } @@ -65,24 +65,24 @@ public void intakePeriodic(Modes mode) { switch (mode) { case INTAKE: - if (isCube) { - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_CUBE_PERCENT); - } else { + if (isCone) { intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_CONE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_CUBE_PERCENT); } break; case OUTTAKE: - if (isCube) { - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_CUBE_PERCENT); - } else { + if (isCone) { intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_CONE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_CUBE_PERCENT); } break; case HOLD: - if (isCube) { - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_CUBE_PERCENT); - } else { + if (isCone) { intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_CONE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_CUBE_PERCENT); } break; case OFF: @@ -108,8 +108,8 @@ public void setMode(Modes mode) { currentIntakeMode = mode; } - public void setIsCube(boolean isCube) { - this.isCube = isCube; + public void setIsCone(boolean isCone) { + this.isCone = isCone; } private double getFilterCalculatedValue() { @@ -119,9 +119,9 @@ private double getFilterCalculatedValue() { @Override public void periodic() { filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); - if (isCube && filterOutput >= Intake.CUBE_STATOR_LIMIT) { + if (isCone && filterOutput >= Intake.CONE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; - } else if (filterOutput >= Intake.CONE_STATOR_LIMIT) { + } else if (filterOutput >= Intake.CUBE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; } From 8a5e9f5021551fad87ff42a5db01d5904b3dffd1 Mon Sep 17 00:00:00 2001 From: Jay Date: Sun, 12 Nov 2023 08:00:09 -0800 Subject: [PATCH 62/63] more consistent auto stow, faster wrist --- src/main/java/frc/robot/Constants.java | 3 ++- .../robot/commands/GroundPickupCommand.java | 2 +- .../frc/robot/commands/IntakeModeCommand.java | 19 ++++++++++++++++--- .../robot/subsystems/ElevatorSubsystem.java | 2 +- .../frc/robot/subsystems/IntakeSubsystem.java | 4 ++++ 5 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2f1bff5..7957675 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -256,7 +256,8 @@ public static final class Intake { public static final double HOLD_CUBE_PERCENT = -0.2; public static final double CONE_STATOR_LIMIT = 75; - public static final double CUBE_STATOR_LIMIT = 65; + public static final double CUBE_STATOR_LIMIT = 69; + public static final double GROUND_CUBE_STATOR_LIMIT = 60; public static final class Ports { public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor"); diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 5f88ac1..80f06bd 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -23,7 +23,7 @@ public GroundPickupCommand( // Add your commands in the addCommands() call, e.g. addCommands( - new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCone) + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCone, true) .deadlineWith(new GroundIntakeElevatorCommand(elevatorSubsystem, isCone)) .andThen( new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED))); diff --git a/src/main/java/frc/robot/commands/IntakeModeCommand.java b/src/main/java/frc/robot/commands/IntakeModeCommand.java index 1cd4499..5fd9a00 100644 --- a/src/main/java/frc/robot/commands/IntakeModeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeModeCommand.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants.Intake; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; import java.util.function.BooleanSupplier; @@ -13,16 +14,22 @@ public class IntakeModeCommand extends CommandBase { private IntakeSubsystem intakeSubsystem; private Modes mode; private BooleanSupplier isCone; + private boolean isGroundIntake; /** Creates a new IntakeCommand. */ - public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone) { + public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone, boolean isGroundIntake) { // Use addRequirements() here to declare subsystem dependencies. this.intakeSubsystem = intakeSubsystem; this.mode = mode; this.isCone = isCone; + this.isGroundIntake = isGroundIntake; addRequirements(intakeSubsystem); } + public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone) { + this(intakeSubsystem, mode, isCone, false); + } + public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode) { // Use addRequirements() here to declare subsystem dependencies. this.intakeSubsystem = intakeSubsystem; @@ -53,8 +60,14 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - if (intakeSubsystem.getMode() == Modes.HOLD || intakeSubsystem.getMode() == Modes.OFF) { - return true; + if(!isGroundIntake) { + if (intakeSubsystem.getMode() == Modes.HOLD || intakeSubsystem.getMode() == Modes.OFF) { + return true; + } + } else { + if(intakeSubsystem.getFilterOutput() > Intake.GROUND_CUBE_STATOR_LIMIT) { + return true; + } } return false; } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index de67087..82938bb 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -292,7 +292,7 @@ private void positionDrivePeriodic() { } else { wristMotor.set( TalonFXControlMode.PercentOutput, - MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.5, 0.5)); + MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -1, 1)); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 4d52b6d..78c932e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -112,6 +112,10 @@ public void setIsCone(boolean isCone) { this.isCone = isCone; } + public double getFilterOutput() { + return filterOutput; + } + private double getFilterCalculatedValue() { return filter.calculate(intakeMotor.getStatorCurrent()); } From f47a9277de6ea259a45517cda4720cdaa9b5df22 Mon Sep 17 00:00:00 2001 From: Jay Date: Sun, 12 Nov 2023 23:42:20 -0800 Subject: [PATCH 63/63] its over --- .../frc/robot/autonomous/commands/N3_1ConePlusMobility.java | 2 +- .../robot/autonomous/commands/N3_1ConePlusMobilityEngage.java | 2 +- src/main/java/frc/robot/autonomous/commands/N6_1Cone.java | 2 +- .../frc/robot/autonomous/commands/N6_1ConePlusEngage.java | 2 +- .../robot/autonomous/commands/N9_1ConePlus2CubeMobility.java | 2 +- .../frc/robot/autonomous/commands/N9_1ConePlusMobility.java | 2 +- .../robot/autonomous/commands/N9_1ConePlusMobilityEngage.java | 2 +- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 4 ++-- 8 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java index b158d80..e547f7a 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java @@ -37,7 +37,7 @@ public N3_1ConePlusMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) .deadlineWith( - new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> false)), + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java index 9c4da8a..4bf9db8 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java @@ -41,7 +41,7 @@ public N3_1ConePlusMobilityEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java b/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java index 354bbb8..386140f 100644 --- a/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java +++ b/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java @@ -30,7 +30,7 @@ public N6_1Cone( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java b/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java index 930be55..dd6f63e 100644 --- a/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java @@ -39,7 +39,7 @@ public N6_1ConePlusEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java index 33aed87..6118b91 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java @@ -39,7 +39,7 @@ public N9_1ConePlus2CubeMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java index 18e47de..7910e6a 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java @@ -38,7 +38,7 @@ public N9_1ConePlusMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) .deadlineWith( - new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> false)), + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java index a60f2dd..4758dc9 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java @@ -41,7 +41,7 @@ public N9_1ConePlusMobilityEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) .deadlineWith( - new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> false)), + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 82938bb..cf7fca7 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -266,7 +266,7 @@ private void percentDrivePeriodic() { if (wristFilterOutput > Elevator.STATOR_LIMIT) { wristMotor.set(TalonFXControlMode.PercentOutput, 0); } else { - wristMotor.set(TalonFXControlMode.PercentOutput, MathUtil.clamp(wristPercentControl, -1, 1)); + wristMotor.set(TalonFXControlMode.PercentOutput, MathUtil.clamp(wristPercentControl, -0.75, 0.75)); } } @@ -292,7 +292,7 @@ private void positionDrivePeriodic() { } else { wristMotor.set( TalonFXControlMode.PercentOutput, - MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -1, 1)); + MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.75, 0.75)); } }