Skip to content

Commit

Permalink
Merge branch 'main' into enforce-climb-state
Browse files Browse the repository at this point in the history
  • Loading branch information
@mlists
mlists authored Mar 11, 2024
2 parents 7b701e9 + f4beb9a commit 636819b
Show file tree
Hide file tree
Showing 17 changed files with 454 additions and 197 deletions.
103 changes: 79 additions & 24 deletions autonomous/autonomous.py
View file
Original file line number Diff line number Diff line change
@@ -1,23 +1,60 @@
from utilities.position import NotePositions, Path, ShootingPositions, PathPositions
from autonomous.base import AutoBase, rotation_to_red_speaker
from wpimath.geometry import Pose2d, Translation2d
import math

from utilities.position import (
NotePositions,
Path,
ShootingPositions,
PathPositions,
)
from utilities import game
from autonomous.base import AutoBase
from wpimath.geometry import Pose2d, Translation2d, Rotation2d


def rotation_to_red_speaker(position: Translation2d) -> Rotation2d:
t = game.RED_SPEAKER_POSE.toPose2d().translation() - position
return t.angle() + Rotation2d(math.pi)


class PodiumSpeakerAmpTopcentre(AutoBase):
MODE_NAME = "5 notes: internal, podium, speaker, amp, top centre"

def __init__(self) -> None:
note_paths = [
Path([NotePositions.podium_NW], face_target=False),
Path([NotePositions.speaker], face_target=False),
Path([NotePositions.amp], face_target=False),
Path([PathPositions.avoid_wall, NotePositions.Centre1], face_target=False),
]
shoot_paths = [
Path([ShootingPositions.close_straight], face_target=True),
Path([ShootingPositions.amp_speaker_bounce], face_target=True),
Path([NotePositions.amp], face_target=True),
Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
]
super().__init__(note_paths, shoot_paths)


class PodiumSpeakerAmp(AutoBase):
MODE_NAME = "4 notes: internal, podium, speaker, amp"

def __init__(self) -> None:
note_paths = [
Path([NotePositions.podium_NW]),
Path([NotePositions.speaker]),
Path([NotePositions.amp]),
Path([NotePositions.podium_NW], face_target=False),
Path([NotePositions.speaker], face_target=False),
Path([NotePositions.amp], face_target=False),
]
shoot_paths = [
Path([ShootingPositions.close_straight]),
Path([ShootingPositions.amp_speaker_bounce]),
Path([NotePositions.amp]),
Path([ShootingPositions.close_straight], face_target=True),
Path([ShootingPositions.amp_speaker_bounce], face_target=True),
Path([NotePositions.amp], face_target=True),
]
super().__init__(note_paths, shoot_paths)
# Start pose only needs to be on the correct half of the field,
# so choose the podium as a reference point
start_pose = Pose2d(
NotePositions.podium, rotation_to_red_speaker(NotePositions.podium)
)
super().__init__(note_paths, shoot_paths, start_pose)


class AmpCentre1(AutoBase):
Expand All @@ -26,14 +63,19 @@ class AmpCentre1(AutoBase):

def __init__(self) -> None:
note_paths = [
Path([NotePositions.amp]),
Path([PathPositions.avoid_wall, NotePositions.Centre1]),
Path([NotePositions.amp], face_target=False),
Path([PathPositions.avoid_wall, NotePositions.Centre1], face_target=False),
]
shoot_paths = [
Path([NotePositions.amp]),
Path([PathPositions.avoid_wall, NotePositions.amp]),
Path([NotePositions.amp], face_target=True),
Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
]
super().__init__(note_paths, shoot_paths)
# Start pose only needs to be on the correct half of the field,
# so choose the amp as a reference point
start_pose = Pose2d(
NotePositions.amp, rotation_to_red_speaker(NotePositions.amp)
)
super().__init__(note_paths, shoot_paths, start_pose)


class SpeakerCentre3(AutoBase):
Expand All @@ -42,14 +84,25 @@ class SpeakerCentre3(AutoBase):

def __init__(self) -> None:
note_paths = [
Path([NotePositions.speaker]),
Path([PathPositions.stage_transition_N, NotePositions.Centre3]),
Path([NotePositions.speaker], face_target=False),
Path(
[PathPositions.stage_transition_N, NotePositions.Centre3],
face_target=False,
),
]
shoot_paths = [
Path([NotePositions.speaker]),
Path([PathPositions.stage_transition_N, NotePositions.speaker]),
Path([NotePositions.speaker], face_target=True),
Path(
[PathPositions.stage_transition_N, NotePositions.speaker],
face_target=True,
),
]
super().__init__(note_paths, shoot_paths)
# Start pose only needs to be on the correct half of the field,
# so choose the speaker as a reference point
start_pose = Pose2d(
NotePositions.speaker, rotation_to_red_speaker(NotePositions.speaker)
)
super().__init__(note_paths, shoot_paths, start_pose)


class Centre3Centre5(AutoBase):
Expand All @@ -65,9 +118,10 @@ def __init__(self) -> None:
PathPositions.stage_transition_S_entry,
PathPositions.stage_transition_S,
NotePositions.Centre3,
]
],
face_target=False,
),
Path([NotePositions.Centre5]),
Path([NotePositions.Centre5], face_target=False),
]

shoot_paths = [
Expand All @@ -76,9 +130,10 @@ def __init__(self) -> None:
PathPositions.stage_transition_S,
PathPositions.stage_transition_S_entry,
ShootingPositions.source_side,
]
],
face_target=True,
),
Path([ShootingPositions.source_side]),
Path([ShootingPositions.source_side], face_target=True),
]
sim_start_pos = Translation2d(15.4, 2.94)
rotation = rotation_to_red_speaker(sim_start_pos)
Expand Down
120 changes: 64 additions & 56 deletions autonomous/base.py
View file
Original file line number Diff line number Diff line change
@@ -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,
Expand Down Expand Up @@ -34,9 +35,10 @@ class AutoBase(AutonomousStateMachine):
intake_component: IntakeComponent

POSITION_TOLERANCE = 0.05
SHOOTING_POSITION_TOLERANCE = 1
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__(
Expand All @@ -51,8 +53,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(
Expand All @@ -61,23 +63,34 @@ def setup(self) -> None:
# Since robot is stationary from one action to another, point the control vector at the goal to avoid the robot taking unnecessary turns before moving towards the goal
self.kD = 0.3

for i, path in enumerate(self.shoot_paths):
self.shoot_paths[i].final_heading = rotation_to_red_speaker(
path.waypoints[-1]
)

self.goal_heading: Rotation2d
self.goal_heading: float
self.trajectory_marker = self.field.getObject("auto_trajectory")
self.trajectory: Optional[Trajectory] = None

def on_enable(self):
# Setup starting position in the simulator
if RobotBase.isSimulation() and self.starting_pose is not None:
starting_pose = self.starting_pose
if not game.is_red():
starting_pose = game.field_flip_pose2d(self.starting_pose)
starting_pose = self.get_starting_pose()
if RobotBase.isSimulation() and starting_pose is not None:
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

def get_starting_pose(self) -> Pose2d | None:
starting_pose = self.starting_pose
if starting_pose is None:
return None
if not game.is_red():
starting_pose = game.field_flip_pose2d(starting_pose)
return starting_pose

@state(first=True)
def initialise(self) -> None:
# Make a working copy of the NotePaths so that we can pop
Expand Down Expand Up @@ -117,70 +130,58 @@ def pick_up(self, state_tm: float, initial_call: bool) -> None:
self.note_manager.try_intake()

# Drive with the intake always facing the tangent
self.drive_on_trajectory(state_tm, enforce_tangent_heading=True)
self.drive_on_trajectory(state_tm)

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)

@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):
if not self.trajectory:
return

def drive_on_trajectory(
self, trajectory_tm: float, enforce_tangent_heading: bool = False
):
# Grabbing the target position at the current point in time from the trajectory.
target_state = self.trajectory.sample(trajectory_tm)

# Calculating the speeds required to get to the target position.
chassis_speed = self.drive_controller.calculate(
self.chassis.get_pose(),
target_state,
self.goal_heading,
Rotation2d(self.goal_heading),
)
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
if enforce_tangent_heading:
speed = Translation2d(chassis_speed.vx, chassis_speed.vy).norm()
if speed > self.ENFORCE_HEADING_SPEED:
field_chassis_speeds = self.chassis.to_field_oriented(chassis_speed)
heading_target = math.atan2(
field_chassis_speeds.vy, field_chassis_speeds.vx
)
self.goal_heading = Rotation2d(heading_target)
self.chassis.snap_to_heading(heading_target)
# let aiming override path headings
if not self.note_manager.shooter.is_executing:
self.chassis.snap_to_heading(self.goal_heading)

def calculate_trajectory(self, path: Path) -> Trajectory:
pose = self.chassis.get_pose()
Expand All @@ -196,7 +197,7 @@ def calculate_trajectory(self, path: Path) -> Trajectory:
for waypoint in path.waypoints[:-1]
]
self.goal = game.field_flip_translation2d(path.waypoints[-1])
self.goal_heading = game.field_flip_rotation2d(path.final_heading)
self.goal_heading = game.field_flip_angle(path.final_heading)

traj_config = TrajectoryConfig(
maxVelocity=self.MAX_VEL, maxAcceleration=self.MAX_ACCEL
Expand Down Expand Up @@ -237,6 +238,18 @@ def calculate_trajectory(self, path: Path) -> Trajectory:
except Exception:
return Trajectory([Trajectory.State(0, 0, 0, pose)])

# face along final path leg if we are not trying to shoot
if not path.face_target:
waypoints = path.waypoints
endpoint = waypoints[-1]
# second last pose might be our our current pose
second_last = waypoints[-2] if len(waypoints) > 1 else pose.translation()
disp = endpoint - second_last
if not game.is_red():
disp = game.field_flip_translation2d(disp)
heading_target = math.atan2(disp.y, disp.x)
self.goal_heading = heading_target

self.trajectory_marker.setTrajectory(trajectory)
return trajectory

Expand All @@ -249,8 +262,3 @@ def done(self):
self.chassis.stop_snapping()
self.trajectory_marker.setPoses([])
super().done()


def rotation_to_red_speaker(position: Translation2d) -> Rotation2d:
t = game.RED_SPEAKER_POSE.toPose2d().translation() - position
return t.angle() + Rotation2d(math.pi)
Loading

0 comments on commit 636819b

Please sign in to comment.