Adding an example of a Note latch

robot.py

@@ -33,6 +33,14 @@
 from robots import crescendo as robot_config
 
 is_test = True
+
+######################################################################
+# a global latching variable to trigger when the fast moving note
+# crosses the breakbeam and stays true until we fire the note from
+# the shooter
+global latch_note
+latch_note = False
+
 ######################################################################
 
 # To run this code:
@@ -142,8 +150,15 @@ def robotInit(self):
         super().robotInit()
         self.update_test_mode()
         self._command_scheduler = commands2.CommandScheduler()
+        self.field = wpilib.Field2d()
+        # sd.putData("Field", self.field)  # TODO: Does this only need to be called once?
+
+        global latch_note
+        self.latch_note = False
+
         #self.apriltagfieldlayout = robotpy_apriltag.loadAprilTagLayoutField(
-        #    robotpy_apriltag.AprilTagField.k2024Crescendo)
+        #   robotpy_apriltag.AprilTagField.k2024Crescendo)
+
         if robot_config.physical_properties.gyro_on_spi:
             self._navx = navx.AHRS.create_spi()
         else:
@@ -176,6 +191,9 @@ def robotInit(self):
         self.driving_command.requirements = {self.swerve_drive}
         self.heading_command.requirements = {self._heading_control}
         self.try_init_mechanisms()
+
+        sd.putData("Commands", self._command_scheduler)
+
         self.define_autonomous_modes()
         self.auto_chooser = telemetry.create_selector("Autos", [auto.name for auto in self.auto_options])
 
@@ -184,19 +202,9 @@ def define_autonomous_modes(self):
         self.auto_options = [
             autos.AutoFactory("Drive Forward and backward", autos.auto_calibrations.create_drive_forward_and_back_auto,
                               (self.swerve_drive, self._x_axis_control, self._y_axis_control, self._heading_control)),
-            autos.AutoFactory("SysId: Dynamic", self.sysid.create_dynamic_measurement_command, ()),
-            autos.AutoFactory("SysId: Quasistatic", self.sysid.create_quasistatic_measurement_command, ()),
-            autos.AutoFactory("One note auto", autos.manual_autos.one_note_auto(self), (self)),
-
+            autos.AutoFactory("One Note Auto>", autos.manual_autos.one_note_auto, (self,)),
             ]
 
-        if robot_config.has_mechanisms:
-            self.auto_options.append(
-                autos.AutoFactory("One Note auto", autos.manual_autos.one_note_auto,
-                                  (self,))
-
-            )
-
     def try_init_mechanisms(self):
         """Initialize mechanisms if they are present in the robot config"""
         sd.putBoolean("Has Mechanisms", robot_config.has_mechanisms)
@@ -226,7 +234,7 @@ def try_init_mechanisms(self):
 
     def init_mechanism_telemetry(self):
         if robot_config.has_mechanisms:
-            telemetry.mechanisms_telemetry.ShowMechansimPIDs(self)
+            #telemetry.mechanisms_telemetry.ShowMechansimPIDs(self)
             self.intake_telemetry = telemetry.IntakeTelemetry(self.intake.config)
             self.indexer_telemetry = telemetry.IndexerTelemetry(self.indexer)
             self.shooter_telemetry = telemetry.ShooterTelemetry(self.shooter.config)
@@ -311,7 +319,7 @@ def reset_pose_pids_to_current_position(self):
     def autonomousInit(self):
         super().autonomousInit()
         print("Auto Init")
-        #  Hopefully at this point we've gotten an april tag fix.  Use that
+        #  Hopefully at this point we've gotten a tag fix.  Use that
         #  information to update our positioning pids
         self.reset_pose_pids_to_current_position()
         auto_path_index = self.auto_chooser.getSelected()

subsystems/indexer.py

@@ -1,6 +1,7 @@
 import rev
 import wpilib
 import commands2
+import robot
 from typing import Callable
 import hardware
 import logging
@@ -50,7 +51,10 @@ def clearNoteState(self) -> None:
 
     @property
     def ready(self) -> bool:
-        return self._read_indexer_state()
+        state = self._read_indexer_state()
+        global latch_note
+        robot.latch_note = True
+        return state
 
     @property
     def voltage(self):

swerve/swervedrive.py

@@ -1,18 +1,11 @@
-import abc
 import math
 import threading
-from collections.abc import Iterable, Sequence, Set
+from collections.abc import Sequence, Set
 import logging
-import wpilib
-import wpimath
 import wpimath.units
-import rev
 import navx
 from .swervemodule import SwerveModule
-import config
 from config import ModulePosition, PhysicalConfig, SwerveModuleConfig
-import time
-from typing import NamedTuple, Callable, Any
 import wpimath.kinematics as kinematics
 import wpimath.geometry as geom
 import wpimath.estimator as estimator
@@ -23,7 +16,7 @@
 
 
 class SwerveDrive(commands2.subsystem.Subsystem):
-    """Abstract base class for a sweve drive."""
+    """Abstract base class for a swerve drive."""
     _modules: dict[ModulePosition, ISwerveModule]
 
     initialized: bool = False  # True if the swerve drive has been initialized at least once
@@ -33,11 +26,11 @@ class SwerveDrive(commands2.subsystem.Subsystem):
     logger: logging.Logger
 
     _ordered_modules: list[ISwerveModule]
-    _odemetry: estimator.SwerveDrive4PoseEstimator
+    _odometry: estimator.SwerveDrive4PoseEstimator
 
     _physical_config: PhysicalConfig
 
-    _odemetry_lock: threading.Lock = threading.Lock()
+    _odometry_lock: threading.Lock = threading.Lock()
 
     # Automatically invert the gyro if the physical properties say it is inverted without
     # having to check the physical properties every time the gyro is accessed.\
@@ -70,7 +63,7 @@ def __init__(self, gyro: navx.AHRS, swerve_config: dict[ModulePosition, SwerveMo
         # 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
         # The default standard deviations of the vision measurements are
         # 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.
-        self._odemetry = estimator.SwerveDrive4PoseEstimator(self._kinematics,
+        self._odometry = estimator.SwerveDrive4PoseEstimator(self._kinematics,
                                                              geom.Rotation2d.fromDegrees(self.gyro_angle_degrees),
                                                              module_positions,  # type: ignore
                                                              geom.Pose2d(0, 0, geom.Rotation2d(0)))
@@ -82,12 +75,13 @@ def __init__(self, gyro: navx.AHRS, swerve_config: dict[ModulePosition, SwerveMo
 
     #        commands2.CommandScheduler.getInstance().registerSubsystem(self)
 
-#gyro
+    # Gyro
 
     @property
     def gyro_angle_radians(self) -> wpimath.units.radians:
         # return math.radians(self.gyro_angle_degrees + self.offset())
         return math.radians(self.gyro_angle_degrees)
+
     @property
     def gyro_angle_degrees(self) -> wpimath.units.degrees:
         # return math_help.wrap_angle_degrees(self.__gyro_get_lambda() + self.offset() * 180 / math.pi)
@@ -96,12 +90,7 @@ def gyro_angle_degrees(self) -> wpimath.units.degrees:
     def gyro_reset(self):
         self.gyro.reset()
 
-
-
-
-
-
-#module
+    # Module
     @property
     def num_modules(self) -> int:
         return len(self._ordered_modules)
@@ -113,15 +102,14 @@ def modules(self) -> dict[ModulePosition, ISwerveModule]:
     @property
     def estimated_position(self) -> geom.Pose2d:
         """Get the pose estimators best guess about where the robot is"""
-        with self._odemetry_lock:
-            return self._odemetry.getEstimatedPosition()
+        with self._odometry_lock:
+            return self._odometry.getEstimatedPosition()
 
     @property
     def ordered_modules(self) -> list[SwerveModule]:
         """Provides a consistent ordering of modules for use with wpilib swerve functions"""
         return self._ordered_modules
 
-
     def initialize(self):
         """Initialize the swerve drive.  Needs to be called repeatedly until it returns True."""
         results = [module.initialize() for module in self._modules.values()]
@@ -132,15 +120,16 @@ def initialize(self):
         return False
 
     def update_odometry(self):
-        with self._odemetry_lock:
-            self._odemetry.update(geom.Rotation2d.fromDegrees(self.gyro_angle_degrees),  # type: ignore
+        with self._odometry_lock:
+            self._odometry.update(geom.Rotation2d.fromDegrees(self.gyro_angle_degrees),  # type: ignore
                                   self._measured_module_positions)  # type: ignore
 
     def _scale_velocity_to_drive_speed(self, v_x: float, v_y: float) -> tuple[float, float]:
         """Reduce requested speed if it is too fast"""
         requested_speed = math.sqrt(v_x ** 2 + v_y ** 2)
 
-        # Scale the vector vx, vy so that the magnitude of the vector does not exceed robot_config.physical_properties.max_drive_speed
+        # Scale the vector vx, vy so that the magnitude of the vector does not exceed
+        #   robot_config.physical_properties.max_drive_speed
         if requested_speed > self._physical_config.max_drive_speed:
             scale_factor = self._physical_config.max_drive_speed / requested_speed
             v_x *= scale_factor
@@ -152,20 +141,25 @@ def drive(self, v_x: float, v_y: float, rotation: wpimath.units.radians_per_seco
               run_modules: Sequence[ModulePosition] | Set[ModulePosition] | None = None):
         """
         Drive the robot using cartesian coordinates
-        :param run_modules: A set of modules to drive.  If None, all modules will be driven.  This is useful for testing individual modules and ensuring ModulePosition is correct for each module
+        :param v_x: X Velocity
+        :param v_y: Y Velocity
+        :param rotation: Rotation (in radians)
+        :param run_modules: A set of modules to drive.  If None, all modules will be driven.
+           This is useful for testing individual modules and ensuring ModulePosition is correct for each module
         """
         v_x, v_y = self._scale_velocity_to_drive_speed(v_x, v_y)
-        desired_chasis_speeds = kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(v_x, v_y, rotation, geom.Rotation2d(
+        desired_chassis_speeds = kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(v_x, v_y, rotation, geom.Rotation2d(
             -self.gyro_angle_radians))
-        self.drive_with_chassis_speeds(desired_chasis_speeds, run_modules)
+        self.drive_with_chassis_speeds(desired_chassis_speeds, run_modules)
 
-    def drive_with_chassis_speeds(self, desired_chasis_speeds: kinematics.ChassisSpeeds,
+    def drive_with_chassis_speeds(self, desired_chassis_speeds: kinematics.ChassisSpeeds,
                                   run_modules: Sequence[ModulePosition] | Set[ModulePosition] | None = None):
-        module_states = self._kinematics.toSwerveModuleStates(desired_chasis_speeds)
+        module_states = self._kinematics.toSwerveModuleStates(desired_chassis_speeds)
         self.drive_with_module_states(module_states, run_modules=run_modules)
 
     def drive_with_module_states(self, module_states: tuple[
-        kinematics.SwerveModuleState, kinematics.SwerveModuleState, kinematics.SwerveModuleState, kinematics.SwerveModuleState],
+        kinematics.SwerveModuleState, kinematics.SwerveModuleState, kinematics.SwerveModuleState,
+                                 kinematics.SwerveModuleState],
                                  run_modules: Sequence[ModulePosition] | Set[ModulePosition] | None = None):
         """
         Drive the robot using module states.  Module states must be passed in the same order as self._ordered_modules
@@ -194,13 +188,12 @@ def lock_wheels(self):
         self._modules[ModulePosition.back_right].desired_state = kinematics.SwerveModuleState(0, geom.Rotation2d(
             math.pi + quarter_pi))
 
-
-# pose
+    # Pose
     @property
     def pose(self) -> geom.Pose2d:
         """Current pose of the robot"""
-        with self._odemetry_lock:
-            return self._odemetry.getEstimatedPosition()
+        with self._odometry_lock:
+            return self._odometry.getEstimatedPosition()
 
     @pose.setter
     def pose(self, value: geom.Pose2d):
@@ -209,41 +202,41 @@ def pose(self, value: geom.Pose2d):
     def reset_pose(self, pose: geom.Pose2d):
         """Force the robot to a specific pose on the field.  This is good to call at startup when we get our
         first positioning data from the camera."""
-        with self._odemetry_lock:
-            self._odemetry.resetPosition(geom.Rotation2d.fromDegrees(self.gyro_angle_degrees),
+        with self._odometry_lock:
+            self._odometry.resetPosition(geom.Rotation2d.fromDegrees(self.gyro_angle_degrees),
                                          # possible cause of teleporting position on field
                                          self._measured_module_positions,
                                          pose)
 
-# module state
+    # Module state
     @property
     def _measured_module_states(self) -> Sequence[kinematics.SwerveModuleState]:
         """Current state of the modules"""
         return tuple([m.measured_state for m in self._ordered_modules])
 
     @property
     def _measured_module_positions(self) -> tuple[kinematics.SwerveModulePosition,
-    kinematics.SwerveModulePosition,
-    kinematics.SwerveModulePosition,
-    kinematics.SwerveModulePosition]:
+                                                  kinematics.SwerveModulePosition,
+                                                  kinematics.SwerveModulePosition,
+                                                  kinematics.SwerveModulePosition]:
         """Current state of the modules"""
         return tuple([m.position for m in self._ordered_modules])  # type: ignore
 
-# chassis
+    # Chassis
     @property
     def measured_chassis_speed(self) -> kinematics.ChassisSpeeds:
         """Current chassis speed of the robot"""
         return self._kinematics.toChassisSpeeds(self._measured_module_states)  # type: ignore
 
     @property
     def desired_chassis_speed(self) -> kinematics.ChassisSpeeds:
-        """chasis speeds desired by the robot"""
+        """chassis speeds desired by the robot"""
         return self._kinematics.toChassisSpeeds(tuple([m.desired_state for m in self._ordered_modules]))
 
     def add_vision_measurement(self, pose: geom.Pose2d, timestamp: float):
-        """Add a vision measurement to the odemetry"""
-        with self._odemetry_lock:
-            self._odemetry.addVisionMeasurement(pose, timestamp)
+        """Add a vision measurement to the odometry"""
+        with self._odometry_lock:
+            self._odometry.addVisionMeasurement(pose, timestamp)
 
     def drive_set_distance(self, meters: float, angle: float):
         """Drive the robot a certain distance and angle in meters and radians"""
@@ -253,7 +246,7 @@ def drive_set_distance(self, meters: float, angle: float):
     def drive_at_voltage(self, voltage: float):
         """Provide the drive motors a set voltage at the requested angle"""
         for module in self._modules.values():
-            module.drive_at_voltage(voltage)
+            module.drive_at_voltage(voltage, 0.0)
 
     def log_to_sysid(self, log: wpilib.sysid.SysIdRoutineLog):
         """Log the state of the swerve drive for system identification"""
@@ -264,4 +257,4 @@ def log_to_sysid(self, log: wpilib.sysid.SysIdRoutineLog):
     def velocity(self) -> float:
         """Get the velocity of the robot"""
         chassis_speed = self.measured_chassis_speed
-        return math.sqrt(chassis_speed.vx ** 2 + chassis_speed.vy ** 2)
+        return math.sqrt(chassis_speed.vx ** 2 + chassis_speed.vy ** 2)

telemetry/indexer_telemetry.py

@@ -2,6 +2,7 @@
 from telemetry import FloatEntry
 import config
 import ntcore
+import robot
 from wpilib import SmartDashboard as sd
 
 
@@ -36,6 +37,9 @@ def __init__(self, indexer: subsystems.Indexer):
                                                    set_value=self.set_outtake_velocity)
         sd.putBoolean("Intake Sensor", indexer.last_sensor_state)
 
+        global latch_note
+        sd.putBoolean("Note Latch", robot.latch_note)
+
     def periodic(self):
         self._velocity_entry.periodic()
         self._shoot_entry.periodic()

telemetry/mechanisms_telemetry.py

@@ -20,4 +20,3 @@ def ShowMechansimPIDs(r):
     #sd.putData("Shooter PID", shooter_pid_entry)
     # sd.putData("Climber PID", climber_pid_entry)
 
-

tests/note_latch_test.py

@@ -0,0 +1,6 @@
+import unittest
+import commands
+
+class note_latch_test(unittest.TestCase):
+    def test_note_basic_intake(self):
+        commands.IntakeOn