Merge pull request #60 from zaneal/main

Stuff
fairviewrobotics · Mar 20, 2024 · 3133cb7 · 3133cb7
2 parents 6cf29bd + 71d8ce7
commit 3133cb7
Show file tree

Hide file tree

Showing 6 changed files with 54 additions and 46 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import frc.robot.constants.DrivetrainConstants;
+import frc.robot.constants.ShooterConstants;
 import frc.robot.subsystems.*;
 import frc.robot.commands.*;
 import frc.robot.commands.IntakeCommand;
@@ -207,14 +208,20 @@ public void configureButtonBindings() {
             new RunCommand(() -> indexerSubsystem.rotateAllWheelsPercent(0.0))
     );
     new JoystickButton(secondaryController, XboxController.Button.kY.value).whileTrue(
-            new RunCommand(() ->  indexerSubsystem.rotateAllWheelsPercent(0.15))
-    ).whileFalse(
-            new RunCommand(() -> indexerSubsystem.rotateAllWheelsPercent(0.0))
+           new ParallelCommandGroup(
+                   new RotateToCorner(swerveSubsystem,
+                           () -> -primaryController.getLeftY() * DrivetrainConstants.drivingSpeedScalar,
+                           () -> -primaryController.getLeftX() * DrivetrainConstants.drivingSpeedScalar,
+                           ShooterConstants.cornerPoseBlue,
+                           ShooterConstants.cornerPoseRed
+                   ),
+                   new SpinUpCommand(shooterSubsystem, ledSubsystem)
+           )
     );
     new JoystickButton(secondaryController, XboxController.Button.kX.value).whileTrue(
-            new RunCommand(() ->  indexerSubsystem.rotateAllWheelsPercent(-0.15))
+            new RunCommand(() -> shooterSubsystem.setSpeed(1000))
     ).whileFalse(
-            new RunCommand(() -> indexerSubsystem.rotateAllWheelsPercent(0.0))
+            new RunCommand(() -> shooterSubsystem.setSpeed(0))
     );
     new JoystickButton(secondaryController, XboxController.Button.kB.value).whileTrue(
             new ParallelCommandGroup(
@@ -227,9 +234,14 @@ public void configureButtonBindings() {
     );
 
     new POVButton(secondaryController, 0).whileTrue(
-            new RunCommand(() -> shooterSubsystem.setSpeed(1000))
+            new RunCommand(() ->  indexerSubsystem.rotateAllWheelsPercent(0.15))
     ).whileFalse(
-            new RunCommand(() -> shooterSubsystem.setSpeed(0))
+            new RunCommand(() -> indexerSubsystem.rotateAllWheelsPercent(0.0))
+    );
+    new POVButton(secondaryController, 180).whileTrue(
+            new RunCommand(() ->  indexerSubsystem.rotateAllWheelsPercent(-0.15))
+    ).whileFalse(
+            new RunCommand(() -> indexerSubsystem.rotateAllWheelsPercent(0.0))
     );
 
 //

diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -77,7 +77,7 @@ public void execute() {
                     intakeSubsystem.setBottomSpeed(0.4);
                     indexerSubsystem.rotateAllWheelsPercent(0.3);
                 } else if (indexerSubsystem.isCenter()) {
-                   // intakeSubsystem.setSpeed(0);
+
                     try {
                         Thread.sleep(120);
                     } catch (InterruptedException e) {
@@ -95,6 +95,26 @@ public void execute() {
                         }
                     }
 
+                }
+
+            }
+            case ZANE -> {
+                if (!indexerSubsystem.isUp()) {
+                    intakeSubsystem.setTopSpeed(0.4);
+                    intakeSubsystem.setBottomSpeed(0.4);
+                    indexerSubsystem.rotateAllWheelsPercent(0.3);
+                } else if (indexerSubsystem.isUp()) {
+                        indexerSubsystem.rotateAllWheelsPercent(0);
+                        intakeSubsystem.setSpeed(0.0);
+                        double timePassed = Timer.getFPGATimestamp() - this.rumbleTime;
+                        System.out.println("TP: " + timePassed + " CT: " + Timer.getFPGATimestamp() + " RT: " + this.rumbleTime);
+                        if (timePassed >= 0.1) {
+                            System.out.println("Rumbling");
+                            primaryController.setRumble(GenericHID.RumbleType.kBothRumble, 1.0);
+                            this.rumbleTime = Timer.getFPGATimestamp();
+                        }
+
+
                 }
             }
         }
@@ -113,6 +133,7 @@ public void end(boolean interrupted) {
 
     public enum Targets {
         AMP,
-        SPEAKER
+        SPEAKER,
+        ZANE
     }
 }
diff --git a/src/main/java/frc/robot/commands/RotateToCorner.java b/src/main/java/frc/robot/commands/RotateToCorner.java
@@ -30,7 +30,7 @@ public class RotateToCorner extends Command {
     );
 
     /**
-     * Rotates the robot to face the speaker, while still allowing the driver to control forward and backward movement
+     * Rotates the robot to face the corner near the amp, while still allowing the driver to control forward and backward movement
      * @param swerveSubsystem The instance of {@link SwerveSubsystem}
      * @param forward The desired forward percentage of the robot
      * @param sideways The desired sideways percentage of the robot

diff --git a/src/main/java/frc/robot/commands/SetLEDCommand.java b/src/main/java/frc/robot/commands/SetLEDCommand.java
@@ -17,41 +17,7 @@ public SetLEDCommand(LEDSubsystem subsystem, LEDConstants.Status status) {
 
     @Override
     public void execute() {
-        switch(status) {
-//            case TEST_1:
-//                //Confetti
-//                subsystem.setLED(-0.87);
-//            case TEST_2:
-//                //Sinelon, Forest Palette
-//                subsystem.setLED(-0.71);
-            case IDLE:
-                //Solid, Gold
-                subsystem.setLED(0.69);
-            case INTAKING:
-                //Solid, Aqua
-                subsystem.setLED(0.81);
-            case MOVING:
-                //Solid, White
-                subsystem.setLED(0.93);
-            case DISABLED:
-                //Solid, Red
-                subsystem.setLED(0.61);
-            case SHOOTING:
-                //Solid, Lime
-                subsystem.setLED(0.73);
-            case VISION_MOVING:
-                //Solid, Blue
-                subsystem.setLED(0.87);
-            case SPINUP:
-                //Solid, Violet
-                subsystem.setLED(0.91);
-            case CLIMBER_EXTENDING:
-                //Shot, Blue
-                subsystem.setLED(-0.83);
-            case CLIMBER_RETRACTING:
-                //Shot, Red
-                subsystem.setLED(-0.85);
-        }
     }
 
+
 }
diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java
@@ -11,7 +11,8 @@ public class IndexerConstants {
     public final static int indexerRotateID = 11;
 
     public final static int centerLimebreakID = 1;
-//    public final static int topLimebreakID = 1;
+    public final static int upLimeBreakID = 0;
+
 
     public final static double indexerP = 8.0;
     public final static double indexerI = 0.0;

diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java
@@ -28,11 +28,14 @@ public class IndexerSubsystem extends SubsystemBase {
     private final AbsoluteEncoder indexerEncoder = indexerRotate.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
 
     private final DigitalInput centerLinebreak = new DigitalInput(IndexerConstants.centerLimebreakID);
+    private final DigitalInput upLinebreak = new DigitalInput(IndexerConstants.centerLimebreakID);
 
 
 
     private final BooleanEntry centerLinebreaknt = NetworkTableInstance.getDefault()
             .getTable("Indexer").getBooleanTopic("Center Linebreak").getEntry(!centerLinebreak.get());
+    private final BooleanEntry upLinebreaknt = NetworkTableInstance.getDefault()
+            .getTable("Indexer").getBooleanTopic("Up Linebreak").getEntry(!upLinebreak.get());
 
     private Rotation2d indexerPosFirst = new Rotation2d(indexerEncoder.getPosition());
     private double indexerPosRadians = indexerPosFirst.minus(new Rotation2d(IndexerConstants.posOffset)).getRadians();
@@ -141,6 +144,10 @@ public boolean isCenter() {
         return !centerLinebreak.get();
     }
 
+    public boolean isUp() {
+        return !upLinebreak.get();
+    }
+
     /**
      * Check if top limebreak is seeing something
      * @return If the limebreak is seeing something
@@ -184,5 +191,6 @@ public void periodic() {
 //        nt.setEntry("Center Linebreak", isCenter());
 
         centerLinebreaknt.set(!centerLinebreak.get());
+        upLinebreaknt.set(!upLinebreak.get());
     }
 }