def main():
# Initialize frame rate calculation
frame_rate_calc = 1
freq = cv2.getTickFrequency()
font = cv2.FONT_HERSHEY_SIMPLEX
objectifier = Model()
# Start serial connection to arduino
ser = serial.Serial('/dev/ttyACM0', 9600, timeout=2)
time.sleep(1)
# Initialize queues
pic_q = queue.LifoQueue(5)
command_q = queue.Queue()
# Inizialize grid anf gridmovement
grid = Grid(8,8)
movement = GridMovement(grid, ser)
# Initialize VideoThread
vt = VideoThread(pic_q, objectifier)
vt.start()
# Setup GPIO
GPIO.setmode(GPIO.BOARD)
GPIO.setup(BUTTONPIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(CONTACT_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(LEDPIN, GPIO.OUT, initial=GPIO.LOW)
# Keep track of movements after approach is called
approach_movement_list = queue.LifoQueue()
wait_for_button(GPIO)
time.sleep(2)
#
# RUN ROUND
#
print("Starting round")
begin_round(movement, pic_q)
# map the targets from json file
map_JSON(mar1.json,movement)
# now set the maximum amount of obstacles based on amount of targets
grid.set_obstacles_max()
begin_round(movement, pic_q)
print("I will try and map the mothership")
map_mothership(movement, pic_q)
print("Mothership is located in the following tiles: ", grid.mothership)
# We can save these values in movement.access_point so other functions with access to movement
# can use these
mothership_angle, dist, side_angle = approach_mothership_side(movement, pic_q, ser, GPIO)
movement.set_mothership_angle(mothership_angle)
movement.set_side_angle(side_angle)
movement.set_access_dist(dist)
print("Mothership angle: {}, Distance: {}, Side_angle: {}".format(mothership_angle, dist, side_angle))
print("Going home")
go_home(movement, pic_q)
while grid.targets:
# find closest target and set it as the goal
movement.current_target = closest_point(grid.targets, movement.current)
movement.set_goal(movement.current_target)
follow_path(movement, pic_q)
# Once we reach the target we attempt to pick it up
approach(movement, pic_q)
success, target_id = check_pick_up(movement, pic_q)
print("Success: {}, Target Id: {}".format(success, target_id))
if not success:
go_home(movement, pic_q)
else:
grid.targets.remove(movement.current_target)
# We would correct alignment here if errors were fixed
# Move to mothership and drop target
print("Access point is: ",movement.get_access_point())
movement.set_goal(movement.get_access_point())
follow_path(movement, pic_q, True)
movement.face(movement.get_side_point())
print("Going to drop it")
mothership_drop(dist, mothership_angle, side_angle, target_id, movement, serial, pic_q)
go_home(movement, pic_q)
# Once all targets are delivered, go home and turn on finishing light
go_home(movement, pic_q)
wait_for_button(GPIO)
# close video thread
vt.join()
