diff --git a/README.md b/README.md index 7771185..f904af9 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Introducing Da-Pinchi, the Robolancer's 2024 Crescendo bot -## 2024 WORLD CHAMPIONS +## 2024 WORLD CHAMPIONS *Some notes on our code* @@ -8,5 +8,3 @@ - PathPlanner for making auto paths (including the legendary chaos autos) - We have modes for differentiating between climb and shooting - We have lobsters crawling in our code - - diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 864e53b..a08d91b 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -5,7 +5,6 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; - import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.simulation.AddressableLEDSim; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -25,8 +24,8 @@ import org.robolancers321.Constants.PivotConstants; import org.robolancers321.Constants.RetractorConstants; import org.robolancers321.Constants.RetractorConstants.RetractorSetpoint; +import org.robolancers321.commands.AutoCommands.PathAndRetract; import org.robolancers321.commands.AutoPickupNote; -import org.robolancers321.commands.AutoScoreTrap; import org.robolancers321.commands.EmergencyCancel; import org.robolancers321.commands.FeederShot; import org.robolancers321.commands.IntakeNote; @@ -50,7 +49,6 @@ import org.robolancers321.commands.ScoreSpeakerFixedTeleop; import org.robolancers321.commands.ScoreSpeakerFromDistance; import org.robolancers321.commands.Shift; -import org.robolancers321.commands.AutoCommands.PathAndRetract; import org.robolancers321.subsystems.Climber; import org.robolancers321.subsystems.LED.LED; import org.robolancers321.subsystems.LED.LED.Section; @@ -156,8 +154,7 @@ private void configureLEDs() { LED.registerSignal( 6, - () -> - this.retractor.getGoal() == RetractorConstants.RetractorSetpoint.kAmp.angle, + () -> this.retractor.getGoal() == RetractorConstants.RetractorSetpoint.kAmp.angle, LED.solid(Section.FULL, new Color(20, 0, 50))); } @@ -283,9 +280,7 @@ private void configureDriverController() { private void configureManipulatorController() { new Trigger(this.manipulatorController::getBButton) - .onTrue((new Mate() - .andThen(new Shift()) - .unless(() -> climbing))); + .onTrue((new Mate().andThen(new Shift()).unless(() -> climbing))); // .onlyIf(this.sucker::noteDetected) @@ -308,16 +303,16 @@ private void configureManipulatorController() { .and(() -> !climbing) .onFalse( new SequentialCommandGroup( - this.retractor.moveToSpeaker(), - new ParallelDeadlineGroup( - (new WaitUntilCommand(this.indexer::exitBeamBroken) - .andThen(new WaitUntilCommand(this.indexer::exitBeamNotBroken)) - .andThen(new WaitCommand(0.1))) - .withTimeout(1.0), - this.indexer.outtake(), - this.sucker.out(), - Commands.idle(this.pivot, this.flywheel)) - ).unless(() -> climbing)); + this.retractor.moveToSpeaker(), + new ParallelDeadlineGroup( + (new WaitUntilCommand(this.indexer::exitBeamBroken) + .andThen(new WaitUntilCommand(this.indexer::exitBeamNotBroken)) + .andThen(new WaitCommand(0.1))) + .withTimeout(1.0), + this.indexer.outtake(), + this.sucker.out(), + Commands.idle(this.pivot, this.flywheel))) + .unless(() -> climbing)); new Trigger(this.manipulatorController::getLeftBumper).onTrue(toggleClimbingMode()); @@ -428,13 +423,17 @@ private void configureAuto() { Skip - pickup center note first then go back for front note */ - this.autoChooser.addOption("Do Nothing", + this.autoChooser.addOption( + "Do Nothing", new InstantCommand( () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees()))); this.autoChooser.setDefaultOption("Score And Sit", new ScoreAndSit()); - this.autoChooser.addOption("TESTING DONT USE", new InstantCommand( - () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())).andThen(new PathAndRetract(PathPlannerPath.fromPathFile("Bruh")))); + this.autoChooser.addOption( + "TESTING DONT USE", + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())) + .andThen(new PathAndRetract(PathPlannerPath.fromPathFile("Bruh")))); // this.autoChooser.addOption("4NT Sweep", new Auto4NTSweep()); // this.autoChooser.addOption("4NT Close", new Auto4NTClose()); @@ -465,7 +464,7 @@ private void configureAuto() { this.autoChooser.addOption("3 piece bottom center only", new ThreeBotCenter()); this.autoChooser.addOption("3 piece bottom center only second note", new ThreeBotCenterAlt()); - + this.autoChooser.addOption("top chaos", new TopDisrupt()); this.autoChooser.addOption("bottom chaos", new BotDisrupt()); diff --git a/src/main/java/org/robolancers321/commands/ScoreAmpIntake.java b/src/main/java/org/robolancers321/commands/ScoreAmpIntake.java index c742951..980f337 100644 --- a/src/main/java/org/robolancers321/commands/ScoreAmpIntake.java +++ b/src/main/java/org/robolancers321/commands/ScoreAmpIntake.java @@ -1,12 +1,8 @@ /* (C) Robolancers 2024 */ package org.robolancers321.commands; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; - import org.robolancers321.subsystems.intake.Retractor; import org.robolancers321.subsystems.intake.Sucker; @@ -19,11 +15,14 @@ public ScoreAmpIntake() { this.sucker = Sucker.getInstance(); this.addCommands( - new WaitCommand(3.0), - this.retractor.moveToAmp().until(() -> this.retractor.atGoalTimed(0.5)).andThen(this.sucker.ampShot().withTimeout(0.4)) + new WaitCommand(3.0), + this.retractor + .moveToAmp() + .until(() -> this.retractor.atGoalTimed(0.5)) + .andThen(this.sucker.ampShot().withTimeout(0.4)) - // new ParallelDeadlineGroup( - // new WaitCommand(0.4), this.sucker.ampShot(), Commands.idle(this.retractor)) - ); + // new ParallelDeadlineGroup( + // new WaitCommand(0.4), this.sucker.ampShot(), Commands.idle(this.retractor)) + ); } } diff --git a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java index 5a97a60..cd05628 100644 --- a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java +++ b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java @@ -29,9 +29,10 @@ public ScoreSpeakerFixedAuto() { this.addCommands( new ParallelCommandGroup( - this.flywheel.revSpeaker(), - this.retractor.moveToSpeaker(), - this.pivot.aimAtSpeakerFixed()).withTimeout(3.0), + this.flywheel.revSpeaker(), + this.retractor.moveToSpeaker(), + this.pivot.aimAtSpeakerFixed()) + .withTimeout(3.0), new ParallelDeadlineGroup( (new WaitUntilCommand(this.indexer::exitBeamBroken) .andThen(new WaitUntilCommand(this.indexer::exitBeamNotBroken)) diff --git a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedTeleop.java b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedTeleop.java index 1398306..be5c7f4 100644 --- a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedTeleop.java +++ b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedTeleop.java @@ -23,7 +23,8 @@ public ScoreSpeakerFixedTeleop() { this.addCommands( flywheel.revSpeaker(), - // retractor.moveToSpeaker(), // ! allows intake while revving, potentially releases at the incorrect angle + // retractor.moveToSpeaker(), // ! allows intake while revving, potentially releases at the + // incorrect angle pivot.aimAtSpeakerFixed(), Commands.idle()); } diff --git a/src/main/java/org/robolancers321/commands/TrapShot.java b/src/main/java/org/robolancers321/commands/TrapShot.java index 7878372..0f74059 100644 --- a/src/main/java/org/robolancers321/commands/TrapShot.java +++ b/src/main/java/org/robolancers321/commands/TrapShot.java @@ -31,8 +31,7 @@ public TrapShot() { // this.flywheel.revSpeakerFromRPM( // () -> SmartDashboard.getNumber("trap flywheel rpm", 0)), - this.flywheel.revTrap() - ), + this.flywheel.revTrap()), this.indexer.outtake(), this.indexer.off(), this.flywheel.off()); diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index bb26276..4d1b3fa 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -24,8 +24,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; 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.SubsystemBase; import java.util.Optional; import java.util.function.BooleanSupplier; @@ -118,7 +116,7 @@ private Drivetrain() { this.configureField(); } - public void setYaw(double angle){ + public void setYaw(double angle) { this.gyro.setAngleAdjustment(-angle - this.gyro.getYaw()); } @@ -154,14 +152,12 @@ private void configurePathPlanner() { // 0,0,0 DrivetrainConstants.kTranslationP, DrivetrainConstants.kTranslationI, - DrivetrainConstants.kTranslationD - ), + DrivetrainConstants.kTranslationD), new PIDConstants( // 0,0,0 DrivetrainConstants.kRotationP, DrivetrainConstants.kRotationI, - DrivetrainConstants.kRotationD - ), + DrivetrainConstants.kRotationD), DrivetrainConstants.kMaxSpeedMetersPerSecond, 0.5 * Math.hypot( @@ -192,8 +188,7 @@ public Pose2d getPose() { } public void resetPose(Pose2d pose) { - this.odometry.resetPosition( - this.gyro.getRotation2d(), this.getModulePositions(), pose); + this.odometry.resetPosition(this.gyro.getRotation2d(), this.getModulePositions(), pose); } private SwerveModuleState[] getModuleStates() { @@ -250,10 +245,7 @@ private void driveFromInput( ChassisSpeeds speeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - correctedStrafe, - correctedThrottle, - correctedOmega, - this.gyro.getRotation2d()) + correctedStrafe, correctedThrottle, correctedOmega, this.gyro.getRotation2d()) : new ChassisSpeeds(correctedStrafe, correctedThrottle, correctedOmega); this.driveFromSpeeds(speeds); @@ -344,9 +336,12 @@ public TrapPose getClosestTrapPosition() { }; Pose2d[] redTrapPoses = { - GeometryUtil.flipFieldPose(new Pose2d(new Translation2d(3.835, 2.29), Rotation2d.fromDegrees(-120))), - GeometryUtil.flipFieldPose(new Pose2d(new Translation2d(3.835, 5.91), Rotation2d.fromDegrees(120))), - GeometryUtil.flipFieldPose(new Pose2d(new Translation2d(6.97, 4.1), Rotation2d.fromDegrees(0))) + GeometryUtil.flipFieldPose( + new Pose2d(new Translation2d(3.835, 2.29), Rotation2d.fromDegrees(-120))), + GeometryUtil.flipFieldPose( + new Pose2d(new Translation2d(3.835, 5.91), Rotation2d.fromDegrees(120))), + GeometryUtil.flipFieldPose( + new Pose2d(new Translation2d(6.97, 4.1), Rotation2d.fromDegrees(0))) }; Pose2d[] trapPosesForTeam = MyAlliance.isRed() ? redTrapPoses : blueTrapPoses; @@ -641,7 +636,8 @@ public Command pathfindToTrap() { } // TODO: distance filter, same as trap - public Command alignToAmp(){ - return AutoBuilder.pathfindThenFollowPath(PathPlannerPath.fromPathFile("AmpAlign"), DrivetrainConstants.kAutoConstraints); + public Command alignToAmp() { + return AutoBuilder.pathfindThenFollowPath( + PathPlannerPath.fromPathFile("AmpAlign"), DrivetrainConstants.kAutoConstraints); } } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index 92b78c6..bf7f80d 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -314,11 +314,12 @@ protected void tune() { @Override public void periodic() { SmartDashboard.putNumber(this.id + " ref angle", this.commandedState.angle.getDegrees()); - SmartDashboard.putNumber(this.id + " strator current draw", driveMotor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber(this.id + " supply current draw", driveMotor.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber(this.id + " torque current draw", driveMotor.getTorqueCurrent().getValueAsDouble()); - - + SmartDashboard.putNumber( + this.id + " strator current draw", driveMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber( + this.id + " supply current draw", driveMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber( + this.id + " torque current draw", driveMotor.getTorqueCurrent().getValueAsDouble()); // this.driveController.setReference( // this.commandedState.speedMetersPerSecond, ControlType.kVelocity); diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 140a204..cf09ce0 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -234,21 +234,22 @@ private void tune() { public Command moveToAngle(DoubleSupplier angleDegSupplier) { return new SequentialCommandGroup( - new InstantCommand( - () -> - this.setGoal( - MathUtil.clamp( - angleDegSupplier.getAsDouble(), - RetractorConstants.kMinAngle, - RetractorConstants.kMaxAngle))), - this.run( + new InstantCommand( () -> this.setGoal( MathUtil.clamp( angleDegSupplier.getAsDouble(), RetractorConstants.kMinAngle, - RetractorConstants.kMaxAngle))) - .until(this::atGoal)).withTimeout(4.0); + RetractorConstants.kMaxAngle))), + this.run( + () -> + this.setGoal( + MathUtil.clamp( + angleDegSupplier.getAsDouble(), + RetractorConstants.kMinAngle, + RetractorConstants.kMaxAngle))) + .until(this::atGoal)) + .withTimeout(4.0); } public Command moveToAngle(double angleDeg) { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index f16813d..3978a74 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -1,8 +1,6 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.launcher; -import org.robolancers321.Constants.AimConstants; - import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class AimTable { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index 0eb6272..383d046 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import java.util.function.DoubleSupplier; import org.robolancers321.Constants.FlywheelConstants; -import org.robolancers321.Constants.FlywheelConstants.FlywheelSetpoint; public class Flywheel extends SubsystemBase { private static Flywheel instance = null; @@ -178,10 +177,10 @@ public Command revSpeaker() { public Command revTrap() { return runOnce( - () -> { - this.goalRPM = FlywheelConstants.FlywheelSetpoint.kTrap.rpm; - } - ).alongWith(new WaitUntilCommand(this::isRevved)); + () -> { + this.goalRPM = FlywheelConstants.FlywheelSetpoint.kTrap.rpm; + }) + .alongWith(new WaitUntilCommand(this::isRevved)); } public Command revFeeder() { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index d6c1ade..751374a 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -207,21 +207,22 @@ private void tune() { public Command moveToAngle(DoubleSupplier angleDegSupplier) { return new SequentialCommandGroup( - new InstantCommand( - () -> - this.setGoal( - MathUtil.clamp( - angleDegSupplier.getAsDouble(), - PivotConstants.kMinAngle, - PivotConstants.kMaxAngle))), - this.run( + new InstantCommand( () -> this.setGoal( MathUtil.clamp( angleDegSupplier.getAsDouble(), PivotConstants.kMinAngle, - PivotConstants.kMaxAngle))) - .until(this::atGoal)).withTimeout(4.0); + PivotConstants.kMaxAngle))), + this.run( + () -> + this.setGoal( + MathUtil.clamp( + angleDegSupplier.getAsDouble(), + PivotConstants.kMinAngle, + PivotConstants.kMaxAngle))) + .until(this::atGoal)) + .withTimeout(4.0); } public Command moveToAngle(double angleDeg) {