Merge pull request #198 from thedropbears/rolling_comp

Rolling comp

autonomous/autonomous.py

@@ -33,7 +33,11 @@ def __init__(self) -> None:
             Path([NotePositions.amp], face_target=True),
             Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
         ]
-        super().__init__(note_paths, shoot_paths)
+        start_pose = Pose2d(
+            TeamPoses.RED_TEST_POSE.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+        )
+        super().__init__(note_paths, shoot_paths, start_pose)
 
 
 class PodiumSpeakerAmp(AutoBase):
@@ -72,11 +76,9 @@ def __init__(self) -> None:
             Path([NotePositions.amp], face_target=True),
             Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
         ]
-        # 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_AMP_START_POSE.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_AMP_START_POSE.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
@@ -99,8 +101,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_AMP_START_POSE.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_AMP_START_POSE.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
@@ -124,8 +126,6 @@ def __init__(self) -> None:
                 face_target=True,
             ),
         ]
-        # 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()),
@@ -160,8 +160,6 @@ def __init__(self) -> None:
                 face_target=True,
             ),
         ]
-        # 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()),
@@ -175,18 +173,29 @@ class Centre5Centre4(AutoBase):
 
     def __init__(self) -> None:
         note_paths = [
-            Path([NotePositions.Centre5], face_target=False),
             Path(
                 [
+                    PathPositions.avoid_starting_faults,
                     PathPositions.avoid_stage_S,
+                    NotePositions.Centre5,
+                ],
+                face_target=False,
+            ),
+            Path(
+                [
+                    PathPositions.avoid_stage_S,
+                    PathPositions.enforce_pickup_angle,
                     NotePositions.Centre4,
                 ],
                 face_target=False,
             ),
         ]
 
         shoot_paths = [
-            Path([ShootingPositions.source_side], face_target=True),
+            Path(
+                [PathPositions.avoid_stage_S, ShootingPositions.source_side],
+                face_target=True,
+            ),
             Path(
                 [
                     PathPositions.avoid_stage_S,
@@ -195,7 +204,7 @@ def __init__(self) -> None:
                 face_target=True,
             ),
         ]
-        sim_start_pos = Translation2d(15.4, 2.94)
+        sim_start_pos = ShootingPositions.source_side
         rotation = rotation_to_red_speaker(sim_start_pos)
         sim_start_pose = Pose2d(sim_start_pos, rotation)
         super().__init__(note_paths, shoot_paths, sim_start_pose)
@@ -207,10 +216,18 @@ class Centre5Centre4Centre3(AutoBase):
 
     def __init__(self) -> None:
         note_paths = [
-            Path([NotePositions.Centre5], face_target=False),
+            Path(
+                [
+                    PathPositions.avoid_starting_faults,
+                    PathPositions.avoid_stage_S,
+                    NotePositions.Centre5,
+                ],
+                face_target=False,
+            ),
             Path(
                 [
                     PathPositions.avoid_stage_S,
+                    PathPositions.enforce_pickup_angle,
                     NotePositions.Centre4,
                 ],
                 face_target=False,
@@ -226,7 +243,10 @@ def __init__(self) -> None:
         ]
 
         shoot_paths = [
-            Path([ShootingPositions.source_side], face_target=True),
+            Path(
+                [PathPositions.avoid_stage_S, ShootingPositions.source_side],
+                face_target=True,
+            ),
             Path(
                 [
                     PathPositions.avoid_stage_S,
@@ -243,7 +263,7 @@ def __init__(self) -> None:
                 face_target=True,
             ),
         ]
-        sim_start_pos = Translation2d(15.4, 2.94)
+        sim_start_pos = ShootingPositions.source_side
         rotation = rotation_to_red_speaker(sim_start_pos)
         sim_start_pose = Pose2d(sim_start_pos, rotation)
         super().__init__(note_paths, shoot_paths, sim_start_pose)

autonomous/base.py

@@ -152,13 +152,11 @@ def drive_and_shoot(self, state_tm: float, initial_call: bool) -> None:
         if self.is_close_enough_to_shoot():
             self.note_manager.try_shoot()
 
-        if self.note_manager.has_just_fired() or (
-            self.is_at_goal() and not self.note_manager.has_note()
-        ):
-            if len(self.shoot_paths_working_copy) != 0:
-                self.next_state("pick_up")
-            else:
-                self.done()
+        if (
+            self.note_manager.has_just_fired()
+            or (self.is_at_goal() and not self.note_manager.has_note())
+        ) and len(self.shoot_paths_working_copy) != 0:
+            self.next_state("pick_up")
 
     def drive_on_trajectory(self, trajectory_tm: float):
         if not self.trajectory:

components/chassis.py

@@ -294,6 +294,10 @@ def __init__(self) -> None:
     def get_velocity(self) -> ChassisSpeeds:
         return self.kinematics.toChassisSpeeds(self.get_module_states())
 
+    @feedback
+    def imu_rotation(self) -> float:
+        return self.imu.getAngle()
+
     def get_module_states(
         self,
     ) -> tuple[
@@ -315,7 +319,7 @@ def setup(self) -> None:
             self.get_module_positions(),
             initial_pose,
             stateStdDevs=(0.05, 0.05, 0.01),
-            visionMeasurementStdDevs=(0.4, 0.4, 50),
+            visionMeasurementStdDevs=(0.4, 0.4, 0.03),
         )
         self.field_obj = self.field.getObject("fused_pose")
         self.set_pose(initial_pose)

components/led.py

@@ -14,7 +14,7 @@
 Hsv = tuple[int, int, int]
 
 FLASH_SPEED = 2
-BREATHE_SPEED = 4
+BREATHE_SPEED = 0.5
 RAINBOW_SPEED = 8
 MORSE_SPEED = 0.2
 

components/shooter.py

@@ -47,17 +47,17 @@ class ShooterComponent:
         70,
         70,
         76,
-        76,
-        77,
+        78,
+        78,
         90,
         0,
     )
     FLYWHEEL_ANGLE_LOOKUP = (
-        0.96,
-        0.96,
+        0.95,
+        0.95,
         0.80,
-        0.61,
-        0.50,
+        0.615,
+        0.495,
         0.45,
         MIN_INCLINE_ANGLE,
     )

controllers/shooter.py

@@ -22,8 +22,8 @@ class Shooter(StateMachine):
 
     data_log: DataLog
 
-    SPEED_LIMIT = tunable(1)
-    SPINNING_SPEED_LIMIT = tunable(1)
+    SPEED_LIMIT = tunable(0.1)
+    SPINNING_SPEED_LIMIT = tunable(0.1)
 
     def __init__(self):
         self.range = 0.0

robot.py

@@ -22,7 +22,7 @@
 from utilities.game import is_red
 from utilities.scalers import rescale_js
 from utilities.functions import clamp
-from utilities.position import on_same_side_of_stage, y_close_to_stage
+from utilities.position import distance_between
 
 
 class MyRobot(magicbot.MagicRobot):
@@ -39,14 +39,16 @@ class MyRobot(magicbot.MagicRobot):
 
     status_lights: LightStrip
 
-    max_speed = magicbot.tunable(4)  # m/s
+    max_speed = magicbot.tunable(5)  # m/s
     lower_max_speed = magicbot.tunable(2)  # m/s
     max_spin_rate = magicbot.tunable(4)  # m/s
     lower_max_spin_rate = magicbot.tunable(2)  # m/s
     inclination_angle = tunable(0.0)
     vision_port: VisualLocalizer
     vision_starboard: VisualLocalizer
 
+    START_POS_TOLERANCE = 1
+
     def createObjects(self) -> None:
         self.data_log = wpilib.DataLogManager.getLog()
 
@@ -71,7 +73,7 @@ def createObjects(self) -> None:
         )
 
     def teleopInit(self) -> None:
-        pass
+        self.field.getObject("Intended start pos").setPoses([])
 
     def teleopPeriodic(self) -> None:
         if self.climber.should_lock_mechanisms():
@@ -84,13 +86,13 @@ def teleopPeriodic(self) -> None:
         # Set max speed
         max_speed = self.max_speed
         max_spin_rate = self.max_spin_rate
-        if self.gamepad.getXButton():
+        if self.gamepad.getRightBumper():
             max_speed = self.lower_max_speed
             max_spin_rate = self.lower_max_spin_rate
 
         # Driving
-        drive_x = -rescale_js(self.gamepad.getLeftY(), 0.1) * max_speed
-        drive_y = -rescale_js(self.gamepad.getLeftX(), 0.1) * max_speed
+        drive_x = -rescale_js(self.gamepad.getLeftY(), 0.05, 2.5) * max_speed
+        drive_y = -rescale_js(self.gamepad.getLeftX(), 0.05, 2.5) * max_speed
         drive_z = (
             -rescale_js(self.gamepad.getRightX(), 0.1, exponential=2) * max_spin_rate
         )
@@ -204,6 +206,7 @@ def disabledPeriodic(self) -> None:
         if (
             not self.vision_port.sees_target()
             and not self.vision_starboard.sees_target()
+            and not self.isSimulation()
         ):
             self.status_lights.no_vision()
         else:
@@ -213,11 +216,14 @@ def disabledPeriodic(self) -> None:
                 intended_start_pose = selected_auto.get_starting_pose()
                 current_pose = self.chassis.get_pose()
                 if intended_start_pose is not None:
-                    if on_same_side_of_stage(intended_start_pose, current_pose):
-                        if y_close_to_stage(current_pose):
-                            self.status_lights.too_close_to_stage()
-                        else:
-                            self.status_lights.rainbow()
+                    self.field.getObject("Intended start pos").setPose(
+                        intended_start_pose
+                    )
+                    if (
+                        distance_between(intended_start_pose, current_pose)
+                        < self.START_POS_TOLERANCE
+                    ):
+                        self.status_lights.rainbow()
                     else:
                         self.status_lights.invalid_start()
                 else:
@@ -226,7 +232,7 @@ def disabledPeriodic(self) -> None:
                 self.status_lights.missing_start_pose()
 
     def autonomousInit(self) -> None:
-        pass
+        self.field.getObject("Intended start pos").setPoses([])
 
 
 if __name__ == "__main__":

utilities/position.py

@@ -34,7 +34,7 @@ def __init__(self, waypoints: list[Translation2d], face_target: bool):
             self.final_heading = 0
 
 
-stage_tolerance = 0.35
+stage_tolerance = 0.4
 
 
 class NotePositions:
@@ -66,30 +66,26 @@ class ShootingPositions:
     amp_speaker_bounce = Translation2d(
         14.7, (NotePositions.amp.y + NotePositions.speaker.y) / 2
     )
-    source_side = Translation2d(14.7, 2.8)
+    source_side = Translation2d(15.556, 4.034)
 
 
 class TeamPoses:
     RED_TEST_POSE = Pose2d(15.1, 5.5, math.pi)
     BLUE_TEST_POSE = field_flip_pose2d(RED_TEST_POSE)
     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)
 
 
-def on_same_side_of_stage(intended_start_pose: Pose2d, current_pose: Pose2d) -> bool:
-    return not (
-        (intended_start_pose.y > TeamPoses.BLUE_PODIUM.y)
-        ^ (current_pose.y > TeamPoses.BLUE_PODIUM.y)
-    )
-
-
-def y_close_to_stage(pose: Pose2d) -> bool:
-    return abs(pose.y - TeamPoses.BLUE_PODIUM.y) < 0.9
+def distance_between(intended_start_pose: Pose2d, current_pose: Pose2d) -> float:
+    return (intended_start_pose.translation() - current_pose.translation()).norm()
 
 
 class PathPositions:
     stage_transition_N = Translation2d(11.4, 4.5)
     stage_transition_S = Translation2d(11.4, 3.74)
     stage_transition_S_entry = Translation2d(13.0, 2.5)
     avoid_wall = Translation2d(10.80, 6.55)
-    avoid_stage_S = Translation2d(10.66, 1.40)
+    avoid_stage_S = Translation2d(11.66, 1.40)
+    avoid_starting_faults = Translation2d(14.429, 2.946)
+    enforce_pickup_angle = Translation2d(9.6, 1.6)