the manual hood works :D

FRCTeam3255 · May 11, 2024 · 5e83b22 · 5e83b22
1 parent 9887999
commit 5e83b22
Show file tree

Hide file tree

Showing 4 changed files with 76 additions and 60 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -123,7 +123,7 @@ private void configureButtonBindings() {
     conOperator.btn_RTrig
         .whileTrue(comShootCargo);
 
-    conOperator.btn_RBump.onTrue(new RunCommand(() -> subShooter.setMotorSpeed(0.5), subShooter));
+    conOperator.btn_RBump.onTrue(new RunCommand(() -> subShooter.setMotorSpeed(1), subShooter));
     conOperator.btn_Start.onTrue(new InstantCommand(() -> subShooter.neutralOutput(), subShooter));
 
     // Turret
@@ -133,6 +133,14 @@ private void configureButtonBindings() {
     conOperator.btn_RStick
         .onTrue(Commands.runOnce(() -> subTurret.setAngle(prefTurret.turretFacingAwayFromIntakeDegrees)));
 
+    // Hood
+    conOperator.btn_A.whileTrue(Commands.runOnce(() -> subHood.setSpeed(0.15)))
+        .onFalse(Commands.runOnce(() -> subHood.setSpeed(0)))
+        .onFalse(Commands.runOnce(() -> subHood.setAngle(subHood.getAngleDegrees())));
+    conOperator.btn_Y.whileTrue(Commands.runOnce(() -> subHood.setSpeed(-0.15)))
+        .onFalse(Commands.runOnce(() -> subHood.setSpeed(0)))
+        .onFalse(Commands.runOnce(() -> subHood.setAngle(subHood.getAngleDegrees())));
+
     // Intake
     conOperator.btn_LTrig.whileTrue(comCollectCargo);
     conOperator.btn_B.whileTrue(comDiscardCargo);

diff --git a/src/main/java/frc/robot/RobotPreferences.java b/src/main/java/frc/robot/RobotPreferences.java
@@ -73,7 +73,7 @@ public static final class prefDrivetrain {
   }
 
   public static final class prefHood {
-    public static final SN_DoublePreference hoodP = new SN_DoublePreference("hoodP", 10);
+    public static final SN_DoublePreference hoodP = new SN_DoublePreference("hoodP", 0.05);
     public static final SN_DoublePreference hoodI = new SN_DoublePreference("hoodI", 0);
     public static final SN_DoublePreference hoodD = new SN_DoublePreference("hoodD", 0);
 

diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java
@@ -81,6 +81,10 @@ public void setAngle(SN_DoublePreference degrees) {
     setAngle(degrees.getValue());
   }
 
+  public void setSpeed(double speed) {
+    hoodMotor.set(speed);
+  }
+
   public boolean isBottomSwitch() {
     return !bottomSwitch.get();
   }
@@ -107,10 +111,11 @@ public void periodic() {
     if (displayOnDashboard) {
 
       SmartDashboard.putNumber("Hood Angle Degrees", getAngleDegrees());
-      SmartDashboard.putNumber("Hood Desired Angle Degrees", desiredAngle);
 
-      SmartDashboard.putNumber("Hood Raw Angle", getRawAngle());
-      SmartDashboard.putBoolean("Hood Is Bottom Switch", isBottomSwitch());
+      // SmartDashboard.putNumber("Hood Desired Angle Degrees", desiredAngle);
+
+      // SmartDashboard.putNumber("Hood Raw Angle", getRawAngle());
+      // SmartDashboard.putBoolean("Hood Is Bottom Switch", isBottomSwitch());
 
     }
 

diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
@@ -16,64 +16,67 @@
 
 public class Vision extends SubsystemBase {
 
-  public SN_Limelight limelight;
-
-  int buttonTimerLoops;
-
-  public Vision() {
-    limelight = new SN_Limelight();
-  }
-
-  public void setLEDOn() {
-    limelight.setLEDMode(LEDMode.on);
-  }
-
-  public void setLEDOff() {
-    limelight.setLEDMode(LEDMode.off);
-  }
-
-  /**
-   * Get distance from hub using ty inputed to the lerp
-   * 
-   * @return Distance from limelight lens to center of hub
-   */
-  public double getDistanceFromHub() {
-    return constVision.tyDistanceTable.getOutput(limelight.getOffsetY());
-  }
-
-  /**
-   * Calculate Robot Position from Vision target. Specific to the Rapid React
-   * field.
-   * <p>
-   * Radian measures require counterclockwise rotation to be positive.
-   * 
-   * @param distanceFromHub Distance from hub in meters
-   * @param robotAngle      Robot angle in radians
-   * @param turretAngle     Turret angle in radians
-   * @param offsetToTarget  Angle offset to target in radians
-   * @return Calculated position of the robot
-   */
-  public Pose2d calculatePoseFromVision(
-      double distanceFromHub,
-      double robotAngle,
-      double turretAngle,
-      double offsetToTarget) {
-
-    double goalAngle = robotAngle + turretAngle + offsetToTarget;
-
-    double calculatedRobotXPosition = constField.HUB_POSITION.getX() - (distanceFromHub * Math.cos(goalAngle));
-    double calculatedRobotYPosition = constField.HUB_POSITION.getY() + (distanceFromHub * Math.sin(goalAngle));
-
-    return new Pose2d(calculatedRobotXPosition, calculatedRobotYPosition, new Rotation2d(-robotAngle));
-  }
+  // public SN_Limelight limelight;
+
+  // int buttonTimerLoops;
+
+  // public Vision() {
+  // limelight = new SN_Limelight();
+  // }
+
+  // public void setLEDOn() {
+  // limelight.setLEDMode(LEDMode.on);
+  // }
+
+  // public void setLEDOff() {
+  // limelight.setLEDMode(LEDMode.off);
+  // }
+
+  // /**
+  // * Get distance from hub using ty inputed to the lerp
+  // *
+  // * @return Distance from limelight lens to center of hub
+  // */
+  // public double getDistanceFromHub() {
+  // return constVision.tyDistanceTable.getOutput(limelight.getOffsetY());
+  // }
+
+  // /**
+  // * Calculate Robot Position from Vision target. Specific to the Rapid React
+  // * field.
+  // * <p>
+  // * Radian measures require counterclockwise rotation to be positive.
+  // *
+  // * @param distanceFromHub Distance from hub in meters
+  // * @param robotAngle Robot angle in radians
+  // * @param turretAngle Turret angle in radians
+  // * @param offsetToTarget Angle offset to target in radians
+  // * @return Calculated position of the robot
+  // */
+  // public Pose2d calculatePoseFromVision(
+  // double distanceFromHub,
+  // double robotAngle,
+  // double turretAngle,
+  // double offsetToTarget) {
+
+  // double goalAngle = robotAngle + turretAngle + offsetToTarget;
+
+  // double calculatedRobotXPosition = constField.HUB_POSITION.getX() -
+  // (distanceFromHub * Math.cos(goalAngle));
+  // double calculatedRobotYPosition = constField.HUB_POSITION.getY() +
+  // (distanceFromHub * Math.sin(goalAngle));
+
+  // return new Pose2d(calculatedRobotXPosition, calculatedRobotYPosition, new
+  // Rotation2d(-robotAngle));
+  // }
 
   @Override
   public void periodic() {
 
-    buttonTimerLoops++;
-    if (RobotController.getUserButton() && buttonTimerLoops > 25) {
-      limelight.toggleLEDs();
-      buttonTimerLoops = 0;
-    }
+    // buttonTimerLoops++;
+    // if (RobotController.getUserButton() && buttonTimerLoops > 25) {
+    // limelight.toggleLEDs();
+    // buttonTimerLoops = 0;
+    // }
   }
 }