def __init__(self):
super().__init__()
self.points = (
Waypoint(0, 0, 0, 1),
Waypoint(0.8, 0, 0, 0.5),
Waypoint(1, 0, 0, 0.2),
)
self.pursuit = PurePursuit(look_ahead=0.2,
look_ahead_speed_modifier=0.25)
def on_enable(self):
super().on_enable()
self.chassis.odometry_x = 0
self.chassis.odometry_y = 0
self.points = (
self.current_pos,
Waypoint(2, 0, 0, 1),
Waypoint(2, 2, 0, 1),
Waypoint(0, 2, 0, 1),
)
self.pursuit = PurePursuit(look_ahead=0.2,
look_ahead_speed_modifier=0.0)
def drive_to_loading_bay(self, initial_call):
if initial_call:
if self.completed_runs == 1:
waypoints = insert_trapezoidal_waypoints(
(
self.current_pos,
Waypoint(
self.current_pos[0] - 0.5,
self.current_pos[1],
self.imu.getAngle(),
1.5,
),
self.setup_loading_bay,
self.loading_bay,
),
self.acceleration,
self.deceleration,
)
elif self.completed_runs == 2:
waypoints = insert_trapezoidal_waypoints(
(self.current_pos, self.setup_loading_bay,
self.loading_bay),
self.acceleration,
self.deceleration,
)
else:
self.next_state("stop")
return
self.pursuit.build_path(waypoints)
self.follow_path()
if (self.vision.fiducial_in_sight
and self.ready_for_vision()) or self.pursuit.completed_path:
self.next_state("intake_hatch")
def drive_forwards(self, initial_call):
if initial_call:
waypoints = insert_trapezoidal_waypoints(
(self.current_pos, Waypoint(1.5, 0, 0, 0)),
acceleration=self.chassis.acceleration,
deceleration=self.chassis.deceleration,
)
self.pursuit.build_path(waypoints)
self.follow_path()
if self.pursuit.completed_path:
self.chassis.set_inputs(0, 0, 0)
self.done()
def __init__(self):
super().__init__()
self.front_cargo_bay = Waypoint(5.6 - SwerveChassis.LENGTH / 2, 0.2, 0,
0.75)
self.setup_loading_bay = Waypoint(3.3, 3.3, math.pi, 2)
self.loading_bay = Waypoint(0.2 + SwerveChassis.LENGTH / 2, 3.4,
math.pi, 1)
self.side_cargo_bay = Waypoint(7, 0.8 + SwerveChassis.WIDTH / 2,
-math.pi / 2, 1)
self.side_cargo_bay_alignment_point = Waypoint(
7, 1.8 + SwerveChassis.WIDTH / 2, -math.pi / 2, 0.75)
self.start_pos = Waypoint(
1.2 + SwerveChassis.LENGTH / 2,
0,
0,
2,
# + SwerveChassis.WIDTH / 2
)
self.completed_runs = 0
self.desired_angle = 0
self.desired_angle_navx = 0
self.minimum_path_completion = 0.85
self.acceleration = 1
self.deceleration = -0.5
self.pursuit = PurePursuit(look_ahead=0.2,
look_ahead_speed_modifier=0.25)
def current_pos(self):
return Waypoint(self.chassis.odometry_x, self.chassis.odometry_y,
self.imu.getAngle(), 2)
class Coordinates:
start_pos: Waypoint
start_pos_cargo: Waypoint
front_cargo_ship: Waypoint
setup_loading_bay: Waypoint
loading_bay: Waypoint
cargo_depot_setup: Waypoint
cargo_depot: Waypoint
side_cargo_ship_alignment_point: Waypoint
side_cargo_ship_middle: Waypoint
side_cargo_ship: Waypoint
cargo_endpoint: Waypoint
left_coordinates = Coordinates(
start_pos=Waypoint(1.3 + SwerveChassis.LENGTH / 2,
0 + SwerveChassis.WIDTH / 2, 0, 2),
start_pos_cargo=Waypoint(1.3 + SwerveChassis.LENGTH / 2,
1.63 - SwerveChassis.WIDTH / 2, 0, 2),
front_cargo_ship=Waypoint(5.5 - SwerveChassis.LENGTH / 2, 0.3, 0, 1.5),
setup_loading_bay=Waypoint(3.3, 3, math.pi, 2),
loading_bay=Waypoint(0.2 + SwerveChassis.LENGTH / 2, 3.4, math.pi, 1.5),
cargo_depot_setup=Waypoint(2.4, 3, -math.atan2(2.4 - 1.2, 3 - 2.3), 1),
cargo_depot=Waypoint(1.2, 2.3, -math.atan2(2.4 - 1.2, 3 - 2.3), 0.5),
side_cargo_ship_alignment_point=Waypoint(6.6,
1.8 + SwerveChassis.WIDTH / 2,
-math.pi / 2, 1.5),
side_cargo_ship_middle=Waypoint(
(6.6 + 7.4) / 2, 0.8 + SwerveChassis.WIDTH / 2, math.pi / 2, 0.75),
side_cargo_ship=Waypoint(6.6, 0.8 + SwerveChassis.WIDTH / 2, -math.pi / 2,
1),
cargo_endpoint=Waypoint(7.4, 1.3, 0, 3),
def reflect_y(v: Waypoint) -> Waypoint:
return Waypoint(v.x, -v.y, v.theta, v.v)
from utilities.navx import NavX
from utilities.pure_pursuit import PurePursuit, Waypoint, insert_trapezoidal_waypoints
@dataclass
class Coordinates:
start_pos: Waypoint
front_cargo_bay: Waypoint
setup_loading_bay: Waypoint
loading_bay: Waypoint
side_cargo_bay_alignment_point: Waypoint
side_cargo_bay: Waypoint
left_coordinates = Coordinates(
start_pos=Waypoint(1.2 + SwerveChassis.LENGTH / 2,
0 + SwerveChassis.WIDTH / 2, 0, 2),
front_cargo_bay=Waypoint(5.5 - SwerveChassis.LENGTH / 2, 0.3, 0, 1.5),
setup_loading_bay=Waypoint(3.3, 3, math.pi, 1),
loading_bay=Waypoint(0.2 + SwerveChassis.LENGTH / 2, 3.4, math.pi, 1),
side_cargo_bay_alignment_point=Waypoint(6.6, 1.8 + SwerveChassis.WIDTH / 2,
-math.pi / 2, 1.5),
side_cargo_bay=Waypoint(6.6, 0.8 + SwerveChassis.WIDTH / 2, -math.pi / 2,
1),
)
right_coordinates = Coordinates(
start_pos=left_coordinates.start_pos.reflect(),
front_cargo_bay=left_coordinates.front_cargo_bay.reflect(),
setup_loading_bay=left_coordinates.setup_loading_bay.reflect(),
loading_bay=left_coordinates.loading_bay.reflect(),
side_cargo_bay_alignment_point=left_coordinates.
side_cargo_bay_alignment_point.reflect(),