diff --git a/simgui.json b/simgui.json index a3ebfcfa..01d8bb0a 100644 --- a/simgui.json +++ b/simgui.json @@ -19,6 +19,7 @@ "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/Visualizers/Climber": "Mechanism2d", "/SmartDashboard/Visualizers/Intake": "Mechanism2d", "/SmartDashboard/Visualizers/Lift": "Mechanism2d" diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 67c36bb9..9115cf99 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -50,6 +50,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; @@ -148,8 +149,9 @@ private void configureDriverBindings() { // note to amper and align then score driver.getLeftBumper() - .whileTrue(new AmpScoreRoutine()) - .onFalse(new AmperStop()) + .onTrue(new ConveyorToAmpUnsafe()) // requirements: conveyor, intake + .whileTrue(new AmpScoreRoutine()) // requirements: swerve, amper + // .onFalse(new ConditionalCommand(new AmperStop(), new InstantCommand(), () -> !Amper.getInstance().hasNote())) .onFalse(new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT)); // score speaker no align @@ -211,7 +213,15 @@ private void configureDriverBindings() { // .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))); driver.getTopButton() - .whileTrue(new SwerveDriveAutoFerry(driver)); + .onTrue(new ShooterSetRPM(Settings.Shooter.FERRY)) + .whileTrue(new SwerveDriveToFerry() + .deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN) + .andThen(new ShooterWaitForTarget() + .withTimeout(0.5))) + .andThen(new ConveyorShoot())) + .onFalse(new ConveyorStop()) + .onFalse(new IntakeStop()) + .onFalse(new ShooterSetRPM(Settings.Shooter.PODIUM_SHOT)); // climb driver.getRightButton() diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java index 9ba14b32..9c7e097d 100644 --- a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java @@ -8,6 +8,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.commands.amper.AmperScore; +import com.stuypulse.robot.commands.amper.AmperScoreThenStop; import com.stuypulse.robot.commands.amper.AmperToHeight; import com.stuypulse.robot.commands.conveyor.ConveyorToAmp; import com.stuypulse.robot.commands.leds.LEDSet; @@ -20,6 +21,7 @@ import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Amper.Lift; import com.stuypulse.robot.constants.Settings.Amper.Score; +import com.stuypulse.robot.subsystems.amper.Amper; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.stuylib.math.Vector2D; @@ -31,6 +33,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class AmpScoreRoutine extends SequentialCommandGroup { @@ -57,7 +60,7 @@ private static Pose2d getTargetPose(double distanceToWall) { public AmpScoreRoutine() { addCommands( new ParallelCommandGroup( - new ConveyorToAmp(), + new WaitUntilCommand(() -> Amper.getInstance().hasNote()), new SwerveDriveToPose(() -> getTargetPose(Alignment.AMP_WALL_SETUP_DISTANCE.get())) .withTolerance(AMP_WALL_SETUP_X_TOLERANCE, AMP_WALL_SETUP_Y_TOLERANCE, AMP_WALL_SETUP_ANGLE_TOLERANCE) .deadlineWith(new LEDSet(LEDInstructions.AMP_ALIGN)) @@ -75,7 +78,7 @@ public AmpScoreRoutine() { .deadlineWith(new LEDSet(LEDInstructions.AMP_SCORE)) ), - AmperScore.untilDone(), + AmperScoreThenStop.scoreThenStop(), new WaitCommand(0.25), diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.java new file mode 100644 index 00000000..1994752d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.java @@ -0,0 +1,37 @@ + +package com.stuypulse.robot.commands.amper; + +import com.stuypulse.robot.subsystems.amper.Amper; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AmperScoreThenStop extends InstantCommand { + + private final Amper amper; + + public static Command scoreThenStop() { + return new AmperScoreThenStop(); + } + + public AmperScoreThenStop() { + amper = Amper.getInstance(); + addRequirements(amper); + } + + @Override + public void initialize() { + amper.amp(); + } + + @Override + public boolean isFinished() { + return !amper.hasNote(); + } + + @Override + public void end(boolean interrupted) { + amper.stopRoller(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.java new file mode 100644 index 00000000..0d3e9aa5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.java @@ -0,0 +1,65 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.commands.conveyor; + +import com.stuypulse.robot.commands.amper.AmperToHeight; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Amper.Lift; +import com.stuypulse.robot.subsystems.amper.Amper; +import com.stuypulse.robot.subsystems.conveyor.Conveyor; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ConveyorToAmpUnsafe extends Command { + public static Command withCheckLift() { + return AmperToHeight.untilBelow(Lift.MIN_HEIGHT, Lift.MAX_HEIGHT_ERROR) + .andThen(new ConveyorToAmp()); + } + + private final Conveyor conveyor; + private final Shooter shooter; + private final Intake intake; + private final Amper amper; + + public ConveyorToAmpUnsafe() { + conveyor = Conveyor.getInstance(); + shooter = Shooter.getInstance(); + intake = Intake.getInstance(); + amper = Amper.getInstance(); + + addRequirements(conveyor, intake); + } + + @Override + public void initialize() { + shooter.setTargetSpeeds(Settings.Shooter.HANDOFF); + } + + @Override + public void execute() { + if (shooter.atTargetSpeeds()) { + conveyor.toAmp(); + intake.acquire(); + amper.fromConveyor(); + } + } + + @Override + public boolean isFinished() { + return amper.hasNote(); + } + + @Override + public void end(boolean interrupted) { + conveyor.stop(); + // shooter.stop(); + intake.stop(); + amper.stopRoller(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java new file mode 100644 index 00000000..c035afbf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java @@ -0,0 +1,41 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.commands.swerve; + + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.odometry.Odometry; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveDriveToFerry extends SwerveDriveToPose { + + public SwerveDriveToFerry() { + super(() -> { + Pose2d robot = Odometry.getInstance().getPose(); + + Translation2d target = Robot.isBlue() + ? new Translation2d(0, Field.WIDTH - 1.5) + : new Translation2d(0, 1.5); + + Rotation2d targetAngle = new Translation2d(Field.CONST_FERRY_X, robot.getY()) + .minus(target) + .getAngle(); + + // use robot's y, put in x and rotation + + return new Pose2d( + Field.CONST_FERRY_X, + Robot.isBlue() ? Math.max(robot.getY(), Field.FERRY_CUTOFF) : Math.min(robot.getY(), Field.WIDTH - Field.FERRY_CUTOFF), + targetAngle + ); + }); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java index ca2db889..305b3169 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -13,7 +13,6 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; import com.stuypulse.stuylib.streams.numbers.IStream; -import com.stuypulse.stuylib.streams.numbers.filters.Derivative; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.pathplanner.lib.util.PIDConstants; import com.stuypulse.robot.constants.Field; diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 2f92da19..b6677aab 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -282,6 +282,9 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria double FERRY_SHOT_MIN_X = 6.0; double FERRY_SHOT_MIN_FAR_X = 8.5; + double CONST_FERRY_X = (FERRY_SHOT_MAX_X + FERRY_SHOT_MIN_FAR_X) / 2.0; + double FERRY_CUTOFF = 1.8; + /**** EMPTY FIELD POSES ****/ Pose2d EMPTY_FIELD_POSE2D = new Pose2d(new Translation2d(-1, -1), new Rotation2d()); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 70ab23a3..fbceec41 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -400,11 +400,12 @@ public interface Alignment { SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); + SmartNumber FERRY_SHOT_DISTANCE = new SmartNumber("Shooter/Ferry Shot Distance", 7.0); SmartNumber PODIUM_SHOT_DISTANCE = new SmartNumber("Shooter/Podium Distance", 2.85); // 2.75 in lab double PODIUM_SHOT_MAX_ANGLE = 80; SmartNumber AMP_WALL_SETUP_DISTANCE = new SmartNumber("Alignment/Amp/Setup Pose Distance to Wall", Units.inchesToMeters(25.5)); - SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5)); + SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.5)); // NOTE: remove "- 1.5" for battlecry SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(21.0)); SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0));