Skip to content

Commit

Permalink
Merge pull request #188 from thedropbears/driver-aid
Browse files Browse the repository at this point in the history
Driver setup aids
  • Loading branch information
LucienMorey authored Mar 11, 2024
2 parents a7b2cca + ab40aa5 commit 88a3da9
Show file tree
Hide file tree
Showing 5 changed files with 68 additions and 5 deletions.
21 changes: 18 additions & 3 deletions autonomous/autonomous.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,12 @@ def __init__(self) -> None:
Path([ShootingPositions.amp_speaker_bounce]),
Path([NotePositions.amp]),
]
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 @@ -52,7 +57,12 @@ def __init__(self) -> None:
Path([NotePositions.amp]),
Path([PathPositions.avoid_wall, NotePositions.amp]),
]
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 @@ -68,7 +78,12 @@ def __init__(self) -> None:
Path([NotePositions.speaker]),
Path([PathPositions.stage_transition_N, NotePositions.speaker]),
]
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 Down
9 changes: 9 additions & 0 deletions components/led.py
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,15 @@ def morse(self) -> None:
def rainbow(self) -> None:
self.pattern = Rainbow(HsvColour.RED)

def invalid_start(self) -> None:
self.pattern = Flash(HsvColour.RED)

def missing_start_pose(self) -> None:
self.pattern = Flash(HsvColour.CYAN)

def no_vision(self) -> None:
self.pattern = Flash(HsvColour.ORANGE)

def disabled(self) -> None:
self.pattern = Solid(HsvColour.OFF)

Expand Down
9 changes: 9 additions & 0 deletions components/vision.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
import math
import time
from typing import Optional

import wpilib
Expand All @@ -21,6 +22,9 @@ class VisualLocalizer:
# Give bias to the best pose by multiplying this const to the alt dist
BEST_POSE_BIAS = 1.2

# Time since the last target sighting we allow before informing drivers
TIMEOUT = 1.0 # s

add_to_estimator = tunable(True)
should_log = tunable(False)

Expand All @@ -41,6 +45,7 @@ def __init__(
self.camera = PhotonCamera(name)
self.camera_to_robot = Transform3d(pos, rot).inverse()
self.last_timestamp = -1
self.last_recieved_timestep = -1.0

self.single_best_log = field.getObject(name + "single_best_log")
self.single_alt_log = field.getObject(name + "single_alt_log")
Expand Down Expand Up @@ -68,6 +73,7 @@ def execute(self) -> None:

if timestamp == self.last_timestamp:
return
self.last_recieved_timestep = time.monotonic()
self.last_timestamp = timestamp

if results.multiTagResult.estimatedPose.isPresent:
Expand Down Expand Up @@ -140,6 +146,9 @@ def execute(self) -> None:
)
)

def sees_target(self):
return time.monotonic() - self.last_recieved_timestep < self.TIMEOUT


def estimate_poses_from_apriltag(
cam_to_robot: Transform3d, target: PhotonTrackedTarget
Expand Down
28 changes: 26 additions & 2 deletions robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,12 @@
from controllers.intake import Intake
from controllers.shooter import Shooter

from utilities.game import is_red

from autonomous.base import AutoBase

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


class MyRobot(magicbot.MagicRobot):
Expand Down Expand Up @@ -192,6 +193,29 @@ def disabledPeriodic(self) -> None:
self.vision_port.execute()
self.vision_starboard.execute()

# check if we can see targets
if (
not self.vision_port.sees_target()
and not self.vision_starboard.sees_target()
):
self.status_lights.no_vision()
else:
# check we start on the correct side of the stage
selected_auto = self._automodes.chooser.getSelected()
if isinstance(selected_auto, AutoBase):
intended_start_pose = selected_auto.get_starting_pose()
if intended_start_pose is not None:
if on_same_side_of_stage(
intended_start_pose, self.chassis.get_pose()
):
self.status_lights.rainbow()
else:
self.status_lights.invalid_start()
else:
self.status_lights.missing_start_pose()
else:
self.status_lights.missing_start_pose()

def autonomousInit(self) -> None:
pass

Expand Down
6 changes: 6 additions & 0 deletions utilities/position.py
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,12 @@ class TeamPoses:
RED_PODIUM = field_flip_pose2d(BLUE_PODIUM)


def on_same_side_of_stage(pose1: Pose2d, pose2: Pose2d) -> bool:
return not (
(pose1.y > TeamPoses.BLUE_PODIUM.y) ^ (pose2.y > TeamPoses.BLUE_PODIUM.y)
)


class PathPositions:
stage_transition_N = Translation2d(11.4, 4.5)
stage_transition_S = Translation2d(11.4, 3.74)
Expand Down

0 comments on commit 88a3da9

Please sign in to comment.