diff --git a/robot_constants.py b/robot_constants.py index 1b8c39a..5045ed4 100644 --- a/robot_constants.py +++ b/robot_constants.py @@ -1 +1,19 @@ -LOGGING = True \ No newline at end of file + +from wpimath.geometry import Rotation2d +LOGGING = True + +tower_drivetrain_angle = Rotation2d(0) # PLACEHOLDER +tower_flywheel_velocity = 0 # PLACEHOLDER +tower_hood_angle = 0 # PLACEHOLDER + +hub_drivetrain_angle = Rotation2d(0) # PLACEHOLDER +hub_flywheel_velocity = 0 # PLACEHOLDER +hub_hood_angle = 0 # PLACEHOLDER + +leftpass_drivetrain_angle = Rotation2d(-160) # PLACEHOLDER +leftpass_flywheel_velocity = 0 # PLACEHOLDER +leftpass_hood_angle = 0 # PLACEHOLDER + +rightpass_drivetrain_angle = Rotation2d(160) # PLACEHOLDER +rightpass_flywheel_velocity = 0 # PLACEHOLDER +rightpass_hood_angle = 0 # PLACEHOLDER \ No newline at end of file diff --git a/robotcontainer.py b/robotcontainer.py index de9b4fa..4a39791 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -3,13 +3,14 @@ # Open Source Software; you can modify and/or share it under the terms of # the WPILib BSD license file in the root directory of this project. # -from commands2 import ParallelCommandGroup +from commands2 import ParallelCommandGroup, ConditionalCommand, SelectCommand from commands2.button import CommandXboxController, Trigger from commands2.sysid import SysIdRoutine from generated.tuner_constants import TunerConstants from telemetry import Telemetry from subsystems import * +from robot_constants import * from phoenix6 import swerve from wpilib import DriverStation, SendableChooser, SmartDashboard @@ -38,7 +39,6 @@ def __init__(self) -> None: ) # Use open-loop control for drive motors ) self._brake = swerve.requests.SwerveDriveBrake() - self._point = swerve.requests.PointWheelsAt() self._logger = Telemetry(max_speed) @@ -99,20 +99,62 @@ def configureButtonBindings(self) -> None: self.drivetrain.runOnce(self.drivetrain.seed_field_centric) ) - # Aim drivetrain and shooter - self.driver_controller.rightTrigger().whileTrue( + # stationary commands + + self.tower = ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, tower_drivetrain_angle), + SetShooter(self.shooter, tower_flywheel_velocity, tower_hood_angle) + ) + + self.hub = ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, hub_drivetrain_angle), + SetShooter(self.shooter, hub_flywheel_velocity, hub_hood_angle) + ) + + self.pass_right = ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, rightpass_drivetrain_angle), + SetShooter(self.shooter, rightpass_flywheel_velocity, rightpass_hood_angle) + ) + + self.pass_left = ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, hub_drivetrain_angle), + SetShooter(self.shooter, tower_flywheel_velocity, hub_hood_angle) + ) + + # aim drivetrain and shooter based on operator input + # self.driver_controller.rightTrigger().onTrue( + # SelectCommand( + # { + # 0: self.hub, + # 90: self.pass_right, + # 180: self.tower, + # 270: self.pass_left, + # -1: ParallelCommandGroup( + # AimShooter(self.shooter, self.drivetrain), + # AimDrivetrain(self.drivetrain, self.driver_controller) + # ) + # }, + # self.operator_controller.getHID().getPOV + # ) + # ) + + # command used to tune the shooter by taking in a value from networktables + self.driver_controller.rightTrigger().onTrue( ParallelCommandGroup( - AimDrivetrain(self.drivetrain, self.driver_controller), - AimShooter(self.shooter, self.drivetrain) + TuneShooter(self.shooter, self.drivetrain), + AimDrivetrain(self.drivetrain, self.driver_controller) ) ) + Trigger(lambda: self.drivetrain.ready_to_shoot and self.shooter.ready_to_shoot()).whileTrue( + RunIndexer(self.indexer) + ) + + # drive in "snake mode" (intake faces direction of travel) self.driver_controller.rightBumper().whileTrue( SnakeMode(self.drivetrain, self.driver_controller) ) - - # force the indexer to spin self.operator_controller.a().or_(self.driver_controller.a()).whileTrue( RunIndexer(self.indexer) diff --git a/subsystems/drivetrain/__init__.py b/subsystems/drivetrain/__init__.py index beaffe9..197c5f5 100644 --- a/subsystems/drivetrain/__init__.py +++ b/subsystems/drivetrain/__init__.py @@ -1,3 +1,3 @@ from .command_swerve_drivetrain import CommandSwerveDrivetrain -from .command import AimDrivetrain, SnakeMode +from .command import AimDrivetrain, DriveAtAngle, SnakeMode from .constants import max_angular_rate, max_speed, deadband, curve \ No newline at end of file diff --git a/subsystems/drivetrain/command.py b/subsystems/drivetrain/command.py index eea6369..e8973c3 100644 --- a/subsystems/drivetrain/command.py +++ b/subsystems/drivetrain/command.py @@ -17,9 +17,9 @@ def __init__(self, subsystem: CommandSwerveDrivetrain, controller: commands2.but self.controller = controller self._brake = swerve.requests.SwerveDriveBrake() self._aim_at = swerve.requests.FieldCentricFacingAngle().with_heading_pid( - aiming_pid_p, - aiming_pid_i, - aiming_pid_d + aiming_kP, + aiming_kI, + aiming_kD ) self.addRequirements(self.drivetrain) @@ -52,16 +52,71 @@ def execute(self): self.v_y = math_utils.curve(-self.controller.getLeftX(), deadband, curve) * max_speed self.cmd_speed = math.hypot(self.v_x, self.v_y) + self.is_facing_angle = self.drivetrain.is_facing_angle(self.target_angle.radians()) - if self.drivetrain.is_facing_angle(self.target_angle.radians()) and self.cmd_speed == 0: + if self.is_facing_angle and self.cmd_speed == 0: self.drivetrain.set_control(self._brake) - elif self.cmd_speed > 0 or not self.drivetrain.is_facing_angle(self.target_angle.radians()): + else: self.drivetrain.set_control( self._aim_at.with_target_direction(self.target_angle) .with_velocity_x(self.v_x) .with_velocity_y(self.v_y) ) + + self.drivetrain.ready_to_shoot = self.cmd_speed < drivetrain_shooting_velocity_tolerance and self.is_facing_angle + + def isFinished(self) -> bool: + return False + + def end(self, interrupted: bool) -> None: + pass + +class DriveAtAngle(commands2.Command): + def __init__(self, subsystem: CommandSwerveDrivetrain, controller: commands2.button.CommandXboxController, target_angle: Rotation2d): + super().__init__() + + self.drivetrain = subsystem + self.controller = controller + self._brake = swerve.requests.SwerveDriveBrake() + self._aim_at = swerve.requests.FieldCentricFacingAngle().with_heading_pid( + aiming_kP, + aiming_kI, + aiming_kD + ) + + self.addRequirements(self.drivetrain) + self.target_angle = target_angle + + def initialize(self): + self.target_angle = alliance_flip_util.get_alliance(self.target_angle) + + def execute(self): + """ + 1. Calculate v_x and v_y based on controller inputs + 2. If speed is too high, drive at angle + 3. Check if drivetrain is facing angle within tolerance + 4. If facing angle and speed is 0, apply brake + 5. Else: drive at angle + """ + + self.v_x = math_utils.curve(-self.controller.getLeftY(), deadband) * max_speed + self.v_y = math_utils.curve(-self.controller.getLeftX(), deadband, curve) * max_speed + + self.cmd_speed = math.hypot(self.v_x, self.v_y) + self.is_facing_angle = self.drivetrain.is_facing_angle(self.target_angle.radians()) + + if self.is_facing_angle and self.cmd_speed == 0: + self.drivetrain.set_control(self._brake) + + else: + self.drivetrain.set_control( + self._aim_at.with_target_direction(self.target_angle) + .with_velocity_x(self.v_x) + .with_velocity_y(self.v_y) + ) + + self.drivetrain.ready_to_shoot = self.cmd_speed < drivetrain_shooting_velocity_tolerance and self.is_facing_angle def isFinished(self) -> bool: return False @@ -76,9 +131,9 @@ def __init__(self, subsystem: CommandSwerveDrivetrain, controller: commands2.but self.drivetrain = subsystem self.controller = controller self.aim_at = swerve.requests.FieldCentricFacingAngle().with_heading_pid( - snake_mode_pid_p, - snake_mode_pid_i, - snake_mode_pid_d + snake_mode_kP, + snake_mode_kI, + snake_mode_kD ) self.drive = swerve.requests.FieldCentric().with_drive_request_type( swerve.SwerveModule.DriveRequestType.OPEN_LOOP_VOLTAGE diff --git a/subsystems/drivetrain/command_swerve_drivetrain.py b/subsystems/drivetrain/command_swerve_drivetrain.py index 7780c45..a018dcb 100644 --- a/subsystems/drivetrain/command_swerve_drivetrain.py +++ b/subsystems/drivetrain/command_swerve_drivetrain.py @@ -249,6 +249,8 @@ def __init__( self ) + self.ready_to_shoot = False + def get_pose(self) -> Pose2d: return self.get_state().pose diff --git a/subsystems/drivetrain/constants.py b/subsystems/drivetrain/constants.py index ca5dd9c..70649dc 100644 --- a/subsystems/drivetrain/constants.py +++ b/subsystems/drivetrain/constants.py @@ -14,13 +14,13 @@ curve = 2 # curve exponent for controller inputs #heading pid tolerance ALL PLACEHOLDERS -aiming_pid_p = 10 -aiming_pid_i = 0 -aiming_pid_d = 0 +aiming_kP = 10 +aiming_kI = 0 +aiming_kD = 0 -snake_mode_pid_p = 10 -snake_mode_pid_i = 0 -snake_mode_pid_d = 0 +snake_mode_kP = 10 +snake_mode_kI = 0 +snake_mode_kD = 0 - -drive_at_angle_tolerance: units.radian = 0.05 #placeholder value, ~2.86 degrees \ No newline at end of file +drive_at_angle_tolerance: units.radian = 0.05 #placeholder value, ~2.86 degrees +drivetrain_shooting_velocity_tolerance = 0.1 # PLACEHOLDER, percentage (0 to 1) \ No newline at end of file diff --git a/subsystems/shooter/__init__.py b/subsystems/shooter/__init__.py index 57a6798..97b6484 100644 --- a/subsystems/shooter/__init__.py +++ b/subsystems/shooter/__init__.py @@ -1,2 +1,2 @@ from .subsystem import Shooter -from .command import AimShooter, SetShooterAuto, SetShooterIdle \ No newline at end of file +from .command import AimShooter, SetShooterAuto, SetShooterIdle, TuneShooter, SetShooter diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index 4f80f09..ddbb886 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -2,8 +2,11 @@ from .subsystem import Shooter from wpimath.geometry import Pose2d from subsystems.drivetrain.command_swerve_drivetrain import CommandSwerveDrivetrain -from utils import alliance_flip_util, field_constants +from utils import alliance_flip_util, field_constants, shooter_utils from .constants import * +from phoenix6 import units + +from ntcore import NetworkTableInstance class AimShooter(commands2.Command): """ @@ -39,6 +42,7 @@ def isFinished(self): def end(self, interrupted): pass + class SetShooterAuto(commands2.Command): """ uses target_stationary function to set left and right flywheels to specified velocity and set hood to specified angle. @@ -57,7 +61,7 @@ def __init__(self, subsystem: Shooter, pose: Pose2d): self.pose = pose def initialize(self): - self.subsystem.target_stationary(Pose2d, False) + self.subsystem.target_stationary(self.pose, False) def execute(self): pass @@ -89,5 +93,70 @@ def execute(self): def isFinished(self): return False + def end(self, interrupted): + pass + +class SetShooter(commands2.Command): + """ + sets left and right flywheels to specified velocity and set hood to specified angle + never ends + + Args: + velocity (rotations per second): desired left and right flywheel velocity + angle (rotations): desired hood angle + """ + + def __init__(self, subsystem: Shooter, velocity: units.rotations_per_second, angle: units.radian): + super().__init__() + + self.subsystem = subsystem + self.addRequirements(self.subsystem) + self.velocity = velocity + self.angle = angle + + def initialize(self): + self.subsystem.set_left_target_velocity(self.velocity) + self.subsystem.set_right_target_velocity(self.velocity) + self.subsystem.set_hood_angle(self.angle) + + def execute(self): + pass + + def isFinished(self): + return False + + def end(self, interrupted): + pass + + +class TuneShooter(commands2.Command): + def __init__(self, subsystem: Shooter, drivetrain: CommandSwerveDrivetrain): + super().__init__() + + self.subsystem = subsystem + self.addRequirements(self.subsystem) + self.drivetrain = drivetrain + + self.nt_inst = NetworkTableInstance.getDefault() + self.shot_tuner = self.nt_inst.getTable("Shot Tuner") + + + self.hood_angle_pub = self.shot_tuner.getDoubleTopic("hood angle").publish() + self.flywheel_rps_pub = self.shot_tuner.getDoubleTopic("flywheel rps").publish() + self.hood_angle_sub = self.shot_tuner.getDoubleTopic("hood angle").subscribe(20.0) + self.flywheel_rps_sub = self.shot_tuner.getDoubleTopic("flywheel rps").subscribe(15.0) + self.distance_pub = self.shot_tuner.getDoubleTopic("distance to hub").publish() + + def initialize(self): + self.subsystem.set_left_target_velocity(self.flywheel_rps_sub.get()) + self.subsystem.set_right_target_velocity(self.flywheel_rps_sub.get()) + self.subsystem.set_hood_angle(self.hood_angle_sub.get() / 360) + + def execute(self): + self.distance_pub.set(shooter_utils.shot_distance_from_pose(self.drivetrain.get_pose())) + + def isFinished(self): + return False + def end(self, interrupted): pass \ No newline at end of file diff --git a/subsystems/shooter/constants.py b/subsystems/shooter/constants.py index 85d8bbd..f02dc75 100644 --- a/subsystems/shooter/constants.py +++ b/subsystems/shooter/constants.py @@ -1,4 +1,4 @@ -from phoenix6 import hardware, configs, signals +from phoenix6 import hardware, configs, signals, units from wpimath.geometry import Translation2d import wpilib import numpy as np @@ -8,19 +8,29 @@ right_lead_id: int = 60 # placeholder right_follow_id: int = 61 # placeholder hood_id: int = 62 # placeholder +hood_cancoder_id: int = 57 #placeholder -flywheel_threshold = 2.0 # placeholder -hood_threshold = 2.0 # placeholder +flywheel_velocity_threshold: units.rotations_per_second = 4.0 # placeholder +hood_angle_threshold: units.rotation = 1.0 / 360# placeholder -hood_gear_ratio = 0 # placeholder +hood_gear_ratio = 69 # 69:1 +max_hood_angle: units.rotation = 43 / 360 # placeholder +min_hood_angle: units.rotation = 10 / 360 -idle_velocity = 0 # placeholder +idle_velocity: units.rotations_per_second = 0 # placeholder NT_SHOOTER: bool = True left_direction = signals.InvertedValue.COUNTER_CLOCKWISE_POSITIVE right_direction = signals.InvertedValue.CLOCKWISE_POSITIVE +hood_cancoder_config = configs.CANcoderConfiguration().with_magnet_sensor( + configs.MagnetSensorConfigs() + .with_absolute_sensor_discontinuity_point(0) # placeholder + .with_magnet_offset(0) # placeholder + .with_sensor_direction(signals.SensorDirectionValue.COUNTER_CLOCKWISE_POSITIVE) +) + flywheel_config = configs.TalonFXConfiguration().with_motor_output( configs.MotorOutputConfigs() .with_neutral_mode(signals.NeutralModeValue.BRAKE) @@ -32,6 +42,9 @@ .with_k_s(0) # placeholder .with_k_v(0) # placeholder .with_k_a(0) # placeholder +).with_current_limits( + configs.CurrentLimitsConfigs() + .with_stator_current_limit(80) # placeholder ) hood_config = configs.TalonFXConfiguration().with_motor_output( @@ -55,10 +68,16 @@ .with_k_g(0) # placeholder ).with_feedback( configs.FeedbackConfigs() - .with_sensor_to_mechanism_ratio(0) # placeholder - .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.ROTOR_SENSOR) + .with_rotor_to_sensor_ratio(23*1.5) + .with_sensor_to_mechanism_ratio(2) + .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.FUSED_CANCODER) + .with_feedback_remote_sensor_id(hood_cancoder_id) +).with_current_limits( + configs.CurrentLimitsConfigs() + .with_stator_current_limit(60) # placeholder ) + def load_shooter_table_csv(rel_path: str) -> np.ndarray: """ Loads a CSV from the robot deploy directory into a Nx3 float array: @@ -78,10 +97,6 @@ def load_shooter_table_csv(rel_path: str) -> np.ndarray: return table -shooter_offset = Translation2d(1, 1) # placeholder -max_hood_angle = 43 # placeholder -min_hood_angle = 0 # placeholder - # robot distance to hub, hood angle, and RPS SHOT_TABLE = load_shooter_table_csv("shot_table.csv") diff --git a/subsystems/shooter/subsystem.py b/subsystems/shooter/subsystem.py index 01477d2..15b316d 100644 --- a/subsystems/shooter/subsystem.py +++ b/subsystems/shooter/subsystem.py @@ -18,12 +18,16 @@ def __init__(self): self.velocity_torque_current = controls.VelocityTorqueCurrentFOC(0) self.hood_motor = hardware.TalonFX(hood_id) + self.hood_cancoder = hardware.CANcoder(hood_cancoder_id) + self.motion_magic = controls.MotionMagicVoltage(0) self.left_target_velocity = 0 self.right_target_velocity = 0 self.hood_target_angle = 0 - + + self.hood_cancoder.configurator.apply(hood_cancoder_config) + self.left_leader_motor.configurator.apply(flywheel_config.with_motor_output( configs.MotorOutputConfigs() .with_inverted(left_direction) @@ -35,7 +39,7 @@ def __init__(self): .with_inverted(right_direction) )) self.right_follower_motor.set_control(controls.Follower(right_lead_id, signals.MotorAlignmentValue.ALIGNED)) - + self.hood_motor.configurator.apply(hood_config) self.table = ntcore.NetworkTableInstance.getDefault().getTable("shooter") @@ -47,7 +51,7 @@ def __init__(self): self.hood_target_angle_pub = self.table.getDoubleTopic("hood target angle").publish() self.shooter_ready_pub = self.table.getDoubleTopic("shooter ready").publish() - def set_left_target_velocity(self, velocity: float): + def set_left_target_velocity(self, velocity: units.rotations_per_second): """ sets the target velocity of the left flywheel @@ -58,7 +62,7 @@ def set_left_target_velocity(self, velocity: float): self.left_leader_motor.set_control(self.velocity_torque_current.with_velocity(self.left_target_velocity)) - def set_right_target_velocity(self, velocity: float): + def set_right_target_velocity(self, velocity: units.rotations_per_second): """ sets the target velocity of the right flywheel @@ -69,7 +73,7 @@ def set_right_target_velocity(self, velocity: float): self.right_leader_motor.set_control(self.velocity_torque_current.with_velocity(self.right_target_velocity)) - def get_left_velocity(self): + def get_left_velocity(self) -> units.rotations_per_second: """ obtains the current velocity of the left flywheel @@ -78,7 +82,7 @@ def get_left_velocity(self): """ return self.left_leader_motor.get_velocity().value_as_double - def get_right_velocity(self): + def get_right_velocity(self) -> units.rotations_per_second: """ obtains the current velocity of the right flywheel @@ -87,7 +91,7 @@ def get_right_velocity(self): """ return self.right_leader_motor.get_velocity().value_as_double - def left_is_at_velocity(self, velocity: float): + def left_is_at_velocity(self, velocity: units.rotations_per_second): """ checks if the left flywheel is at a certain velocity @@ -97,9 +101,9 @@ def left_is_at_velocity(self, velocity: float): Returns: boolean: whether or not the left flywheel is at the velocity (true it is and false it isn't) """ - return abs(self.get_left_velocity() - velocity) < flywheel_threshold + return abs(self.get_left_velocity() - velocity) < flywheel_velocity_threshold - def right_is_at_velocity(self, velocity: float): + def right_is_at_velocity(self, velocity: units.rotations_per_second): """ checks if the right flywheel is at a certain velocity @@ -109,42 +113,39 @@ def right_is_at_velocity(self, velocity: float): Returns: boolean: whether or not the right flywheel is at the velocity (true it is and false it isn't) """ - return abs(self.get_right_velocity() - velocity) < flywheel_threshold + return abs(self.get_right_velocity() - velocity) < flywheel_velocity_threshold - def set_hood_angle(self, angle: float): + def set_hood_angle(self, angle: units.rotation): """ brings the hood to given angle Args: - angle (radians): intended hood angle in radians + angle (rotations): intended hood angle in rotations """ self.hood_target_angle = max(min_hood_angle, min(angle, max_hood_angle)) - - rotations = self.hood_target_angle / (2 * math.pi) - - self.hood_motor.set_control(self.motion_magic.with_position(rotations)) + self.hood_motor.set_control(self.motion_magic.with_position(self.hood_target_angle)) def get_hood_angle(self): """ obtains the current position of the hood Returns: - return_float: current hood position in radians + return_float: current hood position in rotations """ - return (self.hood_motor.get_position()) * (2 * math.pi) + return self.hood_motor.get_position() - def hood_is_at_angle(self, angle: float): + def hood_is_at_angle(self, angle: units.rotation): """ checks if the right hood is at a certain angle Args: - angle (radians): angle to be checked + angle (rotations): angle to be checked Returns: boolean: whether or not the hood is at the angle (true it is and false it isn't) """ - return abs(self.get_hood_angle() - angle) < hood_threshold + return abs(self.get_hood_angle().value - angle) < hood_angle_threshold def ready_to_shoot(self): """ @@ -153,10 +154,7 @@ def ready_to_shoot(self): Returns: boolean: whether or not the system is ready to shoot (true it is and false it isn't) """ - if self.left_is_at_velocity(self.left_target_velocity) == True and self.right_is_at_velocity(self.right_target_velocity) and self.hood_is_at_angle(self.hood_target_angle) == True: - return True - else: - return False + return self.left_is_at_velocity(self.left_target_velocity) == True and self.right_is_at_velocity(self.right_target_velocity) and self.hood_is_at_angle(self.hood_target_angle) == True def target_stationary(self, robot_pose: Pose2d, passing: bool): """ @@ -165,11 +163,11 @@ def target_stationary(self, robot_pose: Pose2d, passing: bool): :param robot_pose: robot's pose on the field """ if passing: - hood_deg, rps = shooter_utils.pass_setpoints_from_pose(robot_pose) + hood_pos, rps = shooter_utils.pass_setpoints_from_pose(robot_pose) else: - hood_deg, rps = shooter_utils.shot_setpoints_from_pose(robot_pose) + hood_pos, rps = shooter_utils.shot_setpoints_from_pose(robot_pose) - self.set_hood_angle(math.radians(hood_deg)) + self.set_hood_angle(hood_pos) self.set_left_target_velocity(rps) self.set_right_target_velocity(rps) @@ -180,10 +178,10 @@ def update_table(self): self.right_velocity_pub.set(self.get_right_velocity()) self.left_target_velocity_pub.set(self.left_target_velocity) self.right_target_velocity_pub.set(self.right_target_velocity) - self.hood_angle_pub.set(self.get_hood_angle()) - self.hood_target_angle_pub.set(self.hood_target_angle) + self.hood_angle_pub.set(self.get_hood_angle().value * 360) + self.hood_target_angle_pub.set(self.hood_target_angle * 360) self.shooter_ready_pub.set(self.ready_to_shoot()) def periodic(self): if NT_SHOOTER: - self.update_table \ No newline at end of file + self.update_table() \ No newline at end of file diff --git a/utils/shooter_utils.py b/utils/shooter_utils.py index 10830bd..a937e06 100644 --- a/utils/shooter_utils.py +++ b/utils/shooter_utils.py @@ -2,7 +2,7 @@ from utils import alliance_flip_util, field_constants import math -from subsystems.shooter.constants import DIST_M, HOOD_DEG, RPS, shooter_offset, PASS_DIST_M, PASS_HOOD_DEG, PASS_RPS +from subsystems.shooter.constants import DIST_M, HOOD_DEG, RPS, PASS_DIST_M, PASS_HOOD_DEG, PASS_RPS from utils.field_constants import Hub import numpy as np @@ -49,19 +49,18 @@ def shot_setpoints_from_pose(robot_pose: Pose2d) -> tuple[float, float]: - measures distance to hub center - interpolates hood and rps from DIST_M tables """ - hub2d: Translation2d = alliance_flip_util.get_alliance(Translation2d(Hub.INNER_CENTER_POINT.x, Hub.INNER_CENTER_POINT.y)) - - shooter_origin_field: Translation2d = ( - robot_pose.translation() + shooter_offset.rotateBy(robot_pose.rotation()) - ) - - distance_m: float = shooter_origin_field.distance(hub2d) + distance_m: float = shot_distance_from_pose(robot_pose) hood_deg: float = float(np.interp(distance_m, DIST_M, HOOD_DEG)) rps: float = float(np.interp(distance_m, DIST_M, RPS)) return hood_deg, rps +def shot_distance_from_pose(robot_pose: Pose2d) -> float: + hub2d: Translation2d = alliance_flip_util.get_alliance(Translation2d(Hub.INNER_CENTER_POINT.x, Hub.INNER_CENTER_POINT.y)) + + return robot_pose.translation().distance(hub2d) + #passing def pass_setpoints_from_pose(robot_pose: Pose2d) -> tuple[float, float]: """ @@ -72,11 +71,7 @@ def pass_setpoints_from_pose(robot_pose: Pose2d) -> tuple[float, float]: """ pass_setpoint: Translation2d = alliance_flip_util.get_alliance(get_pass_setpoint(robot_pose)) - shooter_origin_field: Translation2d = ( - robot_pose.translation() + shooter_offset.rotateBy(robot_pose.rotation()) - ) - - distance_m: float = shooter_origin_field.distance(pass_setpoint) + distance_m: float = robot_pose.translation().distance(pass_setpoint) hood_deg: float = float(np.interp(distance_m, PASS_DIST_M, PASS_HOOD_DEG)) rps: float = float(np.interp(distance_m, PASS_DIST_M, PASS_RPS))