config

 1# Native imports
 2from typing import Tuple
 3
 4# Third-party imports
 5from wpimath.geometry import Rotation2d
 6
 7class OperatorRobotConfig:
 8    # Default start position for red alliance using always-blue-alliance coordinates
 9    # Coordinates are x (meters), y (meters), and rotation (degrees)
10    red_default_start_pose: Tuple[float] = (10.0, 1.5, 0.0)
11    # Default start position for blue alliance using always-blue-alliance coordinates
12    # Coordinates are x (meters), y (meters), and rotation (degrees)
13    blue_default_start_pose: Tuple[float] = (7.7, 6.0, 180.0)
14    # Give in front-left, front-right, back-left, back-right order
15    # ids for the drive controllers
16    swerve_module_channels: Tuple[int] = (50, 53, 56, 59)
17    # Give in front-left, front-right, back-left, back-right order
18    # starting rotational position for the absolute encoders
19    swerve_abs_encoder_calibrations: Tuple[float] = (
20        10.283203125 / 360.0, 323.6148 / 360.0,  347.2737188 / 360.0, 83.2865625 / 360.0
21    )
22    swerve_steer_pid: Tuple[float] = (0.011, 0, 0)
23    swerve_drive_pid: Tuple[float] = (0.0021, 0, 0, 0) # TODO: tune PIDs
24    pathplanner_translation_pid: Tuple[float] = (4.0, 0.0, 0.0)
25    pathplanner_rotation_pid: Tuple[float] = (5.0, 0.0, 0.0)
26
27    robot_Cam_Translation_Left: Tuple[float] = (0.203716, 0.3063875, 0.365125)
28    robot_Cam_Translation_Right: Tuple[float] = (0.203716, -0.3063875, 0.365125)
29    robot_Cam_Rotation_Degress_Left: Tuple[float] = (0.0, 0.0, 0.0)
30    robot_Cam_Rotation_Degress_Right: Tuple[float] = (0.0, 0.0, 0.0)
31
32    vision_default_std_dev: float = 3.0 # pose estimator sigma to use if no distance is available
33    vision_distance_threshold_m: float = 1.0 # meters; how far away a tag can be before its data is discarded
34    vision_ambiguity_threshold: float = 0.1 # the max ambiguity score a tag estimate can have before its data is discarded
35    vision_std_dev_basis: float = 1.1
36    vision_std_dev_scale_factor: float = 1
37    # First three elements are PID, last two elements are trapezoidal profile
38    # Translation trapezoidal profile units are mps and mps^2, rotation are rps and rps^2
39    pid_to_pose_translation_pid_profile: Tuple[float] = (2.0, 0.0, 0.0, 3, 1.5)
40    pid_to_pose_rotation_pid_profile: Tuple[float] = (
41        1.0, 0.0, 0.0, Rotation2d.fromDegrees(360).radians(), Rotation2d.fromDegrees(360).radians()
42    )
43    # Tolerance of x, y, and omega position errors within which robot is at target pose
44    # x error is in meters, y error is in meters, omega error is in radians
45    pid_to_pose_setpoint_tolerances: Tuple[float] = (0.25, 0.25, Rotation2d.fromDegrees(10).radians())
46
47    # Robot motion constraints when running PathPlanner during teleop.
48    # Values to give are: max translation velocity (mps), max translation acceleration (mps^2),
49    # max angular velocity (dps), max angular acceleration (dps^2).
50    teleop_pathplan_constraints: Tuple[float] = (2.5, 2.0, 360.0, 360.0)
51
52class HoodConfig:
53    # PID gains for hood position control (WPILib PIDController)
54    hoodPID = (0.3, 0.02, 0)
55    # ArmFeedforward gains: (kS, kG, kV, kA)
56    hoodFeedforward = (0.4, 0.04, 0, 0)
57    # Offset in degrees from hood 0-position to true horizontal.
58    horizontalOffsetDegrees = 0.0
59
60
61class ShooterConfig:
62    # Configs for shooter
63    shooterFeedMotorPIDF = (0, 0, 0, 1 / 473)
64    shooterFlywheelMotorPIDF = (0, 0, 0, 1 / 560)
65    # Feed, Lead, Follower
66    shooterInverted = (False, False, False)
67    shooterRangeInterval = 0.2
68    shooterOffsetDelta = 100
69    shooterFeedPercentOfFlywheel = 0.9