Back drive intake in idle whilst not fully retracted

autonomous/autonomous.py

@@ -34,8 +34,8 @@ def __init__(self) -> None:
             Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
         ]
         start_pose = Pose2d(
-            TeamPoses.RED_TEST_POSE.translation(),
-            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+            TeamPoses.RED_SUBWOOFER.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_SUBWOOFER.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
@@ -57,8 +57,8 @@ def __init__(self) -> None:
         # Start pose only needs to be on the correct half of the field,
         # so choose the subwoofer
         start_pose = Pose2d(
-            TeamPoses.RED_TEST_POSE.translation(),
-            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+            TeamPoses.RED_SUBWOOFER.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_SUBWOOFER.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
@@ -127,8 +127,8 @@ def __init__(self) -> None:
             ),
         ]
         start_pose = Pose2d(
-            TeamPoses.RED_TEST_POSE.translation(),
-            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+            TeamPoses.RED_SUBWOOFER.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_SUBWOOFER.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
@@ -161,8 +161,8 @@ def __init__(self) -> None:
             ),
         ]
         start_pose = Pose2d(
-            TeamPoses.RED_TEST_POSE.translation(),
-            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+            TeamPoses.RED_SUBWOOFER.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_SUBWOOFER.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 

components/chassis.py

@@ -311,15 +311,15 @@ def get_module_states(
         )
 
     def setup(self) -> None:
-        initial_pose = TeamPoses.RED_TEST_POSE if is_red() else TeamPoses.BLUE_TEST_POSE
+        initial_pose = TeamPoses.RED_SUBWOOFER if is_red() else TeamPoses.BLUE_SUBWOOFER
 
         self.estimator = SwerveDrive4PoseEstimator(
             self.kinematics,
             self.imu.getRotation2d(),
             self.get_module_positions(),
             initial_pose,
             stateStdDevs=(0.05, 0.05, 0.01),
-            visionMeasurementStdDevs=(0.4, 0.4, 0.03),
+            visionMeasurementStdDevs=(0.4, 0.4, 0.3),
         )
         self.field_obj = self.field.getObject("fused_pose")
         self.set_pose(initial_pose)
@@ -419,9 +419,9 @@ def update_alliance(self) -> None:
         if is_red() != self.on_red_alliance:
             self.on_red_alliance = is_red()
             if self.on_red_alliance:
-                self.set_pose(TeamPoses.RED_TEST_POSE)
+                self.set_pose(TeamPoses.RED_SUBWOOFER)
             else:
-                self.set_pose(TeamPoses.BLUE_TEST_POSE)
+                self.set_pose(TeamPoses.BLUE_SUBWOOFER)
 
     def update_odometry(self) -> None:
         self.estimator.update(self.imu.getRotation2d(), self.get_module_positions())
@@ -451,9 +451,9 @@ def reset_yaw(self) -> None:
     def reset_odometry(self) -> None:
         """Reset odometry to current team's podium"""
         if is_red():
-            self.set_pose(TeamPoses.RED_PODIUM)
+            self.set_pose(TeamPoses.RED_SUBWOOFER)
         else:
-            self.set_pose(TeamPoses.BLUE_PODIUM)
+            self.set_pose(TeamPoses.BLUE_SUBWOOFER)
 
     def get_module_positions(
         self,

components/shooter.py

@@ -30,7 +30,7 @@ class ShooterComponent:
     MAX_INCLINE_ANGLE = 1.045  # ~60 degrees
     MIN_INCLINE_ANGLE = 0.354  # ~20 degrees
     INCLINATOR_TOLERANCE = math.radians(1)
-    INCLINATOR_OFFSET = 4.023 - MIN_INCLINE_ANGLE
+    INCLINATOR_OFFSET = 4.023 - MIN_INCLINE_ANGLE - 0.035
     INCLINATOR_SCALE_FACTOR = math.tau  # rps -> radians
     INCLINATOR_GEAR_RATIO = 18 / 24 * 26 / 300
     INCLINATOR_POSITION_CONVERSION_FACTOR = (
@@ -42,23 +42,19 @@ class ShooterComponent:
     INCLINATOR_JETTISON_ANGLE = (MAX_INCLINE_ANGLE + MIN_INCLINE_ANGLE) / 2
 
     # Add extra point outside our range to ramp speed down to zero
-    FLYWHEEL_DISTANCE_LOOKUP = (0, 1.3, 2.0, 3.0, 4.0, 5.0, 7.0)
+    FLYWHEEL_DISTANCE_LOOKUP = (0, 1.3, 2.0, 3.0, 7.0)
     FLYWHEEL_SPEED_LOOKUP = (
         70,
         70,
         76,
         78,
-        78,
-        90,
         0,
     )
     FLYWHEEL_ANGLE_LOOKUP = (
-        0.95,
-        0.95,
-        0.80,
-        0.615,
-        0.495,
-        0.45,
+        1.02,
+        1.02,
+        0.86,
+        0.685,
         MIN_INCLINE_ANGLE,
     )
 

components/vision.py

@@ -28,6 +28,8 @@ class VisualLocalizer:
     add_to_estimator = tunable(True)
     should_log = tunable(False)
 
+    last_was_multi = tunable(False)
+
     last_pose_z = tunable(0.0, writeDefault=False)
 
     def __init__(
@@ -77,6 +79,7 @@ def execute(self) -> None:
         self.last_timestamp = timestamp
 
         if results.multiTagResult.estimatedPose.isPresent:
+            self.last_was_multi = True
             p = results.multiTagResult.estimatedPose
             pose = (Pose3d() + p.best + self.camera_to_robot).toPose2d()
             reprojectionErr = p.bestReprojError
@@ -106,6 +109,7 @@ def execute(self) -> None:
                     )
                 )
         else:
+            self.last_was_multi = False
             for target in results.getTargets():
                 # filter out likely bad targets
                 if target.getPoseAmbiguity() > 0.25:

controllers/intake.py

@@ -21,6 +21,8 @@ def try_outtake(self) -> None:
 
     @default_state
     def idling(self) -> None:
+        if not self.intake_component.is_fully_retracted():
+            self.intake_component.backdrive_intake()
         if not DriverStation.isAutonomous():
             self.intake_component.retract()
 

utilities/position.py

@@ -70,8 +70,8 @@ class ShootingPositions:
 
 
 class TeamPoses:
-    RED_TEST_POSE = Pose2d(15.1, 5.5, math.pi)
-    BLUE_TEST_POSE = field_flip_pose2d(RED_TEST_POSE)
+    RED_SUBWOOFER = Pose2d(15.175, 5.5, math.pi)
+    BLUE_SUBWOOFER = field_flip_pose2d(RED_SUBWOOFER)
     BLUE_PODIUM = Pose2d(Translation2d(2.992, 4.08455), Rotation2d(math.pi))
     RED_PODIUM = field_flip_pose2d(BLUE_PODIUM)
     RED_AMP_START_POSE = Pose2d(15.9, 6.7, math.pi)