diff --git a/src/main/java/team3647/frc2024/auto/AutoCommands.java b/src/main/java/team3647/frc2024/auto/AutoCommands.java index f2034e6..3a0084c 100644 --- a/src/main/java/team3647/frc2024/auto/AutoCommands.java +++ b/src/main/java/team3647/frc2024/auto/AutoCommands.java @@ -1,16 +1,5 @@ package team3647.frc2024.auto; -import java.util.ArrayList; -import java.util.List; -import java.util.Map; -import java.util.Set; -import java.util.function.BooleanSupplier; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import com.choreo.lib.Choreo; import com.choreo.lib.ChoreoControlFunction; import com.choreo.lib.ChoreoTrajectory; @@ -18,7 +7,6 @@ import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.PathPlannerLogging; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -33,6 +21,15 @@ import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SelectCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; +import java.util.Set; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; import team3647.frc2024.constants.AutoConstants; import team3647.frc2024.constants.FieldConstants; import team3647.frc2024.subsystems.Superstructure; @@ -96,13 +93,10 @@ public class AutoCommands implements AllianceObserver { private final String shoot2_to_f5 = "shoot2 to f5"; private final String f4_to_shoot2 = "f4 to shoot2"; - private final String s15_straight_forward = "s15 straight forward"; private final String trap_test = "trap test 2"; private final String s3_preload_move = "s3 preload and move"; - - public final Trigger currentYes; private boolean hasPiece = true; @@ -298,17 +292,25 @@ public AutoCommands( this.blueRightTest = new AutonomousMode(testRight(), getInitial("test right"), "blue right test"); this.redTest = - new AutonomousMode(testForward(), AllianceFlip.flipForPP(getInitial(s15_straight_forward))); - this.doNothing = - new AutonomousMode(Commands.none(), AllianceFlip.flipForPP(getInitial(s15_straight_forward), color == Alliance.Red)); - this.redPreloadOnly = - new AutonomousMode(preloadOnly(), AllianceFlip.flipForPP(getInitial(s1_to_n1)), "red preload only"); - this.bluePreloadOnly = + new AutonomousMode( + testForward(), AllianceFlip.flipForPP(getInitial(s15_straight_forward))); + this.doNothing = + new AutonomousMode( + Commands.none(), + AllianceFlip.flipForPP( + getInitial(s15_straight_forward), color == Alliance.Red)); + this.redPreloadOnly = + new AutonomousMode( + preloadOnly(), + AllianceFlip.flipForPP(getInitial(s1_to_n1)), + "red preload only"); + this.bluePreloadOnly = new AutonomousMode(preloadOnly(), getInitial(s1_to_n1), "blue preload only"); - this.preloadMove = - new AutonomousMode(preloadMove(), AllianceFlip.flipForPP(getInitial(s3_preload_move), color == Alliance.Red)); - - + this.preloadMove = + new AutonomousMode( + preloadMove(), + AllianceFlip.flipForPP(getInitial(s3_preload_move), color == Alliance.Red)); + redAutoModes = new ArrayList( Set.of( @@ -335,7 +337,6 @@ public AutoCommands( blueForwardTest, blueRightTest, doNothing)); - } public enum MidlineNote { @@ -463,19 +464,13 @@ public Command fullCenterS1(Alliance color) { .repeatedly())); } - public Command preloadOnly(){ - return Commands.parallel( - target().withTimeout(10), - scorePreload() - ); + public Command preloadOnly() { + return Commands.parallel(target().withTimeout(10), scorePreload()); } - public Command preloadMove(){ + public Command preloadMove() { return Commands.sequence( - scorePreload(), - Commands.waitSeconds(2), - followChoreoPath(s3_preload_move, color) - ); + scorePreload(), Commands.waitSeconds(2), followChoreoPath(s3_preload_move, color)); } public Command fullCenterS3(Alliance color) { @@ -728,12 +723,12 @@ public Command poopPreload() { .withTimeout(1.4); } - public Command scorePreload(){ + public Command scorePreload() { return Commands.parallel( superstructure.spinUp(), superstructure.prep(), Commands.sequence(Commands.waitSeconds(1), superstructure.feed())) - .withTimeout(1.4); + .withTimeout(1.4); } public Pose2d getInitial(String path) { @@ -928,32 +923,35 @@ public Command followChoreoPathWithOverrideNoverrideFast(String path, Alliance c AutoConstants.kXController, AutoConstants.kYController, AutoConstants.kRotController), - (ChassisSpeeds speeds) -> - { - var motionRotComponent = deeTheta(); - var motionXComponent = speeds.vxMetersPerSecond; - var motionYComponent = speeds.vyMetersPerSecond; - - var posexthresholdlow = color == Alliance.Blue ? 5 : FieldConstants.kFieldLength - 8.75; - var posexThresholdHigh = color == Alliance.Blue ? 8.75 : FieldConstants.kFieldLength - 5; - - var isInPose = swerve.getOdoPose().getX() > posexthresholdlow && swerve.getOdoPose().getX() < posexThresholdHigh; - - if(!this.hasPiece && hasTarget.getAsBoolean() && isInPose){ - motionXComponent = autoDriveVelocities.get().dx; - motionYComponent = autoDriveVelocities.get().dy; - motionRotComponent = autoDriveVelocities.get().dtheta; - } - - if(isInPose){ - motionRotComponent = speeds.omegaRadiansPerSecond; - } - swerve.drive(motionXComponent, motionYComponent, motionRotComponent);}, + (ChassisSpeeds speeds) -> { + var motionRotComponent = deeTheta(); + var motionXComponent = speeds.vxMetersPerSecond; + var motionYComponent = speeds.vyMetersPerSecond; + + var posexthresholdlow = + color == Alliance.Blue ? 5 : FieldConstants.kFieldLength - 8.75; + var posexThresholdHigh = + color == Alliance.Blue ? 8.75 : FieldConstants.kFieldLength - 5; + + var isInPose = + swerve.getOdoPose().getX() > posexthresholdlow + && swerve.getOdoPose().getX() < posexThresholdHigh; + + if (!this.hasPiece && hasTarget.getAsBoolean() && isInPose) { + motionXComponent = autoDriveVelocities.get().dx; + motionYComponent = autoDriveVelocities.get().dy; + motionRotComponent = autoDriveVelocities.get().dtheta; + } + + if (isInPose) { + motionRotComponent = speeds.omegaRadiansPerSecond; + } + swerve.drive(motionXComponent, motionYComponent, motionRotComponent); + }, () -> mirror) .andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve)); } - public Command followChoreoPathWithOverrideLongTimer(String path, Alliance color) { ChoreoTrajectory traj = Choreo.getTrajectory(path); boolean mirror = color == Alliance.Red; @@ -978,6 +976,7 @@ public Command followChoreoPathWithOverrideLongTimer(String path, Alliance color () -> mirror) .andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve)); } + public Command customChoreoFolloweForOverride( ChoreoTrajectory trajectory, Supplier poseSupplier, @@ -1052,7 +1051,7 @@ public Command customChoreoFolloweForOverrideLongTimer( outputChassisSpeeds.accept(new ChassisSpeeds()); }, () -> - timer.hasElapsed(0.9)//gives the note more time to get in the intake; + timer.hasElapsed(0.9) // gives the note more time to get in the intake; && ((swerve.getVel() < 0.2 & swerve.getOdoPose() .minus( @@ -1072,7 +1071,6 @@ public Command customChoreoFolloweForOverrideLongTimer( swerve); } - public Command customChoreoFolloweForOverrideSlow( ChoreoTrajectory trajectory, Supplier poseSupplier, diff --git a/src/main/java/team3647/frc2024/constants/PivotConstants.java b/src/main/java/team3647/frc2024/constants/PivotConstants.java index ce3a082..981f774 100644 --- a/src/main/java/team3647/frc2024/constants/PivotConstants.java +++ b/src/main/java/team3647/frc2024/constants/PivotConstants.java @@ -11,7 +11,6 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.playingwithfusion.TimeOfFlight; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; @@ -78,8 +77,7 @@ public class PivotConstants { public static final InterpolatingDoubleTreeMap kMasterTrapMap = new InterpolatingDoubleTreeMap(); - public static final InterpolatingDoubleTreeMap feedmap = - new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap feedmap = new InterpolatingDoubleTreeMap(); static { // kMasterSpeakerMap.put(0.0, 60.0); diff --git a/src/main/java/team3647/frc2024/robot/Robot.java b/src/main/java/team3647/frc2024/robot/Robot.java index 767516a..6276431 100644 --- a/src/main/java/team3647/frc2024/robot/Robot.java +++ b/src/main/java/team3647/frc2024/robot/Robot.java @@ -6,8 +6,8 @@ import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java index 0204a78..d1d6349 100644 --- a/src/main/java/team3647/frc2024/robot/RobotContainer.java +++ b/src/main/java/team3647/frc2024/robot/RobotContainer.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.function.BooleanSupplier; import team3647.frc2024.auto.AutoCommands; import team3647.frc2024.commands.ClimbCommands; import team3647.frc2024.commands.DrivetrainCommands; @@ -247,11 +246,12 @@ private void configureButtonBindings() { climbing.onTrue(superstructure.setIsClimbing()); climbing.onFalse(superstructure.setIsNotClimbing()); - coController.leftJoyStickPress.and(coController.rightJoyStickPress) - .onTrue(superstructure.stowChurro()); + coController + .leftJoyStickPress + .and(coController.rightJoyStickPress) + .onTrue(superstructure.stowChurro()); - coController.leftTrigger.and(coController.rightTrigger) - .onTrue(superstructure.stowAll()); + coController.leftTrigger.and(coController.rightTrigger).onTrue(superstructure.stowAll()); // characterization @@ -312,7 +312,7 @@ public void teleopInit() {} void configTestCommands() {} public void configureSmartDashboardLogging() { - //logging offset for interp + // logging offset for interp printer.addDouble("offset", targetingUtil::getOffset); printer.addBoolean("note seen", () -> !autoCommands.noteNotSeen.getAsBoolean()); // SmartDashboard.putNumber("pivot output", 0); @@ -407,11 +407,14 @@ public Command getAutonomousCommand() { public final Intake intake = new Intake(IntakeConstants.kMaster, 1, 1, IntakeConstants.kNominalVoltage, 0.02); private final Trigger isIntaking = - new Trigger(() -> mainController.leftBumper.getAsBoolean() && !DriverStation.isAutonomous()); + new Trigger( + () -> + mainController.leftBumper.getAsBoolean() + && !DriverStation.isAutonomous()); - private final Trigger isNotIntaking = + private final Trigger isNotIntaking = new Trigger(() -> !isIntaking.getAsBoolean()).debounce(0.1); - + public final Pivot pivot = new Pivot( PivotConstants.kMaster, @@ -484,8 +487,6 @@ public Command getAutonomousCommand() { new AprilTagPhotonVision(VisionConstants.zoom, VisionConstants.robotToZoom) .withPriority(true); - - public AllianceChecker allianceChecker = new AllianceChecker(); private final VisionController visionController = @@ -566,8 +567,7 @@ public Command getAutonomousCommand() { private final Trigger piece = new Trigger(() -> superstructure.getPiece()); - private final Trigger climbing = - new Trigger(() -> climbRight.getPosition() > 1); + private final Trigger climbing = new Trigger(() -> climbRight.getPosition() > 1); private final Trigger goodToAmp = new Trigger( @@ -583,6 +583,4 @@ public Command getAutonomousCommand() { new Trigger(() -> superstructure.current() && wrist.getAngle() < 5) // 41 .debounce(0.06) .or(mainController.buttonB); - - } diff --git a/src/main/java/team3647/frc2024/subsystems/ShooterRight.java b/src/main/java/team3647/frc2024/subsystems/ShooterRight.java index d57ac3f..5af5675 100644 --- a/src/main/java/team3647/frc2024/subsystems/ShooterRight.java +++ b/src/main/java/team3647/frc2024/subsystems/ShooterRight.java @@ -1,10 +1,8 @@ package team3647.frc2024.subsystems; - -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import org.littletonrobotics.junction.Logger; import team3647.lib.TalonFXSubsystem; public class ShooterRight extends TalonFXSubsystem { @@ -24,13 +22,13 @@ public ShooterRight( this.ff = ff; } - @Override public void periodic() { // TODO Auto-generated method stub super.periodic(); Logger.recordOutput("realrightshootdemand", velocityVoltage.Velocity); } + public boolean velocityGreater(double setpoint) { return super.getVelocity() > setpoint; } diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java index 5895ebb..da27c4c 100644 --- a/src/main/java/team3647/frc2024/subsystems/Superstructure.java +++ b/src/main/java/team3647/frc2024/subsystems/Superstructure.java @@ -42,7 +42,7 @@ public class Superstructure { private final double churroStowAngle = ChurroConstants.kInitialDegree; private final double shootSpeed; public double currentLimit = 49; - + ; private boolean hasPiece = false; private boolean isClimbing; @@ -169,7 +169,7 @@ public Command spinUpAmp() { } public Command setShootModeStationary() { - return Commands.runOnce(() -> this.wantedShootingMode= DriveMode.SHOOT_STATIONARY); + return Commands.runOnce(() -> this.wantedShootingMode = DriveMode.SHOOT_STATIONARY); } public Command setShootModeMoving() { @@ -204,19 +204,17 @@ public boolean getPiece() { return hasPiece; } - public Command stowAll(){ + public Command stowAll() { return Commands.parallel( - wristCommands + wristCommands .setAngle(wristStowAngle) .until(() -> wrist.angleReached(wristStowAngle, 5)), Commands.parallel( - pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()), - shooterCommands.kill(), - kickerCommands.kill(), - intakeCommands.kill(), - stowChurro()) - - ); + pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()), + shooterCommands.kill(), + kickerCommands.kill(), + intakeCommands.kill(), + stowChurro())); } public boolean currentYes() { @@ -249,20 +247,20 @@ public boolean readyForShot() { public Command shoot() { return Commands.parallel( - prep(), spinUp() - // Commands.sequence( - // // Commands.waitSeconds(2.5), - // Commands.waitUntil( - // () -> - // shooterLeft.velocityReached(30, 2) - // && pivot.angleReached( - // - // pivotAngleSupplier.getAsDouble(), - // 5) - // && swerveAimed.getAsBoolean()) - // .withTimeout(1.2), - // feed()) - ); + prep(), spinUp() + // Commands.sequence( + // // Commands.waitSeconds(2.5), + // Commands.waitUntil( + // () -> + // shooterLeft.velocityReached(30, 2) + // && pivot.angleReached( + // + // pivotAngleSupplier.getAsDouble(), + // 5) + // && swerveAimed.getAsBoolean()) + // .withTimeout(1.2), + // feed()) + ); } public Command cleanShoot() { diff --git a/src/main/java/team3647/frc2024/util/RobotTracker.java b/src/main/java/team3647/frc2024/util/RobotTracker.java index e58de78..9aa94d9 100644 --- a/src/main/java/team3647/frc2024/util/RobotTracker.java +++ b/src/main/java/team3647/frc2024/util/RobotTracker.java @@ -172,24 +172,13 @@ public Pose2d getFeed() { @Override public void onAllianceFound(Alliance color) { //remove debug stmts - DriverStation.reportError("COLOLOR" + color.name(), false); - - DriverStation.reportError("SAMECHECK??" + this.color.equals(color), false); - DriverStation.reportError("SPEAKERPOSE " + speakerPose.getX(), false); - if(this.color == color){ - return; - } - DriverStation.reportError("PASSED SAME CHECK", false); - - speakerPose = AllianceFlip.flipForPP(speakerPose); - DriverStation.reportError("speaker flipped", false); - ampPose = AllianceFlip.flipForPP(ampPose); - DriverStation.reportError("amp flipped", false); - feedPose = AllianceFlip.flipForPP(feedPose); - DriverStation.reportError("feed flipped", false); - - - + this.color = color; + + speakerPose = AllianceFlip.flipForPP(speakerPose/*,this.color == color*/); + + ampPose = AllianceFlip.flipForPP(ampPose); + + feedPose = AllianceFlip.flipForPP(feedPose); } } diff --git a/src/main/java/team3647/frc2024/util/TargetingUtil.java b/src/main/java/team3647/frc2024/util/TargetingUtil.java index 26fdfa6..0ab97c5 100644 --- a/src/main/java/team3647/frc2024/util/TargetingUtil.java +++ b/src/main/java/team3647/frc2024/util/TargetingUtil.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import team3647.frc2024.constants.PivotConstants; -import team3647.frc2024.subsystems.Pivot; public class TargetingUtil { @@ -104,7 +103,8 @@ public AimingParameters feedOnTheMove(Pose2d pose) { Pose2d shotPose = robotTracker.compensateFeed(robotTracker.getPose()); double pivotAngle = Math.toRadians( - PivotConstants.feedmap.get(robotTracker.getDistanceFromSpeaker(shotPose)) + offset); + PivotConstants.feedmap.get(robotTracker.getDistanceFromSpeaker(shotPose)) + + offset); final var toPose = shotPose.minus(pose).getTranslation(); double angle = Math.acos(toPose.getX() / toPose.getNorm()) diff --git a/src/main/java/team3647/lib/inputs/ButtonHandler.java b/src/main/java/team3647/lib/inputs/ButtonHandler.java index 568d1e6..e0e0c8d 100644 --- a/src/main/java/team3647/lib/inputs/ButtonHandler.java +++ b/src/main/java/team3647/lib/inputs/ButtonHandler.java @@ -24,7 +24,6 @@ // public static ArrayList presses = new ArrayList(10); // public static ButtonHandler instance = new ButtonHandler(); - // public ButtonHandler(){ // super(); // } @@ -38,24 +37,22 @@ // } // public Command addPress(String name){ -// return Commands.runOnce(() -> presses.add(new ButtonPress(name, Timer.getFPGATimestamp()))); +// return Commands.runOnce(() -> presses.add(new ButtonPress(name, +// Timer.getFPGATimestamp()))); // } - - // public Boolean getIsDoubleTap(ButtonPress press){ // for(ButtonPress p : presses){ // if(p.name.equals(press.name) && Math.abs(p.timestamp - press.timestamp) < 0.35){ // Logger.recordOutput("string", Math.abs(p.timestamp - press.timestamp)); // return true; - + // } // } // return false; // } - // public void periodic() { // // if(Timer.getFPGATimestamp() - cacheTimer >= 2.0){ // // presses.clear(); @@ -67,15 +64,14 @@ // DriverStation.reportError(String.valueOf(p.timestamp), false); // DriverStation.reportError("types hit", false); // DriverStation.reportError(getIsDoubleTap(p).toString(), false); - + // } // DriverStation.reportError("looping",false ); - + // } // public static ButtonHandler getInstance(){ // return instance; // } - - + // } diff --git a/src/main/java/team3647/lib/team9442/AllianceChecker.java b/src/main/java/team3647/lib/team9442/AllianceChecker.java index f69be8e..4d10301 100644 --- a/src/main/java/team3647/lib/team9442/AllianceChecker.java +++ b/src/main/java/team3647/lib/team9442/AllianceChecker.java @@ -11,6 +11,7 @@ public class AllianceChecker { private final List observers = new ArrayList<>(); private Optional alliance = DriverStation.getAlliance(); + private Alliance cachedColor = Alliance.Red; public void registerObserver(AllianceObserver observer) { @@ -26,6 +27,12 @@ public void registerObservers(AllianceObserver... addObservers) { public void periodic() { alliance = DriverStation.getAlliance(); - alliance.ifPresent(color -> observers.forEach(observer -> observer.onAllianceFound(color))); + alliance.ifPresent(color -> { + // DriverStation.reportError("Run method? " + (cachedColor != color), false); + if(cachedColor != color){ + observers.forEach(observer -> observer.onAllianceFound(color)); + } + cachedColor = color; + }); } } diff --git a/src/main/java/team3647/lib/team9442/AutoChooser.java b/src/main/java/team3647/lib/team9442/AutoChooser.java index 8f46a3f..64e5304 100644 --- a/src/main/java/team3647/lib/team9442/AutoChooser.java +++ b/src/main/java/team3647/lib/team9442/AutoChooser.java @@ -1,16 +1,12 @@ package team3647.lib.team9442; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import java.util.List; import java.util.function.Consumer; - -import org.littletonrobotics.junction.inputs.LoggedDriverStation.DriverStationInputs; - import team3647.frc2024.auto.AutoCommands; import team3647.frc2024.auto.AutonomousMode; @@ -37,11 +33,10 @@ public AutoChooser(AutoCommands commands, Consumer setStartPose) { @Override public void onAllianceFound(Alliance color) { - if(color != chachedColor){ + if (color != chachedColor) { setStartPose.accept(getSelected().getPathplannerPose2d()); } - - + autosList = color == Alliance.Blue ? autoCommands.blueAutoModes : autoCommands.redAutoModes; addAutos(); chachedColor = color;