def create_objects(config):
# initialize nodelist
nodelist = Nodelist(config)
# start all processes
gps = GPS(config)
compass = Compass(config)
arduino = ArduinoComm(config)
gps.start()
compass.start()
arduino.start()
# wait for arduino to be ready
arduino.wait_for_readiness()
# wait for gps to be fixed
print("waiting for gps fix...")
gps.wait_for_fix()
print("gps fix found!")
# wait for go button to be pressed
print("waiting for go button press...")
arduino.wait_for_button_press()
print("go button pressed!")
# get the first node
node = nodelist.get_next_node()
while not nodelist.all_nodes_visited() and gps.is_properly_alive(
) and compass.is_properly_alive() and arduino.is_properly_alive():
# if at coordinate, get next node and start from top of loop
if gps.is_overlapping(node):
node = nodelist.get_next_node()
continue
desiredHeading = gps.get_desired_heading(compass.get_heading(), node)
print(node.get_coordinate())
print(gps.get_location())
print(gps.calculate_angle_to_node(node.get_coordinate()))
print(compass.get_heading())
print("Desired: {}".format(desiredHeading))
arduino.commandTurn(desiredHeading)
sleep(0.1)
arduino.commandForward(node.get_throttle())
sleep(0.1)
# stop car
for i in range(0, 10):
arduino.commandReset()
sleep(0.005)
sleep(2)
# print possible exceptions
print("Exception ({}): {}".format("GPS", gps.get_raised_exception()))
print("Exception ({}): {}".format("COMPASS",
compass.get_raised_exception()))
print("Exception ({}): {}".format("ARDUINO",
arduino.get_raised_exception()))
# stop all processes
gps.stop()
compass.stop()
arduino.stop()
print("done now!")
def test_gps_relative_angle_to_goal():
gps = GPS(config)
gps.current_coordinate = Coordinate(35.0000, 85.0000)
test_fixtures = []
# Goal is to the East
test_fixtures.append((Coordinate(35.0000, 86.0000), 90, 0)) # facing East
test_fixtures.append((Coordinate(35.0000,
86.0000), 45, 45)) # facing North-East
test_fixtures.append((Coordinate(35.0000, 86.0000), 0, 90)) # facing North
test_fixtures.append((Coordinate(35.0000,
86.0000), -45, 135)) # facing North-West
test_fixtures.append((Coordinate(35.0000,
86.0000), -90, 180)) # facing West
test_fixtures.append(
(Coordinate(35.0000, 86.0000), -135, -135)) # facing South-West
test_fixtures.append((Coordinate(35.0000,
86.0000), 180, -90)) # facing South
test_fixtures.append((Coordinate(35.0000,
86.0000), 135, -45)) # facing South-East
# Goal is to the North
test_fixtures.append((Coordinate(36.0000,
85.0000), 90, -90)) # facing East
test_fixtures.append((Coordinate(36.0000,
85.0000), 45, -45)) # facing North-East
test_fixtures.append((Coordinate(36.0000, 85.0000), 0, 0)) # facing North
test_fixtures.append((Coordinate(36.0000,
85.0000), -45, 45)) # facing North-West
test_fixtures.append((Coordinate(36.0000,
85.0000), -90, 90)) # facing West
test_fixtures.append((Coordinate(36.0000,
85.0000), -135, 135)) # facing South-West
test_fixtures.append((Coordinate(36.0000,
85.0000), 180, 180)) # facing South
test_fixtures.append((Coordinate(36.0000,
85.0000), 135, -135)) # facing South-East
# Goal is to the West
test_fixtures.append((Coordinate(35.0000,
84.0000), 90, 180)) # facing East
test_fixtures.append((Coordinate(35.0000,
84.0000), 45, -135)) # facing North-East
test_fixtures.append((Coordinate(35.0000,
84.0000), 0, -90)) # facing North
test_fixtures.append((Coordinate(35.0000,
84.0000), -45, -45)) # facing North-West
test_fixtures.append((Coordinate(35.0000, 84.0000), -90, 0)) # facing West
test_fixtures.append((Coordinate(35.0000,
84.0000), -135, 45)) # facing South-West
test_fixtures.append((Coordinate(35.0000,
84.0000), 180, 90)) # facing South
test_fixtures.append((Coordinate(35.0000,
84.0000), 135, 135)) # facing South-East
# Goal is to the South
test_fixtures.append((Coordinate(34.0000, 85.0000), 90, 90)) # facing East
test_fixtures.append((Coordinate(34.0000,
85.0000), 45, 135)) # facing North-East
test_fixtures.append((Coordinate(34.0000,
85.0000), 0, 180)) # facing North
test_fixtures.append((Coordinate(34.0000,
85.0000), -45, -135)) # facing North-West
test_fixtures.append((Coordinate(34.0000,
85.0000), -90, -90)) # facing West
test_fixtures.append((Coordinate(34.0000,
85.0000), -135, -45)) # facing South-West
test_fixtures.append((Coordinate(34.0000,
85.0000), 180, 0)) # facing South
test_fixtures.append((Coordinate(34.0000,
85.0000), 135, 45)) # facing South-East
# test if expectations are met
for coordinate, heading, expected_angle in test_fixtures:
calculated_angle = gps.get_desired_heading(heading, coordinate)
print("calc: {}, expect: {}".format(calculated_angle, expected_angle))
assert calculated_angle == expected_angle