diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index e3f865ce..b8b2f211 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -3,6 +3,25 @@ from wpimath.geometry import Pose2d, Translation2d +class PodiumSpeakerAmpTopcentre(AutoBase): + MODE_NAME = "5 notes: internal, podium, speaker, amp, top centre" + + def __init__(self) -> None: + note_paths = [ + Path([NotePositions.podium_NW]), + Path([NotePositions.speaker]), + Path([NotePositions.amp]), + Path([PathPositions.avoid_wall, NotePositions.Centre1]), + ] + shoot_paths = [ + Path([ShootingPositions.close_straight]), + Path([ShootingPositions.amp_speaker_bounce]), + Path([NotePositions.amp]), + Path([PathPositions.avoid_wall, NotePositions.amp]), + ] + super().__init__(note_paths, shoot_paths) + + class PodiumSpeakerAmp(AutoBase): MODE_NAME = "4 notes: internal, podium, speaker, amp" diff --git a/autonomous/base.py b/autonomous/base.py index 0f2b059e..07409947 100644 --- a/autonomous/base.py +++ b/autonomous/base.py @@ -1,6 +1,7 @@ import math from typing import Optional from magicbot.state_machine import AutonomousStateMachine, state +from magicbot import feedback from wpimath.trajectory import ( TrajectoryConfig, Trajectory, @@ -19,6 +20,7 @@ from utilities.position import Path import utilities.game as game +from utilities.game import get_goal_speaker_position from components.chassis import ChassisComponent from components.intake import IntakeComponent @@ -34,9 +36,10 @@ class AutoBase(AutonomousStateMachine): intake_component: IntakeComponent POSITION_TOLERANCE = 0.05 + SHOOTING_POSITION_TOLERANCE = 0.5 ANGLE_TOLERANCE = math.radians(5) - MAX_VEL = 3 - MAX_ACCEL = 2 + MAX_VEL = 4 + MAX_ACCEL = 3 ENFORCE_HEADING_SPEED = MAX_VEL / 6 def __init__( @@ -51,8 +54,8 @@ def __init__( self.starting_pose = starting_pose def setup(self) -> None: - x_controller = PIDController(2.5, 0, 0) - y_controller = PIDController(2.5, 0, 0) + x_controller = PIDController(3.5, 0, 0.4) + y_controller = PIDController(3.5, 0, 0.4) heading_controller = self.chassis.heading_controller self.drive_controller = HolonomicDriveController( @@ -68,6 +71,7 @@ def setup(self) -> None: self.goal_heading: Rotation2d self.trajectory_marker = self.field.getObject("auto_trajectory") + self.trajectory: Optional[Trajectory] = None def on_enable(self): # Setup starting position in the simulator @@ -78,6 +82,15 @@ def on_enable(self): self.chassis.set_pose(starting_pose) super().on_enable() + @feedback + def is_close_enough_to_shoot(self) -> bool: + if self.trajectory: + last = self.trajectory.sample(self.trajectory.totalTime()) + return ( + last.pose.translation() - self.chassis.get_pose().translation() + ).norm() < self.SHOOTING_POSITION_TOLERANCE + return False + @state(first=True) def initialise(self) -> None: # Make a working copy of the NotePaths so that we can pop @@ -119,43 +132,43 @@ def pick_up(self, state_tm: float, initial_call: bool) -> None: # Drive with the intake always facing the tangent self.drive_on_trajectory(state_tm, enforce_tangent_heading=True) - if self.note_manager.has_note(): + if self.note_manager.has_note() or self.is_at_goal(): # Check if we have a note collected # Return heading control to path controller self.chassis.stop_snapping() - self.next_state("drive_to_shoot") - if self.is_at_goal(): - if RobotBase.isSimulation(): - self.next_state(self.drive_to_shoot) - return - # we did not find a note on the path, look for the next note - if len(self.note_paths_working_copy) == 0: - # Couldn't find the last note - self.done() - return - self.shoot_paths_working_copy.pop(0) - # reset the clock - self.next_state(self.pick_up) + self.next_state(self.drive_and_shoot) + + def translation_to_goal(self, position: Translation2d) -> Translation2d: + return get_goal_speaker_position().toTranslation2d() - position @state - def drive_to_shoot(self, state_tm: float, initial_call: bool) -> None: + def drive_and_shoot(self, state_tm: float, initial_call: bool) -> None: if initial_call: self.trajectory = self.calculate_trajectory( self.shoot_paths_working_copy.pop(0) ) - self.note_manager.try_cancel_intake() - # Do some driving... self.drive_on_trajectory(state_tm) - if self.is_at_goal(): - # If we are in position, remove this note from the list and shoot it - self.next_state("shoot_note") + # And maybe some shooting... + 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() def drive_on_trajectory( self, trajectory_tm: float, enforce_tangent_heading: bool = False ): + if not self.trajectory: + return + # Grabbing the target position at the current point in time from the trajectory. target_state = self.trajectory.sample(trajectory_tm) @@ -168,7 +181,7 @@ def drive_on_trajectory( self.chassis.drive_local( chassis_speed.vx, chassis_speed.vy, - chassis_speed.omega, + 0, ) # if we are enforcing heading, hijack rotational control from the main controller diff --git a/components/chassis.py b/components/chassis.py index 51cdf513..cb00d91a 100644 --- a/components/chassis.py +++ b/components/chassis.py @@ -235,7 +235,7 @@ def __init__(self) -> None: self.on_red_alliance = False - self.modules = [ + self.modules = ( # Front Left SwerveModule( self.WHEEL_BASE / 2, @@ -268,7 +268,7 @@ def __init__(self) -> None: TalonIds.steer_4, CancoderIds.swerve_4, ), - ] + ) self.kinematics = SwerveDrive4Kinematics( self.modules[0].translation, @@ -291,6 +291,21 @@ def __init__(self) -> None: wpilib.SmartDashboard.putData("Heading PID", self.heading_controller) + def get_velocity(self) -> ChassisSpeeds: + return self.kinematics.toChassisSpeeds(self.get_module_states()) + + def get_module_states( + self, + ) -> tuple[ + SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState + ]: + return ( + self.modules[0].get(), + self.modules[1].get(), + self.modules[2].get(), + self.modules[3].get(), + ) + def setup(self) -> None: initial_pose = TeamPoses.RED_TEST_POSE if is_red() else TeamPoses.BLUE_TEST_POSE diff --git a/components/shooter.py b/components/shooter.py index d28a1749..a4541c05 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -117,12 +117,16 @@ def __init__(self) -> None: flywheel_right_config.apply(flywheel_pid) flywheel_right_config.apply(flywheel_gear_ratio) - self.inclinator_controller = PIDController(0.8, 0, 0.05) + self.inclinator_controller = PIDController(5.0, 0, 0) self.inclinator_controller.setTolerance(ShooterComponent.INCLINATOR_TOLERANCE) SmartDashboard.putData(self.inclinator_controller) range = tunable(0.0) + @feedback + def get_applied_output(self) -> float: + return self.inclinator.getAppliedOutput() + def on_enable(self) -> None: self.inclinator_controller.reset() diff --git a/controllers/note.py b/controllers/note.py index bf5db226..bad20a64 100644 --- a/controllers/note.py +++ b/controllers/note.py @@ -87,11 +87,13 @@ def not_holding_note(self, initial_call) -> None: if self.intake_desired: self.shooter.update_range() self.intake.engage() + elif wpilib.DriverStation.isAutonomous(): + self.shooter.update_range() - elif self.cancel_intake_desired: + if self.cancel_intake_desired: self.intake.try_cancel_intake() - elif self.has_note(): + if self.has_note(): self.next_state(self.holding_note) @state(must_finish=True) diff --git a/controllers/shooter.py b/controllers/shooter.py index 0369137d..5e06304e 100644 --- a/controllers/shooter.py +++ b/controllers/shooter.py @@ -2,7 +2,7 @@ from wpimath.geometry import Translation2d -from magicbot import StateMachine, state, timed_state, feedback +from magicbot import StateMachine, state, timed_state, feedback, tunable from components.chassis import ChassisComponent from components.intake import IntakeComponent @@ -18,9 +18,8 @@ class Shooter(StateMachine): intake_component: IntakeComponent status_lights: LightStrip - # make sure this is always > chassis heading tolerance - ANGLE_TOLERANCES = (math.radians(5), math.radians(1)) - RANGES = (0, 5) + SPEED_LIMIT = tunable(1) + SPINNING_SPEED_LIMIT = tunable(1) def __init__(self): self.range = 0.0 @@ -65,11 +64,20 @@ def aiming(self, initial_call) -> None: self.is_aiming_finished() and self.shooter_component.is_ready() and self.in_range() + and self.is_below_speed_limit() + and self.is_below_spinning_limit() ): self.next_state(self.firing) else: self.aim() + def is_below_speed_limit(self) -> bool: + vel = self.chassis.get_velocity() + return math.hypot(vel.vx, vel.vy) < self.SPEED_LIMIT + + def is_below_spinning_limit(self) -> bool: + return self.chassis.get_velocity().omega < self.SPINNING_SPEED_LIMIT + def aim(self) -> None: # Update range self.update_range() @@ -78,7 +86,7 @@ def aim(self) -> None: translation_to_goal = self.translation_to_goal() # We need to aim at least a note's radius inside the outer bounds of the goal. Also add a safety margin - margin = 0.05 + margin = 0.10 offset = (SPEAKER_HOOD_WIDTH - NOTE_DIAMETER) / 2.0 - margin offset_bearing = constrain_angle( math.atan2(translation_to_goal.y + offset, translation_to_goal.x) + math.pi diff --git a/physics.py b/physics.py index 0106ba78..581f6d1c 100644 --- a/physics.py +++ b/physics.py @@ -70,7 +70,9 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot): self.physics_controller = physics_controller self.kinematics: SwerveDrive4Kinematics = robot.chassis.kinematics - self.swerve_modules: list[SwerveModule] = robot.chassis.modules + self.swerve_modules: tuple[ + SwerveModule, SwerveModule, SwerveModule, SwerveModule + ] = robot.chassis.modules # Motors self.wheels = [ diff --git a/utilities/position.py b/utilities/position.py index 891ba90b..20258942 100644 --- a/utilities/position.py +++ b/utilities/position.py @@ -14,6 +14,9 @@ def __init__(self, waypoints: list[Translation2d]): self.final_heading = Rotation2d(0) +stage_tolerance = 0.35 + + class NotePositions: # These are the 3 close notes, named for the nearest element amp = Translation2d(13.645, 7.00045) @@ -29,9 +32,13 @@ class NotePositions: # The podium note is very close to the stage leg so we need different positions to avoid collisions # Directions are in the field coordinate system for red side - podium_NW = podium + Translation2d(0.5, 0.5) - podium_NE = podium + Translation2d(0.5, -0.5) - podium_N = podium + Translation2d(0.5, 0) + podium_N = podium + Translation2d(stage_tolerance, 0) + podium_NW = podium + Translation2d( + stage_tolerance / math.sqrt(2), stage_tolerance / math.sqrt(2) + ) + podium_NE = podium + Translation2d( + stage_tolerance / math.sqrt(2), -stage_tolerance / math.sqrt(2) + ) class ShootingPositions: