def detectObstacle(robot: cozmo.robot.Robot, cmap, stack, cube): print("Detect Obstacle State") # cmap.set_start(Node((robot.pose.position.x, robot.pose.position.y))) # nodes = [Node((cube.pose.position.x-5, cube.pose.position.y-5)),Node((cube.pose.position.x+5, cube.pose.position.y+5)),Node ((cube.pose.position.x-5, cube.pose.position.y+5)),Node((cube.pose.position.x+5, cube.pose.position.y-5))] # cmap.add_obstacle(nodes) robot.findPath() #trigger to go to FindPath state return robot, stack, cube # returns cozmo
def findTarget(robot: cozmo.robot.Robot, cmap, stack, cube): # ADD CODE HERE # Cozmo should move towards the middle and scout the area until the cube is found! print("Find Target State") print(robot.pose_angle.degrees) robot.turn_in_place(degrees(45)).wait_for_completed() robot.drive_straight(distance_mm(250), speed_mmps(50)).wait_for_completed() lookAround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) cube = None try: cube = robot.world.wait_for_observed_light_cube() lookAround.stop() except asyncio.TimeoutError: print("did not find cube") # cube.object_id how you detect which CUBE you are using if cube is not None: print(cube) if cube.object_id == cubeID: cmap.set_start(Node((robot.pose.position.x, robot.pose.position.y))) cmap.add_goal(Node((cube.pose.position.x,cube.pose.position.y))) robot.findPath() # trigger to go to FindPath state return robot, stack, cube