Merge branch 'main' into remove-wrist-limit-switch

Author: LeanMeanBeanMachine4774

.github/workflows/ci.yml

@@ -6,6 +6,7 @@ on:
       - main
   pull_request:
   merge_group:
+  workflow_dispatch:
 
 jobs:
   test:

.pre-commit-config.yaml

@@ -7,7 +7,7 @@ minimum_pre_commit_version: '3.2.0'
 
 repos:
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v5.0.0
+    rev: v6.0.0
     hooks:
       # Side effects
       - id: fix-byte-order-marker
@@ -22,7 +22,7 @@ repos:
       - id: check-yaml
 
   - repo: https://github.com/astral-sh/ruff-pre-commit
-    rev: v0.12.0
+    rev: v0.12.11
     hooks:
       - id: ruff
         args:

README.md

@@ -1,4 +1,4 @@
-# py2025
+# pyreefscape
 
 The Drop Bears' robot code for FRC 2025
 
@@ -11,19 +11,14 @@ This includes the Python version, and any Python packages such as `wpilib`.
 
 Install `uv` by following the [`uv` docs](https://docs.astral.sh/uv/).
 
-After installing `uv`, use it to create a virtual environment and install our dependencies.
+After installing `uv`, download the roboRIO dependencies.
 
 ```sh
-uv sync
-```
-
-Then, download the roboRIO dependencies.
-
-```sh
-uv run python -m ensurepip
 uv run robotpy sync --no-install
 ```
 
+`uv run` will automatically create a virtual environment and install our dependencies for local development.
+
 ### pre-commit
 
 [pre-commit][] is configured to run our formatters and linters.
@@ -33,7 +28,7 @@ To use pre-commit, you must install it outside of this project's virtual environ
 Either use your system package manager, or use `uv tool`:
 
 ```sh
-uv tool install pre-commit
+uv tool install pre-commit --with pre-commit-uv
 ```
 
 You can then set up the pre-commit hooks to run on commit:
@@ -59,15 +54,13 @@ pre-commit install
 
 ### Simulation
 
-Before your first run, copy the `*.json.orig` files to the main directory and remove the `.orig` extension.
-
 ```
 uv run robotpy sim
 ```
 
 ### Deploy to Robot
 
-Once on robots network
+First, connect to the robot network. Then run:
 
 ```
 uv run robotpy deploy

autonomous/auto_base.py

@@ -3,14 +3,13 @@
 import choreo
 import wpilib
 from choreo.trajectory import SwerveSample, SwerveTrajectory
-from magicbot import AutonomousStateMachine, state, timed_state
+from magicbot import AutonomousStateMachine, state, timed_state, tunable
 from wpilib import RobotBase
 from wpimath.controller import PIDController
 from wpimath.geometry import Pose2d
 from wpimath.kinematics import ChassisSpeeds
 
 from components.chassis import ChassisComponent
-from components.coral_depositor import CoralDepositorComponent
 from components.injector import InjectorComponent
 from components.shooter import ShooterComponent
 from controllers.algae_shooter import AlgaeShooter
@@ -28,19 +27,25 @@ class AutoBase(AutonomousStateMachine):
     algae_shooter: AlgaeShooter
     reef_intake: ReefIntake
 
-    coral_depositor_component: CoralDepositorComponent
     shooter_component: ShooterComponent
     injector_component: InjectorComponent
     chassis: ChassisComponent
 
     field: wpilib.Field2d
 
     DISTANCE_TOLERANCE = 0.1  # metres
+    SHOOT_DISTANCE_TOLERANCE = 0.4  # metres
+    SHOOT_ANGLE_TOLERANCE = math.radians(10)
     ANGLE_TOLERANCE = math.radians(3)
     CORAL_DISTANCE_TOLERANCE = 0.2  # metres
     TRANSLATIONAL_SPEED_TOLERANCE = 0.2
     ROTATIONAL_SPEED_TOLERANCE = 0.1
 
+    is_shooting_leg = tunable(False)
+    is_in_distance_tolerance = tunable(False)
+    is_in_angle_tolerance = tunable(False)
+    is_in_second_half_of_leg = tunable(False)
+
     def __init__(self, trajectory_names: list[str]) -> None:
         # We want to parameterise these by paths and potentially a sequence of events
         super().__init__()
@@ -59,6 +64,7 @@ def __init__(self, trajectory_names: list[str]) -> None:
 
     def setup(self) -> None:
         #  setup path tracking controllers
+        self.auto_sample_field_obj = self.field.getObject("auto_sample")
 
         # init any other defaults
         pass
@@ -106,49 +112,71 @@ def tracking_trajectory(self, initial_call, state_tm) -> None:
         if initial_call:
             self.current_leg += 1
 
-        if self.current_leg == len(self.trajectories):
-            self.done()
-            return
+            if self.current_leg == len(self.trajectories):
+                self.done()
+                return
+
+            trajectory = (
+                self.trajectories[self.current_leg].flipped()
+                if game.is_red()
+                else self.trajectories[self.current_leg]
+            )
+
+            trajectory_poses = trajectory.get_poses()
+            self.field.getObject("trajectory").setPoses(trajectory_poses)
 
-        # get next leg on entry
-        current_pose = self.chassis.get_pose()
         final_pose = self.trajectories[self.current_leg].get_final_pose(game.is_red())
         if final_pose is None:
             self.done()
             return
 
+        # get next leg on entry
+        current_pose = self.chassis.get_pose()
+
         distance = current_pose.translation().distance(final_pose.translation())
         angle_error = (final_pose.rotation() - current_pose.rotation()).radians()
-        velocity = self.chassis.get_velocity()
-        speed = math.sqrt(math.pow(velocity.vx, 2.0) + math.pow(velocity.vy, 2.0))
 
-        if self.current_leg % 2 != 0:
+        self.is_shooting_leg = self.current_leg % 2 != 0
+
+        if self.is_shooting_leg:
             self.algae_shooter.shoot()
         else:
             self.reef_intake.intake()
         # Check if we are close enough to deposit coral
         if distance < self.CORAL_DISTANCE_TOLERANCE:
             self.reef_intake.holding_coral = False
 
+        self.is_in_distance_tolerance = (
+            distance < self.SHOOT_DISTANCE_TOLERANCE
+            if self.is_shooting_leg
+            else distance < self.DISTANCE_TOLERANCE
+        )
+
+        self.is_in_angle_tolerance = (
+            math.isclose(angle_error, 0.0, abs_tol=self.SHOOT_ANGLE_TOLERANCE)
+            if self.is_shooting_leg
+            else math.isclose(angle_error, 0.0, abs_tol=self.ANGLE_TOLERANCE)
+        )
+        self.is_in_second_half_of_leg = (
+            state_tm > self.trajectories[self.current_leg].get_total_time() / 2.0
+        )
+
         if (
-            distance < self.DISTANCE_TOLERANCE
-            and math.isclose(angle_error, 0.0, abs_tol=self.ANGLE_TOLERANCE)
-            and math.isclose(speed, 0.0, abs_tol=self.TRANSLATIONAL_SPEED_TOLERANCE)
-            and math.isclose(
-                velocity.omega, 0.0, abs_tol=self.ROTATIONAL_SPEED_TOLERANCE
-            )
-            and state_tm > self.trajectories[self.current_leg].get_total_time() / 2.0
+            self.is_in_distance_tolerance
+            and self.is_in_angle_tolerance
+            and self.chassis.is_stationary()
+            and self.is_in_second_half_of_leg
         ):
             # run cycles of pick up -> shoot
-            if self.injector_component.has_algae():
+            if self.is_shooting_leg:
                 self.next_state("shooting_algae")
             else:
                 self.next_state("intaking_algae")
-            return
 
         sample = self.trajectories[self.current_leg].sample_at(state_tm, game.is_red())
         if sample is not None:
             self.follow_trajectory(sample)
+            self.auto_sample_field_obj.setPose(sample.get_pose())
 
     def follow_trajectory(self, sample: SwerveSample):
         # track path
@@ -170,15 +198,15 @@ def follow_trajectory(self, sample: SwerveSample):
 
     @timed_state(duration=1.0, next_state="tracking_trajectory")
     def intaking_algae(self) -> None:
-        if self.current_leg == 0:
-            self.coral_depositor_component.deposit()
         if self.injector_component.has_algae():
-            self.coral_depositor_component.tuck()
             self.next_state("tracking_trajectory")
 
     @timed_state(duration=1.0, next_state="tracking_trajectory")
     def shooting_algae(self) -> None:
         self.algae_shooter.shoot()
 
-        if not self.algae_shooter.is_executing:
+        if (
+            not self.injector_component.has_algae()
+            and not self.algae_shooter.is_executing
+        ):
             self.next_state("tracking_trajectory")

components/ballistics.py

@@ -48,19 +48,27 @@ class BallisticsComponent:
     # Tuples are values corresponding to the distances above
     # fmt: off
     FLYWHEEL_TOP_SPEED_LOOKUP = {
-        ALGAE_MIN_DIAMETER: (17, 19, 26.0),
-        ALGAE_MAX_DIAMETER: (21, 22, 24.0),
+        ALGAE_MIN_DIAMETER: (19, 22, 26.0),
+        ALGAE_MAX_DIAMETER: (25, 28, 28.0),
     }
 
     FLYWHEEL_BOTTOM_SPEED_LOOKUP = {
-        ALGAE_MIN_DIAMETER: (33, 36, 44.0),
-        ALGAE_MAX_DIAMETER: (36, 37, 44.0),
+        ALGAE_MIN_DIAMETER: (35, 37, 42.0),
+        ALGAE_MAX_DIAMETER: (40, 40, 44.0),
     }
 
     FLYWHEEL_ANGLE_LOOKUP = {
-        ALGAE_MIN_DIAMETER: (math.radians(-10), math.radians(-15), math.radians(-25)),
+        ALGAE_MIN_DIAMETER: (
+            math.radians(-10),
+            math.radians(-15),
+            math.radians(-25)
+        ),
         # 16.5: (math.radians(-11.0), math.radians(-19), -25),
-        ALGAE_MAX_DIAMETER: (math.radians(-10), math.radians(-15), math.radians(-25)),
+        ALGAE_MAX_DIAMETER: (
+            math.radians(-10),
+            math.radians(-15),
+            math.radians(-25)
+        ),
     }
     # fmt: on
     BALL_SIZES = list(FLYWHEEL_ANGLE_LOOKUP.keys())
@@ -104,7 +112,7 @@ def corrected_range(self) -> float:
             robot_to_barge_X_offset
             / (robot_pose.rotation() + self.robot_to_shooter).cos()
         )
-        return distance + 0.3
+        return distance
 
     @feedback
     def is_aligned(self) -> bool:

components/chassis.py

@@ -5,7 +5,7 @@
 import magicbot
 import ntcore
 import wpilib
-from magicbot import feedback
+from magicbot import feedback, tunable
 from phoenix6.configs import (
     ClosedLoopGeneralConfigs,
     FeedbackConfigs,
@@ -28,6 +28,8 @@
     SwerveModuleState,
 )
 
+import utilities
+import utilities.scalers
 from ids import CancoderId, TalonId
 from utilities.ctre import FALCON_FREE_RPS
 from utilities.functions import rate_limit_module
@@ -229,6 +231,9 @@ class ChassisComponent:
 
     DRIVE_CURRENT_THRESHOLD = 35
 
+    MIN_ALIGN_Y_AXIS_SPEED = tunable(0.3)
+    MAX_ALIGN_Y_AXIS_SPEED = tunable(1.5)
+
     HEADING_TOLERANCE = math.radians(1)
 
     swerve_config: SwerveConfig
@@ -381,6 +386,23 @@ def drive_local(self, vx: float, vy: float, omega: float) -> None:
         """Robot oriented drive commands"""
         self.chassis_speeds = ChassisSpeeds(vx, vy, omega)
 
+    def align_on_y(self, offset: float, distance_tol: float, angle_tol: float) -> None:
+        if math.isclose(offset, 0.0, abs_tol=distance_tol):
+            self.chassis_speeds.vy = 0
+
+        elif math.isclose(self.heading_controller.getError(), 0.0, abs_tol=angle_tol):
+            # move in direction opposite to offset, proportional to offset
+            self.chassis_speeds.vy = -math.copysign(
+                utilities.scalers.scale_value(
+                    abs(offset),
+                    0,
+                    1.5,
+                    self.MIN_ALIGN_Y_AXIS_SPEED,
+                    self.MAX_ALIGN_Y_AXIS_SPEED,
+                ),
+                offset,
+            )
+
     def limit_to_positive_longitudinal_velocity(self) -> None:
         self.chassis_speeds.vy = 0.0
         self.chassis_speeds.omega = 0.0

components/coral_depositor.py

@@ -19,6 +19,9 @@ class InjectorComponent:
     INJECTOR_RPS_TOLERANCE = 0.5
     INJECTOR_MAX_ACCEL = 0.5
 
+    cycle_counter = tunable(0)
+    cycle_segment = tunable("intaking")
+
     def __init__(self) -> None:
         self.algae_limit_switch = DigitalInput(DioChannel.ALGAE_INTAKE_SWITCH)
 
@@ -55,6 +58,14 @@ def __init__(self) -> None:
 
     def on_enable(self) -> None:
         self.has_seen_algae = False
+        self.cycle_counter = 0
+
+    def increment_segment(self) -> None:
+        if self.cycle_segment == "intaking":
+            self.cycle_segment = "shooting"
+        else:
+            self.cycle_segment = "intaking"
+            self.cycle_counter += 1
 
     @feedback
     def _algae_limit_switch_pressed(self) -> bool: