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
