Merge pull request #56 from Ocebots/vision-commmenting

tuning-intake

Author: Yaypixel

src/main/java/frc/robot/RobotContainer.java

@@ -40,7 +40,7 @@ public class RobotContainer {
   // used to determine which section of the reef to score on, defaults at start to pole to front and
   // left of driver
   private int reefSection = 7;
-
+  public static boolean stopGrab = false;
   // slows down robot during pickup
   private Command pickup =
       coral
@@ -206,19 +206,19 @@ private void configureBindings() {
     // when b is pressed and if auto is disabled, raises elevator for l1 scoring, when left stick is
     // pressed, completes scoring motion
     // is auto is enabled, automatically goes to previously selected reef section and l1 scores
-    controller
-        .b()
-        .whileTrue(
-            Commands.deferredProxy(
-                () -> {
-                  if (autoDisabled) {
-                    return Commands.startEnd(
-                            () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0)
-                        .withDeadline(coral.l1Score(controller.leftStick()));
-                  } else {
-                    return coral.goToReef(drivetrain, () -> reefSection, () -> 0);
-                  }
-                }));
+    /* controller
+    .b()
+    .whileTrue(
+        Commands.deferredProxy(
+            () -> {
+              if (autoDisabled) {
+                return Commands.startEnd(
+                        () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0)
+                    .withDeadline(coral.l1Score(controller.leftStick()));
+              } else {
+                return coral.goToReef(drivetrain, () -> reefSection, () -> 0);
+              }
+            }));*/
 
     // clearing algae off reef, down on plus for l1 clear, right on plus for l2 clear
     controller.povDown().onTrue(coral.l1ReefClear(controller.leftStick()));
@@ -248,6 +248,8 @@ private void configureBindings() {
     // when left plus pressed, disable or enable auto, defaults at disabled
     controller.povLeft().onTrue(Commands.runOnce(() -> autoDisabled = !autoDisabled));
     controller.back().onTrue(Commands.runOnce(() -> gyro.zeroYaw()));
+    controller.leftStick().onTrue(Commands.runOnce(() -> stopGrab = true));
+    controller.leftStick().onFalse(Commands.runOnce(() -> stopGrab = false));
   }
 
   // deadbands for driving

src/main/java/frc/robot/config/CoralConfig.java

@@ -12,12 +12,12 @@ public class CoralConfig {
   public static final double VELOCITY_TOLERANCE = 1.0;
 
   public static final double INTAKE_HEIGHT = 0.065;
-  public static final Rotation2d INTAKE_ANGLE_UPPER = Rotation2d.fromRadians(5.9);
-  public static final Rotation2d INTAKE_ANGLE_LOWER = Rotation2d.fromRadians(5.7);
+  public static final Rotation2d INTAKE_ANGLE_UPPER = Rotation2d.fromRadians(1.65);
+  public static final Rotation2d INTAKE_ANGLE_LOWER = Rotation2d.fromRadians(1.7);
 
   // all heights are in meters
-  public static final double INTAKE_HEIGHT_SOURCE = 0.490;
-  public static final Rotation2d INTAKE_ANGLE_SOURCE = Rotation2d.fromRadians(.497);
+  public static final double INTAKE_HEIGHT_SOURCE = 0.55;
+  public static final Rotation2d INTAKE_ANGLE_SOURCE = Rotation2d.fromRadians(.75);
 
   public static final double MOVEMENT_DISTANCE = 1.75;
   public static final double L4_OFFSET = Units.inchesToMeters(-18);

src/main/java/frc/robot/subsystems/Coral.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotContainer;
 import frc.robot.config.CoralConfig;
 import frc.robot.config.Positions;
 import java.util.Set;
@@ -33,10 +34,10 @@ public class Coral extends SubsystemBase {
     Rotation2d.fromRadians(0.3)
   };
   private Rotation2d[] reefClearRotationsPrimary = {
-    Rotation2d.fromDegrees(-35), Rotation2d.fromDegrees(-35)
+    Rotation2d.fromRadians(1.5), Rotation2d.fromRadians(1.5)
   };
   private Rotation2d[] reefClearRotationsSecondary = {
-    Rotation2d.fromDegrees(30), Rotation2d.fromDegrees(30)
+    Rotation2d.fromRadians(0.95), Rotation2d.fromRadians(0.95)
   };
   private double[] reefClearHeights = {0.65, 1.0564};
 
@@ -48,7 +49,9 @@ public Coral() {
     grabber.setDefaultCommand(
         Commands.run(
             () -> {
-              if (!elevator.isAtPosition() || !coralPivot.isPivotReady()) {
+              if (!elevator.isAtPosition()
+                  || !coralPivot.isPivotReady()
+                  || !RobotContainer.stopGrab) {
                 grabber.run();
               } else {
                 grabber.stop();
@@ -62,7 +65,6 @@ private Command score(int idx, BooleanSupplier completeScore) {
     return Commands.waitSeconds(0.3)
         .andThen(coralPivot.setPivotAngle(() -> coralScoringRotations[idx]))
         .alongWith(elevator.setElevatorHeight(() -> elevatorScoringHeights[idx]))
-        .alongWith(Commands.run(grabber::run, grabber))
         .withDeadline(
             Commands.waitUntil(elevator::isAtPosition)
                 .andThen(Commands.waitUntil(coralPivot::isPivotReady), Commands.waitSeconds(0.8))