def drive_to_cargo_ship(self, initial_call):
if initial_call:
if self.completed_runs == 0:
waypoints = insert_trapezoidal_waypoints(
(self.current_pos, self.coordinates.front_cargo_ship),
self.chassis.acceleration,
self.chassis.deceleration,
)
elif self.completed_runs == 1:
waypoints = insert_trapezoidal_waypoints(
(
self.current_pos,
self.coordinates.setup_loading_bay,
self.coordinates.front_cargo_ship.reflect(),
),
self.chassis.acceleration,
self.chassis.deceleration,
)
else:
self.next_state("drive_to_loading_bay")
self.completed_runs += 1
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("deposit_hatch")
self.completed_runs += 1
def drive_to_cargo_bay(self, initial_call):
if initial_call:
# print(f"odometry = {self.current_pos}")
if self.completed_runs == 0:
waypoints = insert_trapezoidal_waypoints(
(self.current_pos, self.front_cargo_bay),
self.acceleration,
self.deceleration,
)
elif self.completed_runs == 1:
waypoints = insert_trapezoidal_waypoints(
(
self.current_pos,
self.side_cargo_bay_alignment_point,
self.side_cargo_bay,
),
self.acceleration,
self.deceleration,
)
else:
self.next_state("drive_to_loading_bay")
self.completed_runs += 1
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("deposit_hatch")
self.completed_runs += 1
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 test_trapezoidal():
waypoints = [pp.Waypoint(0, 0, 0, 0), pp.Waypoint(10, 10, 0, 2)]
trap = pp.insert_trapezoidal_waypoints(waypoints, 2, -2)
assert len(trap) == 3
assert trap[1][2] == waypoints[1][2], (
"Intermediate waypoint should have end speed when accelerating: %s" %
trap)
waypoints = [pp.Waypoint(10, 10, 0, 2), pp.Waypoint(0, 0, 0, 0)]
trap = pp.insert_trapezoidal_waypoints(waypoints, 2, -2)
assert len(trap) == 3
assert trap[1][2] == waypoints[0][2], (
"Intermediate waypoint should have beginning speed when decelerating: %s"
% trap)
def drive_to_cargo_depot_setup(self, initial_call):
if initial_call:
waypoints = insert_trapezoidal_waypoints(
(self.current_pos, self.coordinates.cargo_depot_setup),
self.chassis.acceleration,
self.chassis.deceleration,
)
self.pursuit.build_path(waypoints)
self.follow_path()
if self.pursuit.completed_path:
self.done()
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 drive_to_cargo_ship(self, initial_call):
if initial_call:
waypoints = insert_trapezoidal_waypoints(
(
self.current_pos,
self.coordinates.side_cargo_ship_alignment_point,
self.coordinates.side_cargo_ship,
),
self.chassis.acceleration,
self.chassis.deceleration,
)
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("deposit_hatch")
def drive_to_endpoint(self, initial_call):
"""
Move to the point where we hand over to drivers
"""
if initial_call:
waypoints = insert_trapezoidal_waypoints(
(
self.current_pos,
self.coordinates.side_cargo_ship_alignment_point,
self.coordinates.cargo_endpoint,
),
self.chassis.acceleration,
self.chassis.deceleration,
)
self.pursuit.build_path(waypoints)
self.follow_path()
if self.pursuit.completed_path:
self.done()
