def explore(room): # Move to a new area, returns area object # initialize room.current_area if it does not exsist if not room.current_area: room.current_area = Area() # Test output print "Exploring (moving to a new location) ..." # Beep to indicate begining of explore step buzzer = Buzzer() buzzer.play(5) # Create new area and move objects new_area = Area() move = Move() # Make sure move is set as a real move (performed by robot) move.type = "Real" # Link it to the previous object new_area.previous.append(room.current_area.name) # Initial position set to position of previous area move.initial_pos = room.current_area.pos # Vector of movement used move = get_move_vector(move) # Break down movement vector into motion primitives that robot can execute move.getMotionPlan() for (direction, amount) in move.primitives: print "Moving " + str(direction) + " " + str(amount) # moveBot(direction, amount, params.p['MOTOR_PWR']) # Get final position by summing initial position and delta move.final_pos = [init + delt for init, delt in zip(move.initial_pos, move.delta)] # TODO: put a kalman filter on the movement. Use camera and sonar as # truth? Not sure here # # Test print # print "Describing move in explore function" move.describe() # Update location of new area to final position of move new_area.pos = move.final_pos # Add move to new area's dictionary of moves new_area.moves_performed[room.current_area.name] = move # Redirect current area to this new_area room.current_area = new_area # Localize bot in new area # new_area.localize # # Test print # print "Describing new_area in explore function" # new_area.describe() # Append new_area to room room.areas.append(new_area) # Return updated room object return room