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 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 27fac49..7957675 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Filesystem; 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; @@ -151,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(162.59765625 + 180) // comp bot offset : -Math.toRadians(184.833984); // practice bot offset } @@ -162,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(219.7265625) // comp bot offset : -Math.toRadians(307.968750); // practice bot offset } @@ -173,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(135.966796875) // comp bot offset : -Math.toRadians(306.738281 + 180); // practice bot offset } @@ -184,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(200.0390625 + 180) // comp bot offset : -Math.toRadians(60.908203); // practice bot offset } } @@ -193,45 +194,75 @@ 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 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 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 SCORE_HIGH = new ElevatorState(MAX_HEIGHT, 0); - public static final ElevatorState SCORE_MID = new ElevatorState(15, 20); - public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_HEIGHT, 40); + public static final ElevatorState STOWED = + new ElevatorState(MIN_EXTENSION_INCHES, MIN_ANGLE_DEGREES); + 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, 72.5); + public static final ElevatorState GROUND_INTAKE_CUBE = + 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); + public static final ElevatorState SCORE_MID_CUBE = new ElevatorState(30, 74.5); + public static final ElevatorState SCORE_LOW_CONE = + new ElevatorState(MIN_EXTENSION_INCHES, 55); + public static final ElevatorState SCORE_LOW_CUBE = + new ElevatorState(MIN_EXTENSION_INCHES, 48); } - public static final double MAX_HEIGHT = 20; - public static final double MIN_HEIGHT = 0; + public static final double MAX_EXTENSION_INCHES = 54; + public static final double MIN_EXTENSION_INCHES = 1; - 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 MIN_ANGLE_DEGREES = 5; + 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; + public static final double CARRIAGE_RATIO = 2; 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 / 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; + + public static final double ZERO_MOTOR_POWER = -0.12; + public static final double ZERO_STATOR_LIMIT = 7; + public static final double STATOR_LIMIT = 25; + + public static final double GRAVITY_OFFSET_PERCENT = .2; } public static final class Intake { - public static final double INTAKE_PERCENT = 1; + public static final double INTAKE_CONE_PERCENT = 0.7; + public static final double INTAKE_CUBE_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 OUTTAKE_PERCENT = -1; + public static final double HOLD_CONE_PERCENT = 0.1; + public static final double HOLD_CUBE_PERCENT = -0.2; - public static final double HOLD_PERCENT = 0.15; + public static final double CONE_STATOR_LIMIT = 75; + 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"); + public static final int CONE_TOF_PORT = 0; + public static final int CUBE_TOF_PORT = 0; } } @@ -239,28 +270,31 @@ 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(new ElevatorState(22.5, 0), IntakeSubsystem.Modes.OUTTAKE, false)), 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, false)), NodeType.CONE.atHeight(Height.LOW), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(), - new ScoreStep(IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_LOW_CONE).canWaitHere(), + new ScoreStep(IntakeSubsystem.Modes.OUTTAKE, false)), 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, true)), 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, true)), NodeType.CUBE.atHeight(Height.LOW), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(), - new ScoreStep(IntakeSubsystem.Modes.OUTTAKE))); + new ScoreStep(Elevator.Setpoints.SCORE_LOW_CUBE).canWaitHere(), + new ScoreStep(IntakeSubsystem.Modes.OUTTAKE, true))); public static final class Vision { public static record VisionSource(String name, Transform3d robotToCamera) {} @@ -271,9 +305,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, @@ -283,9 +317,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)))); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69c8fd7..01e2d02 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,16 +25,13 @@ 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_1Cone; 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.AlignGamepieceCommand; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.DefenseModeCommand; import frc.robot.commands.DriveToPlaceCommand; @@ -51,7 +48,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 +64,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; @@ -109,11 +104,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<>(); @@ -124,9 +119,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() { @@ -140,23 +135,21 @@ 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.setDefaultCommand( - new WristManualCommand( - elevatorSubsystem, () -> ControllerUtil.deadband(jason.getRightY(), 0.2))); + elevatorSubsystem, + () -> ControllerUtil.deadband(jacob.getLeftY() * -0.42, 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); @@ -177,7 +170,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)); } /** @@ -203,38 +196,47 @@ public void containerMatchStarting() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - // vibrate jason controller when in layer - jasonLayer.whenChanged( + + // 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)); - // 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)); + anthony.leftBumper().onTrue(new DefenseModeCommand(drivebaseSubsystem)); + + anthony.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); - will.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); - jason.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); + anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + + jacob.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); + + jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); DoubleSupplier rotation = exponential( () -> ControllerUtil.deadband( - (will.getRightTriggerAxis() + -will.getLeftTriggerAxis()), .1), + (anthony.getRightTriggerAxis() + -anthony.getLeftTriggerAxis()), .1), 2); + DoubleSupplier rotationVelocity = () -> rotation.getAsDouble() * 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( @@ -243,121 +245,156 @@ private void configureButtonBindings() { translationXSupplier, translationYSupplier, rotationVelocity, - will.rightBumper(), - will.leftBumper())); + 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() - .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() + anthony + .b() .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(), + anthony.rightBumper(), Optional.of(rgbSubsystem), - Optional.of(will.getHID()))); - - will.x() + Optional.of(anthony.getHID()))); + + // anthony.y() + // .onTrue( + // new DriveToPlaceCommand( + // drivebaseSubsystem, + // manueverGenerator, + // (new AlliancePose2d(15.3639 - 1.5, 7.3965, Rotation2d.fromDegrees(0)))::get, + // translationXSupplier, + // translationYSupplier, + // anthony.rightBumper(), + // Optional.of(rgbSubsystem), + // Optional.of(anthony.getHID()))); + + anthony + .x() .onTrue( new EngageCommand( drivebaseSubsystem, elevatorSubsystem, EngageCommand.EngageDirection.GO_BACKWARD)); // outtake states - jasonLayer - .off(jason.leftTrigger()) - .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)); + jacobLayer + .off(jacob.leftTrigger()) + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.leftBumper())); + + jacobLayer + .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)); // intake presets - // jasonLayer - // .off(jason.a()) - // .onTrue(new ScoreCommand( elevatorSubsystem, Setpoints.GROUND_INTAKE)) + // jacobLayer + // .off(jacob.a()) + // .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... + anthony + .povUp() .onTrue( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) - .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)); - - // ground pickup - jasonLayer - .off(jason.a()) - .onTrue( - new GroundPickupCommand( elevatorSubsystem, - intakeSubsystem, () -> - jason.getHID().getPOV() == 180 - ? IntakeSubsystem.Modes.HOLD - : IntakeSubsystem.Modes.INTAKE)); + 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 IntakeModeCommand(intakeSubsystem, Modes.OFF)); + + anthony + .povLeft() + .onTrue( + new ElevatorPositionCommand( + elevatorSubsystem, () -> Constants.Elevator.Setpoints.STOWED)) + .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF)) + .onTrue(new SetZeroModeCommand(elevatorSubsystem)); + + anthony + .povDown() + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, anthony.rightBumper())); + + jacob + .a() + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.leftBumper())); + + jacobLayer + .off(jacob.povUp()) + .onTrue( + new IntakeModeCommand(intakeSubsystem, Modes.OUTTAKE) + .alongWith( + new ElevatorPositionCommand( + elevatorSubsystem, () -> Constants.Elevator.Setpoints.GROUND_INTAKE_CONE))); + + // jacob.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem)); + + jacobLayer + .off(jacob.back()) + .whileTrue( + new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.leftBumper()) + .alongWith( + new ElevatorPositionCommand( + elevatorSubsystem, + () -> + jacob.leftBumper().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))); // 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)))); + () -> currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.LOW)), + elevatorSubsystem)); - 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( () -> @@ -366,23 +403,32 @@ 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())); + // jacob.b())); + anthony.povRight())); - jasonLayer - .on(jason.x()) + anthony + .povRight() + // jacob + // .b() .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)))); + // 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)))); // control the lights currentNodeSelection.subscribe( @@ -415,21 +461,33 @@ private void setupAutonomousCommands() { final List drivingCubeOuttake = List.of( - new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_HEIGHT)).canWaitHere(), - new ScoreStep(Modes.OUTTAKE)); + new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_EXTENSION_INCHES)) + .canWaitHere(), + 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 = Map.of( "stow elevator", - new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED), + new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED), "zero everything", - new SetZeroModeCommand(elevatorSubsystem, true), - "intake", + 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), + "intake cube", 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_CUBE + : Elevator.Setpoints.SHELF_INTAKE_CUBE), // squeeze intake isn't relevant to offseason bot - leaving here just in case /*"squeeze intake", new CommandBase() { @@ -471,32 +529,31 @@ 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", - new N1_1ConePlus2CubeHybridMobilityEngage( - 4.95, 4, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem)); + // 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( + // "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.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)); + "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem)); autoSelector.addOption( "Near Substation Mobility [APRILTAG]", @@ -531,6 +588,10 @@ public void end(boolean interrupted) { 4.95, 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)); @@ -543,18 +604,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, - 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.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/autonomous/commands/N3_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java index f98fda6..e547f7a 100644 --- a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java @@ -36,17 +36,18 @@ public N3_1ConePlusMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)), + .deadlineWith( + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(NodeType.CONE.atHeight(Height.HIGH)), - 1), + 2.5), (new FollowTrajectoryCommand(path, true, drivebaseSubsystem)) .alongWith( (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 1a85ee0..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)), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, @@ -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_1Cone.java b/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java new file mode 100644 index 0000000..386140f --- /dev/null +++ b/src/main/java/frc/robot/autonomous/commands/N6_1Cone.java @@ -0,0 +1,42 @@ +// 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, () -> true)), + new ScoreCommand( + intakeSubsystem, + elevatorSubsystem, + Constants.SCORE_STEP_MAP.get(NodeType.CONE.atHeight(Height.HIGH)), + 2), + new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED) + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); + } +} 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..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)), + .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, @@ -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_1ConePlus2CubeMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java index 594104d..6118b91 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,11 +38,8 @@ public N9_1ConePlus2CubeMobility( "n9 1cone + 2cube", maxVelocityMetersPerSecond, maxAccelerationMetersPerSecondSq); addCommands( - (new SetZeroModeCommand(elevatorSubsystem) - .alongWith( - new SetZeroModeCommand( - elevatorSubsystem, Optional.of(true), Optional.of(false)))) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)), + new SetZeroModeCommand(elevatorSubsystem) + .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 f70baef..7910e6a 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java @@ -37,17 +37,18 @@ public N9_1ConePlusMobility( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)), + .deadlineWith( + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, Constants.SCORE_STEP_MAP.get(NodeType.CONE.atHeight(Height.HIGH)), - 1), + 2.5), (new FollowTrajectoryCommand(path, true, drivebaseSubsystem)) .alongWith( (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 209e12d..4758dc9 100644 --- a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java +++ b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobilityEngage.java @@ -40,7 +40,8 @@ public N9_1ConePlusMobilityEngage( addCommands( new SetZeroModeCommand(elevatorSubsystem) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)), + .deadlineWith( + new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE, () -> true)), new ScoreCommand( intakeSubsystem, elevatorSubsystem, @@ -50,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/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java index 8977b85..5283e96 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; @@ -23,21 +24,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 +49,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/ElevatorManualCommand.java b/src/main/java/frc/robot/commands/ElevatorManualCommand.java index 17ce315..3cc5602 100644 --- a/src/main/java/frc/robot/commands/ElevatorManualCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorManualCommand.java @@ -6,28 +6,49 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.Modes; import java.util.function.DoubleSupplier; 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); } // 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.setTargetHeight(elevatorSubsystem.getHeight() + rate.getAsDouble()); + // 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_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(), 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 d4ce2f0..9a48693 100644 --- a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java @@ -8,42 +8,56 @@ 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; +import java.util.function.Supplier; public class ElevatorPositionCommand extends CommandBase { private final ElevatorSubsystem elevatorSubsystem; - private double targetHeight; + private double targetExtension; private double targetAngle; + private Supplier elevatorStateSupplier; /** Creates a new ElevatorPositionCommand. */ - public ElevatorPositionCommand( - ElevatorSubsystem subsystem, double targetHeight, double targetAngle) { - this.elevatorSubsystem = subsystem; - this.targetHeight = targetHeight; - 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) { + // 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, Supplier elevatorStateSupplier) { // height and angle this.elevatorSubsystem = elevatorSubsystem; - targetHeight = elevatorState.height(); - 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.setTargetHeight(targetHeight); - elevatorSubsystem.setTargetAngle(targetAngle); - } + 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 @@ -57,9 +71,9 @@ public boolean isFinished() { elevatorSubsystem.getTargetAngle(), Elevator.ANGLE_EPSILON) && Util.epsilonEquals( - elevatorSubsystem.getHeight(), - elevatorSubsystem.getTargetHeight(), - Elevator.HEIGHT_EPSILON); + 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/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java new file mode 100644 index 0000000..43d4512 --- /dev/null +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -0,0 +1,35 @@ +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.function.BooleanSupplier; + +public class GroundIntakeElevatorCommand extends CommandBase { + + private ElevatorSubsystem elevatorSubsystem; + private BooleanSupplier isCone; + + public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanSupplier isCone) { + this.elevatorSubsystem = elevatorSubsystem; + this.isCone = isCone; + + addRequirements(this.elevatorSubsystem); + } + + @Override + public void initialize() { + elevatorSubsystem.setMode(Modes.POSITION_CONTROL); + } + + @Override + public void execute() { + 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 4639b10..80f06bd 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; -import java.util.function.Supplier; +import java.util.function.BooleanSupplier; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -17,14 +17,15 @@ public class GroundPickupCommand extends SequentialCommandGroup { /** Creates a new GroundPickupCommand. */ public GroundPickupCommand( - ElevatorSubsystem elevatorSubsystem, IntakeSubsystem intakeSubsystem, - Supplier modeSupplier) { + ElevatorSubsystem elevatorSubsystem, + BooleanSupplier isCone) { // 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)) - .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED))); + 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 3277fc0..5fd9a00 100644 --- a/src/main/java/frc/robot/commands/IntakeModeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeModeCommand.java @@ -5,19 +5,36 @@ 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; 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) { + 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; + this.mode = mode; + this.isCone = null; addRequirements(intakeSubsystem); } @@ -30,7 +47,11 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (isCone != null) { + intakeSubsystem.setIsCone(isCone.getAsBoolean()); + } + } // Called once the command ends or is interrupted. @Override @@ -39,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/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/commands/ScoreCommand.java b/src/main/java/frc/robot/commands/ScoreCommand.java index 46d3c69..4683f8f 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,16 +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()) { - // FIXME: we need a working elevatorposition command here - return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()) - .deadlineWith(new IntakeModeCommand(intakeSubsystem, intakeState.get())); + return new ElevatorPositionCommand(elevatorSubsystem, () -> elevatorState.get()) + .deadlineWith(new IntakeModeCommand(intakeSubsystem, intakeState.get(), () -> isCube)); } else if (elevatorState.isPresent()) { - - // FIXME: we need a working elevatorposition command here - return new ElevatorPositionCommand(elevatorSubsystem, elevatorState.get()); + 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/commands/SetZeroModeCommand.java b/src/main/java/frc/robot/commands/SetZeroModeCommand.java index bbc0b19..d50244c 100644 --- a/src/main/java/frc/robot/commands/SetZeroModeCommand.java +++ b/src/main/java/frc/robot/commands/SetZeroModeCommand.java @@ -6,35 +6,22 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ElevatorSubsystem; -import java.util.Optional; +import frc.robot.subsystems.ElevatorSubsystem.Modes; 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)); - } - - // FIXME make it zero elevator + wrist @Override - public void initialize() {} + public void initialize() { + elevatorSubsystem.setNotZeroed(); + elevatorSubsystem.setMode(Modes.ZERO); + } // Called every time the scheduler runs while the command is scheduled. @Override @@ -47,6 +34,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/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; - } -} diff --git a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java index 46750b7..9053018 100644 --- a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java @@ -563,13 +563,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); } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index cfa0649..cf7fca7 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -5,14 +5,14 @@ // need: have a way for elevator to go up, take in double and goes up package frc.robot.subsystems; +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; -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; @@ -22,56 +22,93 @@ /** Add your docs here. */ public class ElevatorSubsystem extends SubsystemBase { + public enum Modes { + PERCENT_CONTROL, + POSITION_CONTROL, + ZERO + } + + private Modes currentMode; + private TalonFX leftMotor; private TalonFX rightMotor; private TalonFX wristMotor; - private double currentHeight; - private double targetHeight; - private double currentAngle; + private double currentExtension; + private double targetExtension; + private double currentWristAngle; private double targetAngle; - private double statorCurrentLimit; + private double elevatorPercentControl; + private double wristPercentControl; + + private boolean elevatorZeroed; + private boolean wristZeroed; - private PIDController heightController; + // private ProfiledPIDController extensionController; private PIDController wristController; - private CANCoder canCoder; + // private CANCoder canCoder; - private final ShuffleboardTab tab = Shuffleboard.getTab("Elevator"); + private final ShuffleboardTab elevatorTab = Shuffleboard.getTab("Elevator"); - private double filterOutput; + private final ShuffleboardTab wristTab = Shuffleboard.getTab("Wrist"); - private DigitalInput proxySensor; + private double elevatorFilterOutput; + + 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 height, double angle) {} + 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); - heightController = new PIDController(0.0001, 0, 0); - wristController = new PIDController(0, 0, 0); - canCoder = new CANCoder(0); - proxySensor = new DigitalInput(0); + // extensionController = new PIDController(0.1, 0.03, 0.035); + wristController = new PIDController(0.1, 0, 0); + // canCoder = new CANCoder(Elevator.Ports.CANCODER); + // proxySensor = new DigitalInput(0); - currentHeight = 0.0; - targetHeight = 0.0; - currentAngle = 0.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; - statorCurrentLimit = 50.0; + elevatorPercentControl = 0.0; + wristPercentControl = 0.0; + + elevatorZeroed = false; + wristZeroed = false; rightMotor.configFactoryDefault(); leftMotor.configFactoryDefault(); + wristMotor.configFactoryDefault(); rightMotor.clearStickyFaults(); leftMotor.clearStickyFaults(); + wristMotor.clearStickyFaults(); + + rightMotor.setInverted(true); + leftMotor.setInverted(InvertType.FollowMaster); + // wristMotor.setInverted(false); + wristMotor.setSensorPhase(true); rightMotor.configOpenloopRamp(.5); @@ -79,81 +116,243 @@ 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( + extensionInchesToTicks(Constants.Elevator.MAX_EXTENSION_INCHES), 20); + rightMotor.configReverseSoftLimitThreshold( + extensionInchesToTicks(Elevator.MIN_EXTENSION_INCHES), 20); + 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); + + rightMotor.config_kP(0, 0.08); + rightMotor.config_kD(0, 0); + 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 + + 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()); + } - filter = LinearFilter.movingAverage(30); + public void setMode(Modes mode) { + currentMode = mode; + } - tab.addDouble("Wrist Motor Position", () -> canCoder.getAbsolutePosition()); - tab.addDouble("Wrist Target Angle", () -> targetAngle); - tab.addDouble("Wrist Current Angle", () -> currentAngle); - tab.addDouble("Elevator Target Height", () -> targetHeight); - tab.addDouble("Elevator Current Height", () -> currentHeight); + public Modes getMode() { + return currentMode; } - // FIXME: all the numbers wrong in constants - public static double heightToTicks(double height) { - return height - * ((Elevator.ELEVATOR_GEAR_RATIO * Elevator.ELEVATOR_TICKS) - / (Elevator.ELEVATOR_GEAR_CIRCUMFERENCE)); + public static double extensionInchesToTicks(double inches) { + return (Elevator.FALCON_CPR * inches) + / (Elevator.CARRIAGE_RATIO + * (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) + * Elevator.ELEVATOR_GEAR_RATIO); } - public static double ticksToHeight(double ticks) { - return (ticks * Elevator.ELEVATOR_GEAR_CIRCUMFERENCE) - / (Elevator.ELEVATOR_TICKS * Elevator.ELEVATOR_GEAR_RATIO); + public double ticksToExtensionInches(double ticks) { + return Elevator.CARRIAGE_RATIO + * (Elevator.ELEVATOR_SPROCKET_DIAMETER_INCHES * Math.PI) + * ((ticks / Elevator.FALCON_CPR) * Elevator.ELEVATOR_GEAR_RATIO); } private double getCurrentTicks() { return rightMotor.getSelectedSensorPosition(); } - public void setTargetHeight(double targetHeight) { - this.targetHeight = targetHeight; + public void setTargetExtensionInches(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 void setPercentControl(double elevatorPercentControl, double wristPercentControl) { + this.elevatorPercentControl = elevatorPercentControl; + this.wristPercentControl = wristPercentControl; + } + + public double getElevatorPercentControl() { + return elevatorPercentControl; + } + + public double getWristPercentControl() { + return wristPercentControl; + } + + public double getTargetExtension() { + return targetExtension; } public double getTargetAngle() { return targetAngle; } - public double getHeight() { - return ticksToHeight(getCurrentTicks()); + 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 / 360 * 128 * 2048; + } + + private double ticksToAngle(double ticks) { + return ticks / 2048 / 128 * 360; } public void setTargetAngle(double targetAngle) { this.targetAngle = targetAngle; } - @Override - public void periodic() { - currentHeight = getHeight(); - currentAngle = getCurrentAngleDegrees(); + public void setNotZeroed() { + elevatorZeroed = false; + wristZeroed = false; + } - if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit - || proxySensor.get() == false) { - double motorPower = heightController.calculate(currentHeight, targetHeight); - rightMotor.set(TalonFXControlMode.PercentOutput, motorPower); + private double applySlowZoneToElevatorPercent(double elevatorPercentControl) { + if ((currentExtension > (Elevator.MAX_EXTENSION_INCHES - 7)) + || (currentExtension < (Elevator.MIN_EXTENSION_INCHES + 7))) { + return MathUtil.clamp(elevatorPercentControl * 0.5, -1, 1); + } + return MathUtil.clamp(elevatorPercentControl, -1, 1); + } + + private void percentDrivePeriodic() { + 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)); + if (wristFilterOutput > Elevator.STATOR_LIMIT) { + wristMotor.set(TalonFXControlMode.PercentOutput, 0); + } else { + wristMotor.set(TalonFXControlMode.PercentOutput, MathUtil.clamp(wristPercentControl, -0.75, 0.75)); + } + } + + private void positionDrivePeriodic() { + // 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); + // } + if (elevatorFilterOutput > Elevator.STATOR_LIMIT) { + rightMotor.set(TalonFXControlMode.PercentOutput, 0); + } else { + rightMotor.set(TalonFXControlMode.MotionMagic, extensionInchesToTicks(targetExtension)); + } + + if (wristFilterOutput > Elevator.STATOR_LIMIT) { + wristMotor.set(TalonFXControlMode.PercentOutput, 0); + } else { + wristMotor.set( + TalonFXControlMode.PercentOutput, + MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.75, 0.75)); + } + } + + private void zeroPeriodic() { + + rightMotor.configForwardSoftLimitEnable(false, 20); + rightMotor.configReverseSoftLimitEnable(false, 20); + + rightMotor.set(TalonFXControlMode.PercentOutput, Elevator.ZERO_MOTOR_POWER); + + if (elevatorFilterOutput > Elevator.ZERO_STATOR_LIMIT) { + rightMotor.setSelectedSensorPosition(0); + leftMotor.setSelectedSensorPosition(0); + 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); - wristMotor.set( - TalonFXControlMode.PercentOutput, - MathUtil.clamp(wristController.calculate(currentAngle, targetAngle), -0.25, 0.25)); + wristZeroed = true; + } + } + if (wristZeroed && elevatorZeroed) { + currentMode = Modes.POSITION_CONTROL; + } + } + + @Override + public void periodic() { + currentExtension = getCurrentExtensionInches(); + currentWristAngle = getCurrentAngleDegrees(); + elevatorFilterOutput = elevatorFilter.calculate(rightMotor.getStatorCurrent()); + wristFilterOutput = wristFilter.calculate(wristMotor.getStatorCurrent()); + + switch (currentMode) { + case ZERO: + zeroPeriodic(); + break; + 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 790f738..78c932e 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 { @@ -18,19 +19,39 @@ public class IntakeSubsystem extends SubsystemBase { private ShuffleboardTab shuffleboard = Shuffleboard.getTab("Intake Subsystem"); private Modes currentIntakeMode; private LinearFilter filter; - private double statorCurrentLimit; + private double filterOutput; + private boolean isCone; + // private final TimeOfFlight coneToF, cubeToF; /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { - intakeMotor = new TalonFX(0); + intakeMotor = new TalonFX(Constants.Intake.Ports.INTAKE_MOTOR_PORT); - currentIntakeMode = Modes.OFF; + intakeMotor.configFactoryDefault(); + intakeMotor.clearStickyFaults(); intakeMotor.setNeutralMode(NeutralMode.Brake); + intakeMotor.setInverted(true); filter = LinearFilter.movingAverage(30); - shuffleboard.addDouble("Intake Motor", () -> intakeMotor.getSelectedSensorPosition()); + currentIntakeMode = Modes.OFF; + + filterOutput = 0.0; + + isCone = false; + + // 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("filter output", () -> filterOutput); + shuffleboard.addDouble("motor output", intakeMotor::getMotorOutputPercent); + shuffleboard.addBoolean("is cube intake", () -> isCone); + // shuffleboard.addDouble("coneToFInches", this::getConeToFInches); + // shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches); } public enum Modes { @@ -44,17 +65,41 @@ public void intakePeriodic(Modes mode) { switch (mode) { case INTAKE: - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_PERCENT); + if (isCone) { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_CONE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.INTAKE_CUBE_PERCENT); + } + break; case OUTTAKE: - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_PERCENT); + if (isCone) { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_CONE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.OUTTAKE_CUBE_PERCENT); + } + break; case HOLD: - intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_PERCENT); + if (isCone) { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_CONE_PERCENT); + } else { + intakeMotor.set(TalonFXControlMode.PercentOutput, Intake.HOLD_CUBE_PERCENT); + } + break; case OFF: default: intakeMotor.set(TalonFXControlMode.PercentOutput, 0); + break; } } + // private double getCubeToFInches() { + // return cubeToF.getRange() / 25.4; + // } + + // private double getConeToFInches() { + // return coneToF.getRange() / 25.4; + // } + public Modes getMode() { return currentIntakeMode; } @@ -63,9 +108,24 @@ public void setMode(Modes mode) { currentIntakeMode = mode; } + public void setIsCone(boolean isCone) { + this.isCone = isCone; + } + + public double getFilterOutput() { + return filterOutput; + } + + private double getFilterCalculatedValue() { + return filter.calculate(intakeMotor.getStatorCurrent()); + } + @Override public void periodic() { - if (filter.calculate(intakeMotor.getStatorCurrent()) >= statorCurrentLimit) { + filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); + if (isCone && filterOutput >= Intake.CONE_STATOR_LIMIT) { + currentIntakeMode = Modes.HOLD; + } else if (filterOutput >= Intake.CUBE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; }