constants

 7class RobotConstants:
 8    massKG: float = 63.9565
 9    # MOI: Moment of inertia, kg*m^2
10    MOI: float = 5.94175290870316

 81class CameraGeometry:
 82    """A vision camera on the robot.
 83
 84    Stores the canonical WPILib Transform3d plus the original inch/degree
 85    values for display, serialization, and the visualizer.
 86    """
 87
 88    __slots__ = ('name', 'robot_to_camera', 'fov_deg',
 89                 '_x_in', '_y_in', '_z_in',
 90                 '_roll_deg', '_pitch_up_deg', '_yaw_deg')
 91
 92    def __init__(
 93        self,
 94        name: str,
 95        robot_to_camera: Transform3d,
 96        fov_deg: float = 70.0,
 97        *,
 98        x_in: float = 0, y_in: float = 0, z_in: float = 0,
 99        roll_deg: float = 0, pitch_up_deg: float = 0, yaw_deg: float = 0,
100    ):
101        self.name = name
102        self.robot_to_camera = robot_to_camera
103        self.fov_deg = fov_deg
104        self._x_in = x_in
105        self._y_in = y_in
106        self._z_in = z_in
107        self._roll_deg = roll_deg
108        self._pitch_up_deg = pitch_up_deg
109        self._yaw_deg = yaw_deg
110
111    def to_dict(self) -> dict:
112        """Serialize for the visualizer JSON."""
113        t = self.robot_to_camera.translation()
114        return {
115            "name": self.name,
116            "x_inches": self._x_in,
117            "y_inches": self._y_in,
118            "z_inches": self._z_in,
119            "roll_deg": self._roll_deg,
120            "pitch_deg": self._pitch_up_deg,
121            "yaw_deg": self._yaw_deg,
122            "fov_deg": self.fov_deg,
123            "x_meters": round(t.X(), 4),
124            "y_meters": round(t.Y(), 4),
125            "z_meters": round(t.Z(), 4),
126            "wpi_rotation_deg": {
127                "roll": self._roll_deg,
128                "pitch": -self._pitch_up_deg,
129                "yaw": self._yaw_deg,
130            },
131        }

157class MechanismMount:
158    """A physical mechanism on the robot.
159
160    Each mechanism has:
161      - A local transform (relative to its parent frame)
162      - A resolved robot_to_mechanism transform (absolute from robot center)
163      - Visualization properties (type, size, color)
164
165    Supported types for the visualizer:
166      "swerve_module" - block with wheel direction arrow
167      "turret"        - circle with heading arrow
168      "generic"       - labeled block
169
170    Parent chaining example:
171        turret is at robot_to_turret
172        hood is at turret_to_hood (relative to turret)
173        hood's robot_to_mechanism = robot_to_turret + turret_to_hood
174    """
175
176    __slots__ = ('name', 'type', 'local_transform', 'robot_to_mechanism',
177                 'parent_name', 'width_m', 'length_m', 'radius_m', 'color')
178
179    def __init__(
180        self,
181        name: str,
182        mech_type: str,
183        local_transform: Transform3d,
184        *,
185        parent_transform: Transform3d = None,
186        parent_name: str = None,
187        width_in: float = 3.0,
188        length_in: float = 3.0,
189        radius_in: float = 0,
190        color: str = "",
191    ):
192        self.name = name
193        self.type = mech_type
194        self.local_transform = local_transform
195        self.parent_name = parent_name
196
197        # Resolve absolute transform from robot center
198        if parent_transform is not None:
199            self.robot_to_mechanism = chain_transforms(
200                parent_transform, local_transform
201            )
202        else:
203            self.robot_to_mechanism = local_transform
204
205        c = INCHES_TO_METERS
206        self.width_m = width_in * c
207        self.length_m = length_in * c
208        self.radius_m = radius_in * c
209        self.color = color
210
211    def to_dict(self) -> dict:
212        c = INCHES_TO_METERS
213        t = self.robot_to_mechanism.translation()
214        r = self.robot_to_mechanism.rotation()
215        return {
216            "name": self.name,
217            "type": self.type,
218            "parent": self.parent_name,
219            "x_inches": round(t.X() / c, 2),
220            "y_inches": round(t.Y() / c, 2),
221            "z_inches": round(t.Z() / c, 2),
222            "yaw_deg": round(r.z_degrees, 2),
223            "width_inches": round(self.width_m / c, 2),
224            "length_inches": round(self.length_m / c, 2),
225            "radius_inches": round(self.radius_m / c, 2),
226            "color": self.color,
227            "x_meters": round(t.X(), 4),
228            "y_meters": round(t.Y(), 4),
229            "z_meters": round(t.Z(), 4),
230            "width_meters": round(self.width_m, 4),
231            "length_meters": round(self.length_m, 4),
232            "radius_meters": round(self.radius_m, 4),
233        }

78def get_camera(name: str) -> CameraGeometry:
79    """Look up a camera by name. Raises KeyError if not found."""
80    for cam in CAMERAS:
81        if cam.name == name:
82            return cam
83    raise KeyError(f"No camera '{name}'. "
84                   f"Available: {[c.name for c in CAMERAS]}")

32def transform_from_inches(
33    x_in: float = 0,
34    y_in: float = 0,
35    z_in: float = 0,
36    roll_deg: float = 0,
37    pitch_up_deg: float = 0,
38    yaw_deg: float = 0,
39) -> Transform3d:
40    """Build a Transform3d from inches and intuitive degree conventions.
41
42    This is the general-purpose builder for any robot-relative transform:
43    cameras, turrets, hoods, sensors, etc.
44
45    Args:
46        x_in: forward/back in inches (positive = forward)
47        y_in: left/right in inches (positive = left)
48        z_in: height in inches (positive = up)
49        roll_deg: rotation about forward axis
50        pitch_up_deg: positive = tilts UP (negated internally for WPI)
51        yaw_deg: 0 = forward, 90 = left, 180 = backward
52
53    Returns:
54        Transform3d suitable for use as robot_to_X or parent_to_child.
55    """
56    c = INCHES_TO_METERS
57    return Transform3d(
58        Translation3d(x_in * c, y_in * c, z_in * c),
59        Rotation3d.fromDegrees(roll_deg, -pitch_up_deg, yaw_deg),
60    )

32def transform_from_inches(
33    x_in: float = 0,
34    y_in: float = 0,
35    z_in: float = 0,
36    roll_deg: float = 0,
37    pitch_up_deg: float = 0,
38    yaw_deg: float = 0,
39) -> Transform3d:
40    """Build a Transform3d from inches and intuitive degree conventions.
41
42    This is the general-purpose builder for any robot-relative transform:
43    cameras, turrets, hoods, sensors, etc.
44
45    Args:
46        x_in: forward/back in inches (positive = forward)
47        y_in: left/right in inches (positive = left)
48        z_in: height in inches (positive = up)
49        roll_deg: rotation about forward axis
50        pitch_up_deg: positive = tilts UP (negated internally for WPI)
51        yaw_deg: 0 = forward, 90 = left, 180 = backward
52
53    Returns:
54        Transform3d suitable for use as robot_to_X or parent_to_child.
55    """
56    c = INCHES_TO_METERS
57    return Transform3d(
58        Translation3d(x_in * c, y_in * c, z_in * c),
59        Rotation3d.fromDegrees(roll_deg, -pitch_up_deg, yaw_deg),
60    )

67def chain_transforms(*transforms: Transform3d) -> Transform3d:
68    """Chain multiple transforms: parent_to_A + A_to_B + ... = parent_to_final.
69
70    Uses Pose3d as the accumulator since WPILib Transform3d doesn't
71    support direct addition.
72    """
73    result = Pose3d()
74    for tf in transforms:
75        result = result.transformBy(tf)
76    return Transform3d(Pose3d(), result)

134def make_camera(
135    name: str,
136    x_in: float = 0,
137    y_in: float = 0,
138    z_in: float = 0,
139    roll_deg: float = 0,
140    pitch_up_deg: float = 0,
141    yaw_deg: float = 0,
142    fov_deg: float = 70.0,
143) -> CameraGeometry:
144    """Create a CameraGeometry from inches and intuitive degrees."""
145    tf = transform_from_inches(
146        x_in, y_in, z_in, roll_deg, pitch_up_deg, yaw_deg
147    )
148    return CameraGeometry(
149        name, tf, fov_deg,
150        x_in=x_in, y_in=y_in, z_in=z_in,
151        roll_deg=roll_deg, pitch_up_deg=pitch_up_deg, yaw_deg=yaw_deg,
152    )

18class SwerveModuleName(StrEnum):
19    """Canonical string identifiers for each swerve module position.
20
21    Values are the string names used in NetworkTables, SmartDashboard keys,
22    and subsystem registration (e.g. "frontLeft").
23    """
24    FRONT_LEFT = "frontLeft"
25    FRONT_RIGHT = "frontRight"
26    BACK_LEFT = "backLeft"
27    BACK_RIGHT = "backRight"

35class SwerveDriveConsts(RobotConstants):
36    # Module positions derived from constants/robot_geometry.py (single source)
37    moduleFrontLeftX: float = SWERVE_FL.X()
38    moduleFrontLeftY: float = SWERVE_FL.Y()
39    moduleFrontRightX: float = SWERVE_FR.X()
40    moduleFrontRightY: float = SWERVE_FR.Y()
41    moduleBackLeftX: float = SWERVE_BL.X()
42    moduleBackLeftY: float = SWERVE_BL.Y()
43    moduleBackRightX: float = SWERVE_BR.X()
44    moduleBackRightY: float = SWERVE_BR.Y()
45
46    # inverts if the module or gyro does not rotate counterclockwise positive
47    invertGyro: bool = False
48    moduleFrontLeftInvertDrive: bool = True
49    moduleFrontRightInvertDrive: bool = True
50    moduleBackLeftInvertDrive: bool = True
51    moduleBackRightInvertDrive: bool = True
52
53    moduleFrontLeftInvertSteer: bool = True
54    moduleFrontRightInvertSteer: bool = True
55    moduleBackLeftInvertSteer: bool = True
56    moduleBackRightInvertSteer: bool = True
57
58    maxTranslationMPS: float = 4.6
59    maxAngularDPS: float = math.degrees(
60        maxTranslationMPS / math.hypot(moduleFrontLeftY, moduleFrontLeftX)
61    )

64class SwerveModuleMk4iConsts(SwerveDriveConsts):
65    """
66    https://github.com/SwerveDriveSpecialties/swerve-lib/blob/develop/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java
67    """
68
69    kNominalVoltage: float = 12.0
70    # current limits use amps
71    kDriveCurrentLimit: int = 40
72    kSteerCurrentLimit: int = 20
73    # ramp rate: how fast the bot can go from 0% to 100%, measured in seconds
74    kRampRate: float = 0.25
75    kTicksPerRotation: int = 1
76    kCanStatusFrameHz: int = 10
77    quadratureMeasurementRateMs: int = 10
78    quadratureAverageDepth: int = 2
79    numDriveMotors: int = 1
80    motorType: str = "NEO"  # should be an option in wpimath.system.plant.DCMotor

 83class SwerveModuleMk4iL1Consts(SwerveModuleMk4iConsts):
 84    """
 85    https://docs.yagsl.com/configuring-yagsl/standard-conversion-factors
 86    """
 87
 88    wheelDiameter: float = 0.10033  # in meters
 89    driveGearRatio: float = 8.14
 90    steerGearRatio: float = 150 / 7
 91
 92    # position: meters per rotation
 93    # velocity: meters per second
 94    drivePositionConversionFactor: float = (math.pi * wheelDiameter) / (
 95        driveGearRatio * SwerveModuleMk4iConsts.kTicksPerRotation
 96    )
 97    driveVelocityConversionFactor: float = drivePositionConversionFactor / 60.0
 98    steerPositionConversionFactor: float = 360 / (
 99        steerGearRatio * SwerveModuleMk4iConsts.kTicksPerRotation
100    )
101    steerVelocityConversionFactor: float = steerPositionConversionFactor / 60.0
102
103    moduleType: str = "Mk4i_L1"

106class SwerveModuleMk4iL2Consts(SwerveModuleMk4iConsts):
107    """
108    https://docs.yagsl.com/configuring-yagsl/standard-conversion-factors
109    """
110
111    wheelDiameter: float = 0.10033  # in meters
112    # COf: coefficient, force/force (no units)
113    wheelCOF: float = 1.0130211
114    driveGearRatio: float = 6.75
115    steerGearRatio: float = 150 / 7
116
117    # position: meters per rotation
118    # velocity: meters per second
119    drivePositionConversionFactor: float = (wheelCOF * (math.pi * wheelDiameter)) / (
120        driveGearRatio * SwerveModuleMk4iConsts.kTicksPerRotation
121    )
122    driveVelocityConversionFactor: float = drivePositionConversionFactor / 60.0
123    steerPositionConversionFactor: float = 360 / (
124        steerGearRatio * SwerveModuleMk4iConsts.kTicksPerRotation
125    )
126    steerVelocityConversionFactor: float = steerPositionConversionFactor / 60.0
127
128    moduleType: str = "Mk4i_L2"

145class IntakePivotConstants:
146    name = "intake_pivot"
147    canId = 20
148    inverted = False
149    motorClass = rev.SparkFlex
150    currentLimitAmps = 30
151    # Degrees per motor rotation — measure or calculate from gear ratio
152    positionConversionFactor = 1.0  # TODO: calibrate on hardware
153    # Mechanical safety boundaries (degrees)
154    minSoftLimit = 0.0
155    maxSoftLimit = 155.0
156    # Target positions for commands (degrees)
157    stowedPosition = 0.0
158    deployedPosition = 155.0

161class IntakeRollerConstants:
162    name = "intake_roller"
163    canId = 21
164    inverted = False
165    motorClass = rev.SparkFlex
166    defaultPower = 0.6

169class HopperConstants:
170    name = "hopper"
171    canId = 40
172    inverted = True
173    defaultPower = 0.5

176class ShooterConstants:
177    name = "shooter"
178    flywheelLeadMotorId = 32
179    flywheelFollowerMotorId = 33
180    flywheelLeadInverted = False
181    flywheelFollowerInverted = True
182    currentLimitAmps = 40
183    offsetDelta = 100
184    fixedRPM = 6500
185    # TODO: Get rest and max positions for shooter
186    hoodRestPosition = 0
187    hoodMaxPosition = 1
188    hoodPosition1 = 0.25
189    hoodPosition2 = 0.5
190    hoodPosition3 = 0.75
191    positionConversionFactor = 1 / 1.5

194class FeedConstants:
195    name = "feed"
196    upperMotorId = 35
197    lowerMotorId = 36
198    upperInverted = True
199    lowerInverted = True
200    currentLimitAmps = 30