def pickup_cube(robot: robot.Robot): robot.set_head_angle(util.degrees(-5.0)).wait_for_completed() print("Cozmo is waiting until he sees a cube") cube = robot.world.wait_for_observed_light_cube() print("Cozmo found a cube, and will now attempt to pick it up it:") pickupCube = robot.pickup_object(cube, True, False, num_retries=1) pickupCube.wait_for_completed() driveForward = robot.drive_straight(util.distance_mm(100), util.speed_mmps(100)) driveForward.wait_for_completed() placeCube = robot.place_object_on_ground_here(cube, False, 2) placeCube.wait_for_completed() driveBackward = robot.drive_straight(util.distance_mm(-100), util.speed_mmps(100)) driveBackward.wait_for_completed()
def inspection_state(robot: robot.Robot): isLifted = False for _ in range(4): action1 = robot.drive_straight(util.distance_mm(200), util.speed_mmps(100)) # robot.drive_straight(util.distance_mm(200), util.speed_mmps(100)) if not isLifted: # robot.set_lift_height(1.0, 20.0, 20.0, 3.0, True, 2) action2 = robot.set_lift_height(1.0, 20.0, 20.0, 3.0, True, 2) isLifted = True else: # robot.set_lift_height(0.0, 20.0, 20.0, 3.0, True, 2) action2 = robot.set_lift_height(0.0, 20.0, 20.0, 3.0, True, 2) isLifted = False action1.wait_for_completed() action2.wait_for_completed() robot.turn_in_place(util.degrees(90)).wait_for_completed() # # #Sets Lift back to 0 height and state = idle. robot.set_lift_height(0.0, 20.0, 20.0, 3.0, True, 2)
def light_cubes(robot: cozmo.robot.Robot): robot.say_text("I'm just getting ready").wait_for_completed() global x global cube_picked global y x = "" y = "" robot.say_text( "Ok I'm looking for your stuff. When it lights up I've found it" ).wait_for_completed() lookaround = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) cubes = robot.world.wait_until_observe_num_objects( num=3, object_type=cozmo.objects.LightCube, timeout=60) lookaround.stop() red_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 0, 0))) blue_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(0, 0, 255))) yellow__light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 255, 0))) # tag each cube found as a different colour red_cube = robot.world.get_light_cube(LightCube1Id) blue_cube = robot.world.get_light_cube(LightCube2Id) yellow_cube = robot.world.get_light_cube(LightCube3Id) red_cube.set_lights(red_light) blue_cube.set_lights(blue_light) yellow_cube.set_lights(yellow__light) robot.say_text( "Ok, pick one and then press confirm and I'll get it for you" ).wait_for_completed() global root_window root_window = Tk() button1 = Button(root_window, text="Phone", bg="yellow", command=yellow_clicked, height=4, width=17) button2 = Button(root_window, text="TV Remote", bg="red", command=red_clicked, height=4, width=17) button3 = Button(root_window, text="Medication", bg="light blue", command=blue_clicked, height=4, width=17) button5 = Button(root_window, text="", bg="grey", height=4, width=17) button6 = Button(root_window, text="", bg="grey", height=4, width=17) button7 = Button(root_window, text="", bg="grey", height=4, width=17) button8 = Button(root_window, text="", bg="grey", height=4, width=17) button9 = Button(root_window, text="", bg="grey", height=4, width=17) button10 = Button(root_window, text="", bg="grey", height=4, width=17) global button4 button4 = Button(root_window, text="Cozmo is waiting", command=confirm_clicked, height=4, width=20) global label1 label1 = Label(root_window, text="Nothing selected yet.", height=4, width=25) button1.grid(row=1, column=1) button2.grid(row=1, column=2) button3.grid(row=1, column=3) button5.grid(row=2, column=1) button6.grid(row=2, column=2) button7.grid(row=2, column=3) button8.grid(row=3, column=1) button9.grid(row=3, column=2) button10.grid(row=3, column=3) label1.grid(row=2, column=4) button4.grid(row=2, column=5) root_window.mainloop() while x == "": sleep(1) if x == "Blue": cube_picked = blue_cube elif x == "Red": cube_picked = red_cube elif x == "Yellow": cube_picked = yellow_cube targ = cube_picked if len(cubes) < 3: print("Error") else: robot.pickup_object(targ, num_retries=3).wait_for_completed() robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(180)), relative_to_robot=False).wait_for_completed() robot.say_text("Is this the right one?").wait_for_completed() global confirmationWindow confirmationWindow = Tk() confirmationWindow.geometry("255x70") buttonYES = Button(confirmationWindow, text="YES", bg="green", command=yes_command, height=4, width=17) buttonNO = Button(confirmationWindow, text="NO", bg="red", command=no_command, height=4, width=17) buttonNO.grid(row=2, column=1) buttonYES.grid(row=2, column=2) confirmationWindow.mainloop() while y == "": sleep(1) if y == "yes": robot.say_text("YAY", play_excited_animation=True).wait_for_completed() robot.move_lift(-3) robot.drive_straight(distance_mm(-200), speed_mmps(80)).wait_for_completed() elif y == "no": robot.drive_straight(distance_mm(-200), speed_mmps(50)).wait_for_completed() robot.turn_in_place(degrees(180)).wait_for_completed() robot.move_lift(-5) robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(180)), relative_to_robot=False).wait_for_completed() robot.say_text( "Select something else and I'll get it.").wait_for_completed() exit
def cozmo_program(robot: cozmo.robot.Robot): success = True #see what Cozmo sees robot.camera.image_stream_enabled = True #connect to cubes (in case Cozmo was disconnected from the cubes) robot.world.connect_to_cubes() #identify cubes cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart cube3 = robot.world.get_light_cube(LightCube3Id) # looks like the letters 'ab' over 'T' if cube1 is not None: cube1.set_lights(cozmo.lights.red_light) else: cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.") if cube2 is not None: cube2.set_lights(cozmo.lights.green_light) else: cozmo.logger.warning("Cozmo is not connected to a LightCube2Id cube - check the battery.") if cube3 is not None: cube3.set_lights(cozmo.lights.blue_light) else: cozmo.logger.warning("Cozmo is not connected to a LightCube3Id cube - check the battery.") #have the user tap each of the cubes, in order try: robot.say_text("Tap the red cube and make me say something.").wait_for_completed() cube1.wait_for_tap(timeout=10) except asyncio.TimeoutError: robot.say_text("The red cube was not tapped").wait_for_completed() success = False finally: cube1.set_lights_off() if success: robot.say_text("I'm moving to the right.").wait_for_completed() robot.turn_in_place(cozmo.util.degrees(-90)).wait_for_completed() robot.drive_straight(cozmo.util.distance_mm(50), cozmo.util.speed_mmps(150)).wait_for_completed() else: robot.say_text("You didn't tap the cube properly.").wait_for_completed() success = True try: robot.say_text("Tap the green cube so I can take a picture.").wait_for_completed() cube2.wait_for_tap(timeout=10) except asyncio.TimeoutError: robot.say_text("The green cube was not tapped").wait_for_completed() success = False finally: cube2.set_lights_off() if (success): robot.say_text("I see that you were paying attention. I will take a picture.").wait_for_completed() else: robot.say_text("Do you know how to tap a cube? I will take a picture anyway.").wait_for_completed() success = True new_im = robot.world.wait_for(cozmo.world.EvtNewCameraImage) new_im.image.raw_image.show() #save the raw image as a bmp file img_latest = robot.world.latest_image.raw_image img_convert = img_latest.convert('L') img_convert.save("aPhoto.bmp") #save the raw image data as a png file, named imageName imageName = "myPhoto.png" img = Image.open("aPhoto.bmp") width, height = img.size new_img = img.resize( (width, height) ) new_img.save( imageName, 'png') try: robot.say_text("Tap the blue cube and make me do something.").wait_for_completed() cube3.wait_for_tap(timeout=10) except asyncio.TimeoutError: robot.say_text("The blue cube was not tapped").wait_for_completed() success = False finally: cube3.set_lights_off() cube = robot.world.wait_for_observed_light_cube() if (success): robot.say_text("Well done! I will pop a wheelie.").wait_for_completed() else: robot.say_text("Do you know how to tap a cube? I will pop a wheelie.").wait_for_completed() success = True action = robot.pop_a_wheelie(cube, num_retries=2) action.wait_for_completed() robot.say_text("Oh, Please help me.").wait_for_completed() return