poopoo

MillenniumFalcons · Jul 21, 2023 · 29806ce · 29806ce
1 parent 42c1f56
commit 29806ce
Show file tree

Hide file tree

Showing 5 changed files with 30 additions and 23 deletions.
diff --git a/src/main/java/team3647/frc2023/robot/RobotContainer.java b/src/main/java/team3647/frc2023/robot/RobotContainer.java
@@ -55,16 +55,7 @@ public class RobotContainer {
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
         pdh.clearStickyFaults();
-        scheduler.registerSubsystem(
-                swerve,
-                printer,
-                pivot,
-                extender,
-                rollers,
-                wrist,
-                cubeWrist,
-                cubeShooterBottom,
-                cubeShooterTop);
+        scheduler.registerSubsystem(swerve);
 
         configureDefaultCommands();
         configureButtonBindings();
@@ -114,7 +105,7 @@ private void configureButtonBindings() {
 
         mainController.buttonA.whileTrue(superstructure.intakeForCurrentGamePiece());
 
-        mainController.rightBumper.whileTrue(superstructure.intakeAutomatic(superstructure.ground()));
+        mainController.rightBumper.whileTrue(superstructure.intakeAutomatic());
         mainController
                 .rightBumper
                 .and(() -> superstructure.getWantedStation() == StationType.Double)

diff --git a/src/main/java/team3647/frc2023/subsystems/Rollers.java b/src/main/java/team3647/frc2023/subsystems/Rollers.java
@@ -1,10 +1,9 @@
 package team3647.frc2023.subsystems;
 
-import java.util.function.DoubleSupplier;
-
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.function.DoubleSupplier;
 import team3647.lib.TalonFXSubsystem;
 
 public class Rollers extends TalonFXSubsystem {
@@ -38,7 +37,6 @@ public void intakeCube() {
         super.setOpenloop(0.5);
     }
 
-
     public void intakeGround() {
         super.setOpenloop(-0.5);
     }

diff --git a/src/main/java/team3647/frc2023/subsystems/Superstructure.java b/src/main/java/team3647/frc2023/subsystems/Superstructure.java
@@ -28,19 +28,18 @@ public class Superstructure {
     private Level wantedLevel = Level.Stay;
     private StationType wantedStation = StationType.Double;
     private boolean isAutoSteerEnabled = true;
-    private boolean isGround = true;
+    private boolean isGround = false;
 
     private SuperstructureState wantedIntakeState = SuperstructureState.doubleStationCone;
     private GamePiece intakeGamePiece = GamePiece.Cone;
     private GamePiece currentGamePiece = GamePiece.Cube;
     private double wristAdjust = 0.0;
     private double extendAdjust = 0.0;
-    private boolean isBottom = false;
+    private boolean isBottom = true;
 
     private final Translation2d kMoveIntoField = new Translation2d(0.05, 0);
 
     public void periodic(double timestamp) {
-        printer.addBoolean("ground cone", () -> isGround);
         if (getWantedStation() == StationType.Ground) {
             wantedIntakeState =
                     intakeGamePiece == GamePiece.Cone
@@ -81,15 +80,28 @@ public Command armAutomatic() {
                                 getWantedLevel(), this.currentGamePiece));
     }
 
-    public Command intakeAutomatic(boolean ground) {
+    public Command rollers(BooleanSupplier ground) {
+        // return ground.getAsBoolean()
+        //         ? intakeGround()
+        //         : intakeForGamePiece(() -> this.intakeGamePiece);
+        if (ground.getAsBoolean()) {
+            return intakeGround();
+        } else {
+            return intakeForGamePiece(() -> this.intakeGamePiece);
+        }
+    }
+
+    public Command intakeAutomatic() {
+        printer.addBoolean("ground cone", () -> isGround);
 
-        Command rollers = ground ? intakeGround() : intakeForGamePiece(() -> this.intakeGamePiece);
+        Command rollers =
+                isGround ? intakeGround() : intakeForGamePiece(() -> this.intakeGamePiece);
 
-            return Commands.deadline(
+        return Commands.deadline(
                         waitForCurrentSpikeDebounce(0.6),
                         Commands.parallel(
                                 goToStateParallel(() -> this.wantedIntakeState),
-                                rollers))
+                                rollers(() -> this.isGround)))
                 .finallyDo(
                         interrupted -> {
                             this.currentGamePiece = this.intakeGamePiece;

diff --git a/src/main/java/team3647/frc2023/subsystems/SwerveDrive.java b/src/main/java/team3647/frc2023/subsystems/SwerveDrive.java
@@ -211,7 +211,13 @@ public double getPoseY() {
     }
 
     public double getAverageSpeed() {
-        return Math.abs((frontLeft.getDriveVelocity() + frontRight.getDriveVelocity() + backLeft.getDriveVelocity() + backRight.getDriveVelocity()) / 4.0) / getMaxSpeedMpS();
+        return Math.abs(
+                        (frontLeft.getDriveVelocity()
+                                        + frontRight.getDriveVelocity()
+                                        + backLeft.getDriveVelocity()
+                                        + backRight.getDriveVelocity())
+                                / 4.0)
+                / getMaxSpeedMpS();
     }
 
     public SwerveModulePosition[] getModulePositions() {

diff --git a/src/main/java/team3647/frc2023/util/SuperstructureState.java b/src/main/java/team3647/frc2023/util/SuperstructureState.java
@@ -105,7 +105,7 @@ public SuperstructureState addWristExtend(double degs, double extend) {
     // ================================ comp bot values ==================================
     public static final SuperstructureState groundIntakeCone =
             new SuperstructureState(
-                -14.11, 1151.5, 11.89, "ground intake cone");
+                    193, ExtenderConstants.kMinimumPositionTicks, 25.5, "ground intake cone");
     public static final SuperstructureState groundIntakeConeAuto =
             new SuperstructureState(
                     187.5, ExtenderConstants.kMinimumPositionTicks, 25, "ground intake cone");