diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index 746e6abf..e8948151 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -33,7 +33,11 @@ def __init__(self) -> None: Path([NotePositions.amp], face_target=True), Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True), ] - super().__init__(note_paths, shoot_paths) + start_pose = Pose2d( + TeamPoses.RED_TEST_POSE.translation(), + rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), + ) + super().__init__(note_paths, shoot_paths, start_pose) class PodiumSpeakerAmp(AutoBase): @@ -72,11 +76,9 @@ def __init__(self) -> None: Path([NotePositions.amp], face_target=True), Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True), ] - # Start pose only needs to be on the correct half of the field, - # so choose the subwoofer start_pose = Pose2d( - TeamPoses.RED_TEST_POSE.translation(), - rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), + TeamPoses.RED_AMP_START_POSE.translation(), + rotation_to_red_speaker(TeamPoses.RED_AMP_START_POSE.translation()), ) super().__init__(note_paths, shoot_paths, start_pose) @@ -99,8 +101,8 @@ def __init__(self) -> None: # Start pose only needs to be on the correct half of the field, # so choose the subwoofer start_pose = Pose2d( - TeamPoses.RED_TEST_POSE.translation(), - rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), + TeamPoses.RED_AMP_START_POSE.translation(), + rotation_to_red_speaker(TeamPoses.RED_AMP_START_POSE.translation()), ) super().__init__(note_paths, shoot_paths, start_pose) @@ -124,8 +126,6 @@ def __init__(self) -> None: face_target=True, ), ] - # Start pose only needs to be on the correct half of the field, - # so choose the subwoofer start_pose = Pose2d( TeamPoses.RED_TEST_POSE.translation(), rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), @@ -160,8 +160,6 @@ def __init__(self) -> None: face_target=True, ), ] - # Start pose only needs to be on the correct half of the field, - # so choose the subwoofer start_pose = Pose2d( TeamPoses.RED_TEST_POSE.translation(), rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), @@ -175,10 +173,18 @@ class Centre5Centre4(AutoBase): def __init__(self) -> None: note_paths = [ - Path([NotePositions.Centre5], face_target=False), Path( [ + PathPositions.avoid_starting_faults, PathPositions.avoid_stage_S, + NotePositions.Centre5, + ], + face_target=False, + ), + Path( + [ + PathPositions.avoid_stage_S, + PathPositions.enforce_pickup_angle, NotePositions.Centre4, ], face_target=False, @@ -186,7 +192,10 @@ def __init__(self) -> None: ] shoot_paths = [ - Path([ShootingPositions.source_side], face_target=True), + Path( + [PathPositions.avoid_stage_S, ShootingPositions.source_side], + face_target=True, + ), Path( [ PathPositions.avoid_stage_S, @@ -195,7 +204,7 @@ def __init__(self) -> None: face_target=True, ), ] - sim_start_pos = Translation2d(15.4, 2.94) + sim_start_pos = ShootingPositions.source_side rotation = rotation_to_red_speaker(sim_start_pos) sim_start_pose = Pose2d(sim_start_pos, rotation) super().__init__(note_paths, shoot_paths, sim_start_pose) @@ -207,10 +216,18 @@ class Centre5Centre4Centre3(AutoBase): def __init__(self) -> None: note_paths = [ - Path([NotePositions.Centre5], face_target=False), + Path( + [ + PathPositions.avoid_starting_faults, + PathPositions.avoid_stage_S, + NotePositions.Centre5, + ], + face_target=False, + ), Path( [ PathPositions.avoid_stage_S, + PathPositions.enforce_pickup_angle, NotePositions.Centre4, ], face_target=False, @@ -226,7 +243,10 @@ def __init__(self) -> None: ] shoot_paths = [ - Path([ShootingPositions.source_side], face_target=True), + Path( + [PathPositions.avoid_stage_S, ShootingPositions.source_side], + face_target=True, + ), Path( [ PathPositions.avoid_stage_S, @@ -243,7 +263,7 @@ def __init__(self) -> None: face_target=True, ), ] - sim_start_pos = Translation2d(15.4, 2.94) + sim_start_pos = ShootingPositions.source_side rotation = rotation_to_red_speaker(sim_start_pos) sim_start_pose = Pose2d(sim_start_pos, rotation) super().__init__(note_paths, shoot_paths, sim_start_pose) diff --git a/autonomous/base.py b/autonomous/base.py index 3041b398..74b3fa2b 100644 --- a/autonomous/base.py +++ b/autonomous/base.py @@ -152,13 +152,11 @@ def drive_and_shoot(self, state_tm: float, initial_call: bool) -> None: if self.is_close_enough_to_shoot(): self.note_manager.try_shoot() - if self.note_manager.has_just_fired() or ( - self.is_at_goal() and not self.note_manager.has_note() - ): - if len(self.shoot_paths_working_copy) != 0: - self.next_state("pick_up") - else: - self.done() + if ( + self.note_manager.has_just_fired() + or (self.is_at_goal() and not self.note_manager.has_note()) + ) and len(self.shoot_paths_working_copy) != 0: + self.next_state("pick_up") def drive_on_trajectory(self, trajectory_tm: float): if not self.trajectory: diff --git a/components/chassis.py b/components/chassis.py index f96c8cf1..27dd0bff 100644 --- a/components/chassis.py +++ b/components/chassis.py @@ -294,6 +294,10 @@ def __init__(self) -> None: def get_velocity(self) -> ChassisSpeeds: return self.kinematics.toChassisSpeeds(self.get_module_states()) + @feedback + def imu_rotation(self) -> float: + return self.imu.getAngle() + def get_module_states( self, ) -> tuple[ @@ -315,7 +319,7 @@ def setup(self) -> None: self.get_module_positions(), initial_pose, stateStdDevs=(0.05, 0.05, 0.01), - visionMeasurementStdDevs=(0.4, 0.4, 50), + visionMeasurementStdDevs=(0.4, 0.4, 0.03), ) self.field_obj = self.field.getObject("fused_pose") self.set_pose(initial_pose) diff --git a/components/led.py b/components/led.py index ddb9898a..5a58e12d 100644 --- a/components/led.py +++ b/components/led.py @@ -14,7 +14,7 @@ Hsv = tuple[int, int, int] FLASH_SPEED = 2 -BREATHE_SPEED = 4 +BREATHE_SPEED = 0.5 RAINBOW_SPEED = 8 MORSE_SPEED = 0.2 diff --git a/components/shooter.py b/components/shooter.py index 916583ae..9579c802 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -47,17 +47,17 @@ class ShooterComponent: 70, 70, 76, - 76, - 77, + 78, + 78, 90, 0, ) FLYWHEEL_ANGLE_LOOKUP = ( - 0.96, - 0.96, + 0.95, + 0.95, 0.80, - 0.61, - 0.50, + 0.615, + 0.495, 0.45, MIN_INCLINE_ANGLE, ) diff --git a/controllers/shooter.py b/controllers/shooter.py index 256a2667..96b59f48 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -22,8 +22,8 @@ class Shooter(StateMachine): data_log: DataLog - SPEED_LIMIT = tunable(1) - SPINNING_SPEED_LIMIT = tunable(1) + SPEED_LIMIT = tunable(0.1) + SPINNING_SPEED_LIMIT = tunable(0.1) def __init__(self): self.range = 0.0 diff --git a/robot.py b/robot.py index 8b6c3112..c78c443f 100644 --- a/robot.py +++ b/robot.py @@ -22,7 +22,7 @@ from utilities.game import is_red from utilities.scalers import rescale_js from utilities.functions import clamp -from utilities.position import on_same_side_of_stage, y_close_to_stage +from utilities.position import distance_between class MyRobot(magicbot.MagicRobot): @@ -39,7 +39,7 @@ class MyRobot(magicbot.MagicRobot): status_lights: LightStrip - max_speed = magicbot.tunable(4) # m/s + max_speed = magicbot.tunable(5) # m/s lower_max_speed = magicbot.tunable(2) # m/s max_spin_rate = magicbot.tunable(4) # m/s lower_max_spin_rate = magicbot.tunable(2) # m/s @@ -47,6 +47,8 @@ class MyRobot(magicbot.MagicRobot): vision_port: VisualLocalizer vision_starboard: VisualLocalizer + START_POS_TOLERANCE = 1 + def createObjects(self) -> None: self.data_log = wpilib.DataLogManager.getLog() @@ -71,7 +73,7 @@ def createObjects(self) -> None: ) def teleopInit(self) -> None: - pass + self.field.getObject("Intended start pos").setPoses([]) def teleopPeriodic(self) -> None: if self.climber.should_lock_mechanisms(): @@ -84,13 +86,13 @@ def teleopPeriodic(self) -> None: # Set max speed max_speed = self.max_speed max_spin_rate = self.max_spin_rate - if self.gamepad.getXButton(): + if self.gamepad.getRightBumper(): max_speed = self.lower_max_speed max_spin_rate = self.lower_max_spin_rate # Driving - drive_x = -rescale_js(self.gamepad.getLeftY(), 0.1) * max_speed - drive_y = -rescale_js(self.gamepad.getLeftX(), 0.1) * max_speed + drive_x = -rescale_js(self.gamepad.getLeftY(), 0.05, 2.5) * max_speed + drive_y = -rescale_js(self.gamepad.getLeftX(), 0.05, 2.5) * max_speed drive_z = ( -rescale_js(self.gamepad.getRightX(), 0.1, exponential=2) * max_spin_rate ) @@ -204,6 +206,7 @@ def disabledPeriodic(self) -> None: if ( not self.vision_port.sees_target() and not self.vision_starboard.sees_target() + and not self.isSimulation() ): self.status_lights.no_vision() else: @@ -213,11 +216,14 @@ def disabledPeriodic(self) -> None: intended_start_pose = selected_auto.get_starting_pose() current_pose = self.chassis.get_pose() if intended_start_pose is not None: - if on_same_side_of_stage(intended_start_pose, current_pose): - if y_close_to_stage(current_pose): - self.status_lights.too_close_to_stage() - else: - self.status_lights.rainbow() + self.field.getObject("Intended start pos").setPose( + intended_start_pose + ) + if ( + distance_between(intended_start_pose, current_pose) + < self.START_POS_TOLERANCE + ): + self.status_lights.rainbow() else: self.status_lights.invalid_start() else: @@ -226,7 +232,7 @@ def disabledPeriodic(self) -> None: self.status_lights.missing_start_pose() def autonomousInit(self) -> None: - pass + self.field.getObject("Intended start pos").setPoses([]) if __name__ == "__main__": diff --git a/utilities/position.py b/utilities/position.py index 030fcfe1..6ebef9db 100644 --- a/utilities/position.py +++ b/utilities/position.py @@ -34,7 +34,7 @@ def __init__(self, waypoints: list[Translation2d], face_target: bool): self.final_heading = 0 -stage_tolerance = 0.35 +stage_tolerance = 0.4 class NotePositions: @@ -66,7 +66,7 @@ class ShootingPositions: amp_speaker_bounce = Translation2d( 14.7, (NotePositions.amp.y + NotePositions.speaker.y) / 2 ) - source_side = Translation2d(14.7, 2.8) + source_side = Translation2d(15.556, 4.034) class TeamPoses: @@ -74,17 +74,11 @@ class TeamPoses: BLUE_TEST_POSE = field_flip_pose2d(RED_TEST_POSE) BLUE_PODIUM = Pose2d(Translation2d(2.992, 4.08455), Rotation2d(math.pi)) RED_PODIUM = field_flip_pose2d(BLUE_PODIUM) + RED_AMP_START_POSE = Pose2d(15.9, 6.7, math.pi) -def on_same_side_of_stage(intended_start_pose: Pose2d, current_pose: Pose2d) -> bool: - return not ( - (intended_start_pose.y > TeamPoses.BLUE_PODIUM.y) - ^ (current_pose.y > TeamPoses.BLUE_PODIUM.y) - ) - - -def y_close_to_stage(pose: Pose2d) -> bool: - return abs(pose.y - TeamPoses.BLUE_PODIUM.y) < 0.9 +def distance_between(intended_start_pose: Pose2d, current_pose: Pose2d) -> float: + return (intended_start_pose.translation() - current_pose.translation()).norm() class PathPositions: @@ -92,4 +86,6 @@ class PathPositions: stage_transition_S = Translation2d(11.4, 3.74) stage_transition_S_entry = Translation2d(13.0, 2.5) avoid_wall = Translation2d(10.80, 6.55) - avoid_stage_S = Translation2d(10.66, 1.40) + avoid_stage_S = Translation2d(11.66, 1.40) + avoid_starting_faults = Translation2d(14.429, 2.946) + enforce_pickup_angle = Translation2d(9.6, 1.6)