chore: cleanup

flamingchickens1540 · Sep 30, 2024 · 3b43662 · 3b43662
1 parent 5e276e2
commit 3b43662
Show file tree

Hide file tree

Showing 17 changed files with 21 additions and 113 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
@@ -4,7 +4,6 @@
 import com.pathplanner.lib.commands.FollowPathCommand;
 import com.pathplanner.lib.commands.PathfindingCommand;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -20,7 +19,6 @@
 import org.team1540.robot2024.util.MechanismVisualiser;
 import org.team1540.robot2024.util.auto.AutoManager;
 import org.team1540.robot2024.util.vision.AprilTagsCrescendo;
-import org.team1540.robot2024.util.vision.LimelightHelpers;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -1,9 +1,7 @@
 package org.team1540.robot2024;
 
-import com.ctre.phoenix6.Utils;
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.*;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -16,7 +14,6 @@
 import org.team1540.robot2024.commands.climb.ClimbAlignment;
 import org.team1540.robot2024.commands.drivetrain.*;
 import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
-import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.commands.indexer.ContinuousIntakeCommand;
 import org.team1540.robot2024.commands.indexer.IntakeAndFeed;
 import org.team1540.robot2024.commands.indexer.StageTrampCommand;
@@ -123,16 +120,6 @@ public RobotContainer() {
         configureAutoRoutines();
         // Configure the button bindings
         configureButtonBindings();
-        configureLedBindings();
-    }
-
-    private void configureLedBindings() {
-//        Runnable onDisconnect = () -> leds.setPatternAll(LedPatternFlame::new, Leds.PatternCriticality.HIGH);
-//        onDisconnect.run();
-//        new Trigger(DriverStation::isDSAttached)
-//                .onTrue(Commands.runOnce(() -> leds.clearPatternAll(Leds.PatternCriticality.HIGH))
-//                            .ignoringDisable(true))
-//                .onFalse(Commands.runOnce(onDisconnect).ignoringDisable(true));
     }
 
     private void configureButtonBindings() {
@@ -146,9 +133,6 @@ private void configureButtonBindings() {
             enableBrakeMode(false);
         }).ignoringDisable(true));
 
-        //TODO remove
-//        driver.back().and(driver.start()).onTrue(Commands.runOnce(() -> drivetrain.setPose(new Pose2d())));
-
         Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter)
                 .alongWith(leds.commandShowPattern(
                         new LedPatternProgressBar(shooter::getSpinUpPercent, "#00a9ff", 33),
@@ -247,7 +231,7 @@ private void configureButtonBindings() {
                 )
         );
 
-        copilot.back().onTrue(Commands.runOnce(shooter::zeroPivotToCancoder).andThen(Commands.print("BACK IS PRESSED")));
+        copilot.back().onTrue(Commands.runOnce(shooter::zeroPivotToCancoder));
 
         copilot.leftBumper().whileTrue(new AmpScoreSequence(tramp, indexer, elevator));
         Command intakeCommand = new ContinuousIntakeCommand(indexer, 1)

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/testAuto.java b/src/main/java/org/team1540/robot2024/commands/autos/testAuto.java
@@ -45,8 +45,7 @@ public testAuto(Drivetrain drivetrain, Shooter shooter, Indexer indexer) {
                         ),
                         new Triplet<>(0, ()->true, Commands::none),
                         new Triplet<>(1, ()->false, Commands::none)
-                ),
-                Commands.print("HALLELUJAH")
+                )
 
                 //,
 //

diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java
@@ -8,7 +8,6 @@
 import org.team1540.robot2024.subsystems.drive.Drivetrain;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.indexer.Indexer;
-import org.team1540.robot2024.subsystems.shooter.Shooter;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 
 public class ClimbSequence extends ParallelRaceGroup {
@@ -21,7 +20,7 @@ public ClimbSequence(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Inde
                         Commands.waitUntil(controller.a()),
                         new ElevatorSetpointCommand(elevator, ElevatorState.TOP),
                         Commands.waitSeconds(5), // Confirm that nothing will break. Also might need to be tuned if chain does weird things
-                        Commands.startEnd(()->elevator.setVoltage(-10), elevator::holdPosition, elevator).until(elevator::getLowerLimit)
+                        Commands.startEnd(()->elevator.setPercent(-0.8), elevator::holdPosition, elevator).until(elevator::getLowerLimit)
                 )
         );
     }

diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithCorrectionCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithCorrectionCommand.java
@@ -71,7 +71,6 @@ public void execute() {
                 drivetrain.getPose().getTranslation(),
                 linearDirection
         ));
-//        System.out.println("I am driving woth correxction 1");
 
         double rotPercent = target == null
                 ? JoystickUtils.smartDeadzone(-controller.getRightX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1)

diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithCorrectionCommand2.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithCorrectionCommand2.java
@@ -25,7 +25,6 @@ public class DriveWithCorrectionCommand2 extends Command {
 
     private boolean isFlipped;
 
-
     private final LoggedTunableNumber kP = new LoggedTunableNumber("Targeting/COR_KP", 0.9);
     private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/COR_KI", 0);
     private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/COR_KD", 0);
@@ -84,19 +83,16 @@ public void execute() {
         Rotation2d linearDirection = new Rotation2d(xPercent, yPercent);
 
 //        if(drivetrain.getRotation().rotateBy(Rotation2d.fromDegrees(angleDegrees.get())).getCos() * xPercent > 0){
-            linearDirection = linearDirection.minus(Rotation2d.fromDegrees(correctionController.calculate(angleDegrees.get(), 0)));
+        linearDirection = linearDirection.minus(Rotation2d.fromDegrees(correctionController.calculate(angleDegrees.get(), 0)));
 //        }
 
-        System.out.println("I am correctiong 1");
         Logger.recordOutput("Targeting/targetDirection", new Pose2d(
                 drivetrain.getPose().getTranslation(),
                 linearDirection.rotateBy(Rotation2d.fromDegrees(180))
         ));
         Logger.recordOutput("Targeting/angle", angleDegrees.get());
         Logger.recordOutput("Targeting/target", Constants.Targeting.getSpeakerPose());
 
-        System.out.println();
-
         double rotPercent = target == null
                 ? JoystickUtils.smartDeadzone(-controller.getRightX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1)
                 : rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians());;

diff --git a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import org.team1540.robot2024.Constants;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 
 public class ElevatorManualCommand extends Command {
@@ -25,7 +24,7 @@ public void execute() {
             } else {
                 val -= DEADZONE;
             }
-            elevator.setVoltage(-val * 12 * 0.18);
+            elevator.setPercent(-val * 2.16);
         } else {
             elevator.holdPosition();
         }

diff --git a/.../java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithLeadTargeting.java b/.../java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithLeadTargeting.java
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
@@ -64,13 +64,6 @@ public class Drivetrain extends SubsystemBase {
 
     private Pose2d targetPose = new Pose2d();
 
-    private final LoggedTunableNumber mkP = new LoggedTunableNumber("Drivetrain/M_KP", 5.0);
-    private final LoggedTunableNumber mkI = new LoggedTunableNumber("Drivetrain/M_KI", 0.0);
-    private final LoggedTunableNumber mkD = new LoggedTunableNumber("Drivetrain/M_KD", 0.0);
-    private final LoggedTunableNumber rkP = new LoggedTunableNumber("Drivetrain/R_KP", 7.0);
-    private final LoggedTunableNumber rkI = new LoggedTunableNumber("Drivetrain/R_KI", 0.0);
-    private final LoggedTunableNumber rkD = new LoggedTunableNumber("Drivetrain/R_KD", 0.0);
-
     private Drivetrain(
             GyroIO gyroIO,
             ModuleIO flModuleIO,
@@ -199,21 +192,6 @@ public void periodic() {
         // Update odometry
         poseEstimator.update(rawGyroRotation, getModulePositions());
         visionPoseEstimator.update(rawGyroRotation, getModulePositions());
-
-
-
-        //FIXME: Could be a bad idea
-//        if(mkP.hasChanged(hashCode()) || mkI.hasChanged(hashCode()) || mkD.hasChanged(hashCode())
-//        || rkP.hasChanged(hashCode()) || rkI.hasChanged(hashCode()) || rkD.hasChanged(hashCode())){
-//            AutoBuilder.configureHolonomic(
-//                    this::getPose,
-//                    this::setPose,
-//                    () -> kinematics.toChassisSpeeds(getModuleStates()),
-//                    this::runVelocity,
-//                    new HolonomicPathFollowerConfig(new PIDConstants(mkP.get(), mkI.get(), mkD.get()),new PIDConstants(rkP.get(), rkI.get(), rkD.get()),MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
-//                    () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red,
-//                    this);
-//        }
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java
@@ -88,12 +88,12 @@ public boolean isAtSetpoint() {
         return MathUtil.isNear(setpointMeters, positionFilter.getAverage(), POS_ERR_TOLERANCE_METERS) || (inputs.atLowerLimit && setpointMeters <= 0);
     }
 
-    public void setVoltage(double voltage) {
-        io.setVoltage(voltage);
+    public void setPercent(double percent) {
+        io.setDutyCycle(percent);
     }
 
     public void stop() {
-        io.setVoltage(0.0);
+        io.setDutyCycle(0.0);
     }
 
     @AutoLogOutput(key = "Elevator/setpoint")

diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java
@@ -19,7 +19,7 @@ default void updateInputs(ElevatorIOInputs inputs) {}
 
     default void setSetpointMeters(double position) {}
 
-    default void setVoltage(double voltage) {}
+    default void setDutyCycle(double dutyCycle) {}
 
     default void setBrakeMode(boolean isBrakeMode) {}
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java
@@ -55,9 +55,9 @@ public void updateInputs(ElevatorIOInputs inputs) {
     }
 
     @Override
-    public void setVoltage(double volts) {
+    public void setDutyCycle(double dutyCycle) {
         isClosedLoop = false;
-        elevatorAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+        elevatorAppliedVolts = MathUtil.clamp(dutyCycle * 12, -12.0, 12.0);
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java
@@ -119,8 +119,8 @@ public void setSetpointMeters(double positionMeters) {
     }
 
     @Override
-    public void setVoltage(double voltage) {
-        leader.set(voltage);
+    public void setDutyCycle(double dutyCycle) {
+        leader.set(dutyCycle);
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/util/auto/PathHelper.java b/src/main/java/org/team1540/robot2024/util/auto/PathHelper.java
@@ -63,7 +63,6 @@ public boolean getIsResetting() {
 
     public Pose2d getInitialPose() {
         BooleanSupplier shouldFlip = () -> DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red;
-//        System.out.println(DriverStation.getAlliance().orElse(null) + " " + shouldFlip.getAsBoolean());
         return shouldFlip.getAsBoolean() ? GeometryUtil.flipFieldPose(initialPose) : initialPose;
     }
 
@@ -111,7 +110,6 @@ public Command withInterrupt(Supplier<Command> cmd, Supplier<Command> afterInter
                             )
                     )
             ).andThen(
-//                    Commands.runOnce(()->System.out.println(cancel[0].getAsBoolean())),
                     Commands.sequence(
 //                            Commands.runOnce(()->System.out.println(cancel[0].getAsBoolean())),
                             ((Supplier<Command>) term.getThird()).get(),

diff --git a/src/main/java/org/team1540/robot2024/util/vision/AprilTagsCrescendo.java b/src/main/java/org/team1540/robot2024/util/vision/AprilTagsCrescendo.java
@@ -20,7 +20,7 @@ public enum Tags {
 
         final int blue;
         final int red;
-        private Tags(int red, int blue){
+        Tags(int red, int blue){
             this.red = red;
             this.blue = blue;
         }
@@ -33,21 +33,22 @@ public static AprilTagsCrescendo getInstance() {
         return instance;
     }
 
-    private AprilTagsCrescendo(){
+    private AprilTagsCrescendo() {
         tags = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
     }
 
     public AprilTagFieldLayout getTags() {
         return tags;
     }
 
-    public Pose3d getTag(int tagID){
+    public Pose3d getTag(int tagID) {
         return tags.getTagPose(tagID).get();
     }
-    public Pose3d getTag(Tags tag){
+    public Pose3d getTag(Tags tag) {
         return getTag(tag, DriverStation.getAlliance().orElse(null));
     }
-    public Pose3d getTag(Tags tag, DriverStation.Alliance alliance){
+
+    public Pose3d getTag(Tags tag, DriverStation.Alliance alliance) {
         return getTag(alliance == DriverStation.Alliance.Red ? tag.red : tag.blue);
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/util/vision/FlipUtil.java b/src/main/java/org/team1540/robot2024/util/vision/FlipUtil.java
@@ -13,14 +13,13 @@ public static Pose2d flipIfRed(Pose2d pose){
                 : pose;
     }
 
-    public static Rotation2d flipIfRed(Rotation2d rotation){
+    public static Rotation2d flipIfRed(Rotation2d rotation) {
         return DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red
                 ? GeometryUtil.flipFieldRotation(rotation)
                 : rotation;
-
     }
 
-    public static double flipIfRed(double num){
+    public static double flipIfRed(double num) {
         return DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red
                 ? num *= -1
                 : num;

diff --git a/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java b/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java
@@ -4,7 +4,6 @@
 
 import java.util.function.Supplier;
 
-import static org.team1540.robot2024.Constants.Vision.*;
 import static org.team1540.robot2024.Constants.Vision.AprilTag.*;
 
 public class VisionPoseAcceptor {