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()