Merge branch 'testing-7-14' of https://github.com/DeepBlueRobotics/Ro…

…botCode2024 into testing-changes-7-14
DeepBlueRobotics · Jul 15, 2024 · 5e96381 · 5e96381
2 parents ac563fb + e54992c
commit 5e96381
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 18 deletions.
diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -220,6 +220,9 @@ private void setBindingsManipulator() {
             new SequentialCommandGroup(new AimArmSpeaker(arm, limelight),
                     new PassToOuttake(intakeShooter)));
 
+    // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue(
+    // new PassToOuttake(intakeShooter));
+
     new JoystickButton(manipulatorController, RAMP_OUTTAKE)
         .whileTrue(new RampMaxRPM(intakeShooter));
     new JoystickButton(manipulatorController, OPPOSITE_EJECT)

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java
@@ -23,12 +23,14 @@ public Limelight(Drivetrain drivetrain) {
 
     shooterMap = new InterpolatingDoubleTreeMap(); // add values after testing
     // key is distance (meters), value is angle (rads)
-    // ASSUMING SHOOTING AT 4000 RPM
-    shooterMap.put(2.0, .005);
+    shooterMap.put(2.06, .01);
     shooterMap.put(3.05, 0.26);
-    shooterMap.put(1.39, -.18);
     shooterMap.put(2.5, 0.23);
+    shooterMap.put(1.39, -0.18);
+    shooterMap.put(3.45, 0.3);
+    shooterMap.put(3.1, 0.3);
 
+    // ASSUMING SHOOTING AT 4000 RPM
     // changing speed multipliers for auto intaking note
     SmartDashboard.putNumber("forward speed multiplier", 1.5);
     SmartDashboard.putNumber("strafe speed multiplier", 1.5);
@@ -45,6 +47,7 @@ public Limelight(Drivetrain drivetrain) {
         positionTolerance[2]);
     SmartDashboard.putNumber("apriltag align vel tolerance",
         velocityTolerance[2]);
+
   }
 
   @Override
@@ -65,23 +68,31 @@ public void periodic() {
   }
 
   public double getTXDeg(String limelightName) {
-    return (limelightName == INTAKE_LL_NAME) ? LimelightHelpers.getTX(INTAKE_LL_NAME) : -LimelightHelpers.getTY(SHOOTER_LL_NAME);
+    return (limelightName == INTAKE_LL_NAME)
+        ? LimelightHelpers.getTX(INTAKE_LL_NAME)
+        : -LimelightHelpers.getTY(SHOOTER_LL_NAME);
   }
 
   public double getTYDeg(String limelightName) {
-    return (limelightName == INTAKE_LL_NAME) ? LimelightHelpers.getTY(INTAKE_LL_NAME) : LimelightHelpers.getTX(SHOOTER_LL_NAME);
+    return (limelightName == INTAKE_LL_NAME)
+        ? LimelightHelpers.getTY(INTAKE_LL_NAME)
+        : LimelightHelpers.getTX(SHOOTER_LL_NAME);
   }
 
   public double getDistanceToSpeakerMeters() {
-    if (LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == RED_SPEAKER_CENTER_TAG_ID
-        || LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == BLUE_SPEAKER_CENTER_TAG_ID) {
+    if (LimelightHelpers
+        .getFiducialID(SHOOTER_LL_NAME) == RED_SPEAKER_CENTER_TAG_ID
+        || LimelightHelpers
+            .getFiducialID(SHOOTER_LL_NAME) == BLUE_SPEAKER_CENTER_TAG_ID) {
       Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER)
-          .plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); //because limelight is mounted horizontally
-      double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER) / angleToGoal.getTan();
+          .plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); // because limelight is mounted
+                                                                    // horizontally
+      double distance =
+          (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER)
+              / angleToGoal.getTan();
       // SmartDashboard.putNumber("limelight distance", distance);
       return distance;
-    }
-    else {
+    } else {
       // SmartDashboard.putNumber("limelight distance", -1);
       return -1;
     }
@@ -102,16 +113,21 @@ public double getDistanceToNoteMeters() {
   }
 
   public double getArmAngleToShootSpeakerRad() {
-    double armRestingHeightToSubwooferMeters = HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS;
-    double horizontalDistanceMeters = getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH;
-    return END_EFFECTOR_BASE_ANGLE_RADS - Math.atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters);
+    double armRestingHeightToSubwooferMeters =
+        HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS;
+    double horizontalDistanceMeters =
+        getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH;
+    return END_EFFECTOR_BASE_ANGLE_RADS - Math
+        .atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters);
   }
 
 
   public double getRotateAngleRadMT2() {
-    Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target
+    Pose3d targetPoseRobotSpace =
+        LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target
 
-    double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the robot and target
+    double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the
+                                                  // robot and target
     double targetZ = -targetPoseRobotSpace.getZ(); // the sideways offset
 
     double targetOffsetRads =
@@ -121,7 +137,8 @@ public double getRotateAngleRadMT2() {
   }
 
   public double getDistanceToSpeakerMetersMT2() {
-    Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME);
+    Pose3d targetPoseRobotSpace =
+        LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME);
 
     double x = targetPoseRobotSpace.getX();
     double z = targetPoseRobotSpace.getZ();
@@ -137,4 +154,4 @@ public double getOptimizedArmAngleRadsMT2() {
   public boolean seesTag() {
     return LimelightHelpers.getTV(SHOOTER_LL_NAME);
   }
-}
+}