Skip to content

Instantly share code, notes, and snippets.

@ldaume

ldaume/cozmo_unleashed-devel.py

Forked from acidzebra/cozmo_unleashed-devel.py
Created December 30, 2017 18:09
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save ldaume/1cc0dade705d69bbd5a4ed64797f7f7d to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save ldaume/1cc0dade705d69bbd5a4ed64797f7f7d to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. @acidzebra acidzebra revised this gist Oct 9, 2017. 1 changed file with 1 addition and 0 deletions.
    1 change: 1 addition & 0 deletions cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -38,6 +38,7 @@
    def cozmo_unleashed(robot: cozmo.robot.Robot):
    global freeplay
    global chargerequired
    robot.enable_facial_expression_estimation(enable=True)
    robot.world.disconnect_from_cubes()
    robot.enable_all_reaction_triggers(False)
    robot.enable_stop_on_cliff(True)
  2. @acidzebra acidzebra revised this gist Oct 9, 2017. 1 changed file with 0 additions and 1 deletion.
    1 change: 0 additions & 1 deletion cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -110,7 +110,6 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    os.system('cls' if os.name == 'nt' else 'clear')
    if playokay == 1:
    print("State: charged, schedule OK but not active, sleep loop %d of 30 before next check." % (x))
    x==20
    else:
    print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
    i = random.randint(1, 100)
  3. @acidzebra acidzebra revised this gist Oct 9, 2017. 1 changed file with 17 additions and 11 deletions.
    28 changes: 17 additions & 11 deletions cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -44,7 +44,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.set_robot_volume(0.04)
    os.system('cls' if os.name == 'nt' else 'clear')
    while True:
    #State 1: on charger, charging - wait
    #State 1: on charger, charging - wait for full charge
    if (robot.is_on_charger == 1) and (robot.is_charging == 1):
    #reset charge required (I know we're still charging, trust me)
    chargerequired=0
    @@ -53,10 +53,19 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    i = random.randint(1, 100)
    if i == 100:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    elif i >= 85:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    else:
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    time.sleep(5)
    time.sleep(5)
    #TODO: figure out how to smoothly cycle lights
    @@ -131,20 +140,17 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    if (robot.battery_voltage > 3.7) and (robot.is_on_charger == 0):
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: freeplay, battery %s" % str(round(robot.battery_voltage, 2)))
    #were we already looking for the charger?
    if chargerequired==1:
    print ("in freeplay loop but charge is required.")
    break
    #initiate freeplay
    if freeplay==0:
    if freeplay==0 and chargerequired==0:
    print("freeplay active")
    robot.world.connect_to_cubes()
    robot.enable_all_reaction_triggers(True)
    robot.start_freeplay_behaviors()
    freeplay=1
    time.sleep(5)
    #State 4: not on charger, low battery - seek charger
    if (robot.battery_voltage <= 3.7) and (robot.is_on_charger == 0):
    if chargerequired==0:
    time.sleep(5)
    #State 4: not on charger, low battery, or chargerequired flag is set - seek charger
    if (robot.battery_voltage <= 3.7) and (robot.is_on_charger == 0) or (chargerequired==1) and (robot.is_on_charger == 0):
    #back off from whatever we were doing
    chargerequired=1
    if freeplay==1:
    @@ -389,7 +395,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    time.sleep( 3 )

    cozmo.robot.Robot.drive_off_charger_on_connect = False
    cozmo.run_program(cozmo_unleashed, use_viewer=True)
    #cozmo.run_program(cozmo_unleashed, use_viewer=True)
    # if you have freeglut in the same folder as this script you can change the above line to
    #cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    # which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus
  4. @acidzebra acidzebra revised this gist Oct 9, 2017. 1 changed file with 42 additions and 65 deletions.
    107 changes: 42 additions & 65 deletions cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -31,21 +31,23 @@
    from cozmo.util import degrees, distance_mm, speed_mmps, Pose
    from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes
    global freeplay
    global chargedip
    global chargerequired
    freeplay=0
    chargedip=0
    chargerequired=0
    # main program and loops
    def cozmo_unleashed(robot: cozmo.robot.Robot):
    global freeplay
    global chargedip
    global chargerequired
    robot.world.disconnect_from_cubes()
    robot.enable_all_reaction_triggers(False)
    robot.enable_stop_on_cliff(True)
    robot.set_robot_volume(0.04)
    os.system('cls' if os.name == 'nt' else 'clear')
    while True:
    #State 1: on charger, charging
    #State 1: on charger, charging - wait
    if (robot.is_on_charger == 1) and (robot.is_charging == 1):
    #reset charge required (I know we're still charging, trust me)
    chargerequired=0
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: charging, battery %s" % str(round(robot.battery_voltage, 2)))
    i = random.randint(1, 100)
    @@ -61,8 +63,10 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.set_all_backpack_lights(cozmo.lights.green_light)
    time.sleep(5)
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    #State 2: on charger, fully charged
    #State 2: on charger, fully charged - check schedule for go/no go
    if (robot.is_on_charger == 1) and (robot.is_charging == 0):
    #reset charge required
    chargerequired=0
    # day and time check - are we okay to play at this time and day?
    day_of_week = datetime.date.today().weekday() # 0 is Monday, 6 is Sunday
    ctime = datetime.datetime.now().time()
    @@ -123,26 +127,26 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    break
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    x += 1
    #State 3: not on charger, good battery
    if (robot.battery_voltage > 3.6) and (robot.is_on_charger == 0):
    #State 3: not on charger, good battery - freeplay
    if (robot.battery_voltage > 3.7) and (robot.is_on_charger == 0):
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: freeplay, battery %s" % str(round(robot.battery_voltage, 2)))
    #were we already looking for the charger?
    if chargerequired==1:
    print ("in freeplay loop but charge is required.")
    break
    #initiate freeplay
    if freeplay==0:
    print("freeplay active")
    robot.world.connect_to_cubes()
    robot.enable_all_reaction_triggers(True)
    robot.start_freeplay_behaviors()
    freeplay=1
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: playtime, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep(5)
    #State 4: not on charger, low battery
    if (robot.battery_voltage <= 3.6) and (robot.is_on_charger == 0):
    #TODO: need some code here to smooth out the raw battery data which can bounce around a given value
    #if chargedip < 3:
    # chargedip +=1
    # break
    #
    #
    #State 4: not on charger, low battery - seek charger
    if (robot.battery_voltage <= 3.7) and (robot.is_on_charger == 0):
    #back off from whatever we were doing
    chargerequired=1
    if freeplay==1:
    robot.abort_all_actions(log_abort_messages=False)
    robot.wait_for_all_actions_completed()
    @@ -184,27 +188,21 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
    # randomly drive around for a bit and see if we can spot the lightcubes or charger
    #TODO: better search AI and beter code
    loops=1
    loops=2
    while loops>0:
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    turns=1
    while turns < 10:
    robot.turn_in_place(degrees(36)).wait_for_completed()
    i = random.randint(1, 100)
    for _ in range(11):
    robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    if i >= 95:
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep( 1 )
    turns+=1
    time.sleep(0.3)
    robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    @@ -225,21 +223,14 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    x= random.randrange(-100, 101, 100)
    y= random.randrange(-100, 101, 100)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    robot.turn_in_place(degrees(36)).wait_for_completed()
    for _ in range(11):
    robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    i = random.randint(1, 100)
    if i >= 95:
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep( 1 )
    turns+=1
    time.sleep(0.3)
    robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    @@ -260,21 +251,14 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    x= random.randrange(-100, 101, 100)
    y= random.randrange(-100, 101, 100)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    robot.turn_in_place(degrees(36)).wait_for_completed()
    for _ in range(11):
    robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    i = random.randint(1, 100)
    if i >= 95:
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep( 1 )
    turns+=1
    time.sleep(0.3)
    robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    @@ -295,21 +279,14 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    x= random.randrange(-100, 101, 100)
    y= random.randrange(-100, 101, 100)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    robot.turn_in_place(degrees(36)).wait_for_completed()
    i = random.randint(1, 100)
    if i >= 95:
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    for _ in range(11):
    robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep(1)
    turns+=1
    time.sleep(0.3)
    robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    @@ -360,7 +337,6 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    # check if we're now docked
    if robot.is_on_charger:
    # Yes! we're docked!
    chargedip=0
    robot.play_anim("anim_sparking_success_02").wait_for_completed()
    robot.set_head_angle(degrees(0)).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    @@ -397,22 +373,23 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    while x<20:
    if not robot.world.charger:
    time.sleep( 5 )
    print("State: charger not found, falling back to freeplay for a bit, battery %s" % str(round(robot.battery_voltage, 2)))
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: charger not found, falling back to freeplay for a bit, loop %d of 20." % x)
    x+=1
    #after 100 seconds or spotting the charger end freeplay
    #after ~100 seconds or spotting the charger end freeplay
    robot.enable_all_reaction_triggers(False)
    robot.stop_freeplay_behaviors()
    freeplay=0
    print("total charger loop complete.")
    print("charger loop completed.")
    time.sleep(1)
    print("state cycle complete, looping")
    print("state cycle complete, looping.")
    time.sleep(1)
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: program loop complete (we should not ever reach this state), battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep( 3 )

    cozmo.robot.Robot.drive_off_charger_on_connect = False
    #cozmo.run_program(cozmo_unleashed, use_viewer=True)
    cozmo.run_program(cozmo_unleashed, use_viewer=True)
    # if you have freeglut in the same folder as this script you can change the above line to
    cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    #cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    # which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus
  5. @acidzebra acidzebra revised this gist Oct 9, 2017. 1 changed file with 2 additions and 2 deletions.
    4 changes: 2 additions & 2 deletions cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -91,12 +91,12 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.move_lift(-3)
    robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
    robot.drive_straight(distance_mm(100), speed_mmps(90)).wait_for_completed()
    # we're out of schedule or didn't make the dice roll, snore occasionally for the next 5 minutes and check if we're still in the charger
    # we're out of schedule or didn't make the dice roll, back off for ~5 minutes and check again.
    x = 1
    while x < 20 and (robot.is_on_charger == 1):
    os.system('cls' if os.name == 'nt' else 'clear')
    if playokay == 1:
    print("State: charged, scheduled OK but not active, sleep loop %d of 30 before next check." % (x))
    print("State: charged, schedule OK but not active, sleep loop %d of 30 before next check." % (x))
    x==20
    else:
    print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
  6. @acidzebra acidzebra revised this gist Oct 9, 2017. 1 changed file with 24 additions and 24 deletions.
    48 changes: 24 additions & 24 deletions cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -73,7 +73,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    playokay=1
    #it's a weekday! Check for allowed times.
    else:
    if (ctime > datetime.time(19) and ctime < datetime.time(22)):
    if (ctime > datetime.time(7,30) and ctime < datetime.time(22)):
    playokay=1
    # if the schedule says roll dice to see if we wake up (20 in 100 chance)
    if playokay==1:
    @@ -100,28 +100,28 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    x==20
    else:
    print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
    i = random.randint(1, 100)
    if i == 100:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    elif i >= 90:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    else:
    time.sleep(5)
    #robot.play_anim("anim_gotosleep_off_01").wait_for_completed()
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    i = random.randint(1, 100)
    if i == 100:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    elif i >= 90:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    else:
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    #robot.play_anim("anim_gotosleep_off_01").wait_for_completed()
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    x += 1
    #State 3: not on charger, good battery
    if (robot.battery_voltage > 3.6) and (robot.is_on_charger == 0):
    @@ -412,7 +412,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    time.sleep( 3 )

    cozmo.robot.Robot.drive_off_charger_on_connect = False
    cozmo.run_program(cozmo_unleashed, use_viewer=True)
    #cozmo.run_program(cozmo_unleashed, use_viewer=True)
    # if you have freeglut in the same folder as this script you can change the above line to
    #cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    # which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus
  7. @acidzebra acidzebra revised this gist Oct 9, 2017. 1 changed file with 14 additions and 14 deletions.
    28 changes: 14 additions & 14 deletions cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -93,11 +93,11 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.drive_straight(distance_mm(100), speed_mmps(90)).wait_for_completed()
    # we're out of schedule or didn't make the dice roll, snore occasionally for the next 5 minutes and check if we're still in the charger
    x = 1
    while x < 25 and (robot.is_on_charger == 1):
    while x < 20 and (robot.is_on_charger == 1):
    os.system('cls' if os.name == 'nt' else 'clear')
    if playokay == 1:
    print("State: charged, scheduled OK but not active, sleep loop %d of 30 before next check." % (x))
    x==25
    x==20
    else:
    print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
    i = random.randint(1, 100)
    @@ -221,9 +221,9 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    print("going to cube1!")
    robot.go_to_object(cube1, distance_mm(80)).wait_for_completed()
    robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
    print("going to random point +/-150 in X+Y")
    x= random.randrange(-150, 151, 300)
    y= random.randrange(-150, 151, 300)
    print("going to random point +/-100 in X+Y")
    x= random.randrange(-100, 101, 100)
    y= random.randrange(-100, 101, 100)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    @@ -256,9 +256,9 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    print("going to cube2!")
    robot.go_to_object(cube2, distance_mm(80)).wait_for_completed()
    robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
    print("going to random point +/-150 in X+Y")
    x= random.randrange(-150, 151, 300)
    y= random.randrange(-150, 151, 300)
    print("going to random point +/-100 in X+Y")
    x= random.randrange(-100, 101, 100)
    y= random.randrange(-100, 101, 100)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    @@ -291,9 +291,9 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    print("going to cube3!")
    robot.go_to_object(cube3, distance_mm(80)).wait_for_completed()
    robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
    print("going to random point +/-150 in X+Y")
    x= random.randrange(-150, 151, 300)
    y= random.randrange(-150, 151, 300)
    print("going to random point +/-100 in X+Y")
    x= random.randrange(-100, 101, 100)
    y= random.randrange(-100, 101, 100)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    @@ -333,7 +333,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.set_head_angle(degrees(0)).wait_for_completed()
    action = robot.go_to_pose(charger.pose)
    action.wait_for_completed()
    robot.drive_straight(distance_mm(-120), speed_mmps(50)).wait_for_completed()
    robot.drive_straight(distance_mm(-85), speed_mmps(50)).wait_for_completed()
    action = robot.go_to_pose(charger.pose)
    action.wait_for_completed()
    robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
    @@ -412,7 +412,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    time.sleep( 3 )

    cozmo.robot.Robot.drive_off_charger_on_connect = False
    #cozmo.run_program(cozmo_unleashed, use_viewer=True)
    cozmo.run_program(cozmo_unleashed, use_viewer=True)
    # if you have freeglut in the same folder as this script you can change the above line to
    cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    #cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    # which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus
  8. @acidzebra acidzebra revised this gist Oct 8, 2017. 1 changed file with 23 additions and 22 deletions.
    45 changes: 23 additions & 22 deletions cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -93,34 +93,35 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.drive_straight(distance_mm(100), speed_mmps(90)).wait_for_completed()
    # we're out of schedule or didn't make the dice roll, snore occasionally for the next 5 minutes and check if we're still in the charger
    x = 1
    while x < 30 and (robot.is_on_charger == 1):
    while x < 25 and (robot.is_on_charger == 1):
    os.system('cls' if os.name == 'nt' else 'clear')
    if playokay == 1:
    print("State: charged, scheduled OK but not active, sleep loop %d of 30 before next check." % (x))
    x==25
    else:
    print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
    i = random.randint(1, 100)
    if i == 100:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    elif i >= 90:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    else:
    i = random.randint(1, 100)
    if i == 100:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    elif i >= 90:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    else:
    time.sleep(5)
    #robot.play_anim("anim_gotosleep_off_01").wait_for_completed()
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    time.sleep(5)
    #robot.play_anim("anim_gotosleep_off_01").wait_for_completed()
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    x += 1
    #State 3: not on charger, good battery
    if (robot.battery_voltage > 3.6) and (robot.is_on_charger == 0):
  9. @acidzebra acidzebra revised this gist Oct 8, 2017. 1 changed file with 20 additions and 9 deletions.
    29 changes: 20 additions & 9 deletions cozmo_unleashed-devel.py
    View file
    Original file line number Diff line number Diff line change
    @@ -49,13 +49,16 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: charging, battery %s" % str(round(robot.battery_voltage, 2)))
    i = random.randint(1, 100)
    if i >= 99:
    if i == 100:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    elif i >= 85:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    else:
    time.sleep(5)
    time.sleep(5)
    #TODO: figure out how to smoothly cycle lights
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    robot.set_all_backpack_lights(cozmo.lights.green_light)
    time.sleep(5)
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    #State 2: on charger, fully charged
    @@ -80,7 +83,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: leaving charger, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    robot.play_anim("anim_launch_altwakeup_01").wait_for_completed()
    robot.play_anim("anim_gotosleep_getout_02").wait_for_completed()
    robot.drive_off_charger_contacts().wait_for_completed()
    robot.drive_off_charger_contacts().wait_for_completed()
    robot.drive_off_charger_contacts().wait_for_completed()
    @@ -97,27 +100,36 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    else:
    print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
    i = random.randint(1, 100)
    if i >= 99:
    if i == 100:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    elif i >= 85:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    elif i >= 90:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    else:
    time.sleep(5)
    #robot.play_anim("anim_gotosleep_off_01").wait_for_completed()
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.green_light)
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    time.sleep(5)
    if (robot.is_on_charger == 0):
    print("we were taken off charger")
    break
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    x += 1
    #State 3: not on charger, good battery
    if (robot.battery_voltage > 3.6) and (robot.is_on_charger == 0):
    if freeplay==0:
    print("freeplay active")
    robot.world.connect_to_cubes()
    robot.enable_all_reaction_triggers(True)
    robot.start_freeplay_behaviors()
    freeplay=1
    print("freeplay active")
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: playtime, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep(5)
    @@ -138,7 +150,6 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.world.disconnect_from_cubes()
    freeplay=0
    robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed()
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    robot.set_head_angle(degrees(0)).wait_for_completed()
    robot.move_lift(-3)
    robot.drive_straight(distance_mm(-30), speed_mmps(90)).wait_for_completed()
    @@ -170,7 +181,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.move_lift(-3)
    robot.set_head_angle(degrees(0)).wait_for_completed()
    robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
    # randomly drive around for a bit and see if we can spot the charger
    # randomly drive around for a bit and see if we can spot the lightcubes or charger
    #TODO: better search AI and beter code
    loops=1
    while loops>0:
  10. @acidzebra acidzebra renamed this gist Oct 8, 2017. 1 changed file with 0 additions and 0 deletions.
    0 cozmo_unleashed_dev.py → cozmo_unleashed-devel.py
    View file
    File renamed without changes.
  11. @acidzebra acidzebra renamed this gist Oct 8, 2017. 1 changed file with 0 additions and 0 deletions.
    0 gistfile1.txt → cozmo_unleashed_dev.py
    View file
    File renamed without changes.
  12. @acidzebra acidzebra revised this gist Oct 8, 2017. 1 changed file with 2 additions and 2 deletions.
    4 changes: 2 additions & 2 deletions gistfile1.txt
    View file
    Original file line number Diff line number Diff line change
    @@ -111,7 +111,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    x += 1
    #State 3: not on charger, good battery
    if (robot.battery_voltage > 6) and (robot.is_on_charger == 0):
    if (robot.battery_voltage > 3.6) and (robot.is_on_charger == 0):
    if freeplay==0:
    robot.world.connect_to_cubes()
    robot.enable_all_reaction_triggers(True)
    @@ -122,7 +122,7 @@ def cozmo_unleashed(robot: cozmo.robot.Robot):
    print("State: playtime, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep(5)
    #State 4: not on charger, low battery
    if (robot.battery_voltage <= 6) and (robot.is_on_charger == 0):
    if (robot.battery_voltage <= 3.6) and (robot.is_on_charger == 0):
    #TODO: need some code here to smooth out the raw battery data which can bounce around a given value
    #if chargedip < 3:
    # chargedip +=1
  13. @acidzebra acidzebra created this gist Oct 8, 2017.
    406 changes: 406 additions & 0 deletions gistfile1.txt
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,406 @@
    #!/usr/bin/env python3

    # based on Copyright (c) 2016 Anki, Inc.
    #
    # Licensed under the Apache License, Version 2.0 (the "License");
    # you may not use this file except in compliance with the License.
    # You may obtain a copy of the License in the file LICENSE.txt or at
    #
    # http://www.apache.org/licenses/LICENSE-2.0
    #
    # Unless required by applicable law or agreed to in writing, software
    # distributed under the License is distributed on an "AS IS" BASIS,
    # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    # See the License for the specific language governing permissions and
    # limitations under the License.
    #
    #
    # cozmo_unleashed
    #
    # Script based on the Cozmo SDK drive_to_charger example script
    # and raproenca's modifications of same.
    # rewritten to support battery/charge states and all kinds of bells and whistles by acid zebra
    #
    import sys
    import os
    import datetime
    import random
    import time
    import asyncio
    import cozmo
    from cozmo.util import degrees, distance_mm, speed_mmps, Pose
    from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes
    global freeplay
    global chargedip
    freeplay=0
    chargedip=0
    # main program and loops
    def cozmo_unleashed(robot: cozmo.robot.Robot):
    global freeplay
    global chargedip
    robot.world.disconnect_from_cubes()
    robot.enable_all_reaction_triggers(False)
    robot.enable_stop_on_cliff(True)
    robot.set_robot_volume(0.04)
    os.system('cls' if os.name == 'nt' else 'clear')
    while True:
    #State 1: on charger, charging
    if (robot.is_on_charger == 1) and (robot.is_charging == 1):
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: charging, battery %s" % str(round(robot.battery_voltage, 2)))
    i = random.randint(1, 100)
    if i >= 99:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    elif i >= 85:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    time.sleep(5)
    #TODO: figure out how to smoothly cycle lights
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    time.sleep(5)
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    #State 2: on charger, fully charged
    if (robot.is_on_charger == 1) and (robot.is_charging == 0):
    # day and time check - are we okay to play at this time and day?
    day_of_week = datetime.date.today().weekday() # 0 is Monday, 6 is Sunday
    ctime = datetime.datetime.now().time()
    playokay=0
    #it's weekend! Check for allowed times.
    if day_of_week > 4:
    if (ctime > datetime.time(9) and ctime < datetime.time(23)):
    playokay=1
    #it's a weekday! Check for allowed times.
    else:
    if (ctime > datetime.time(19) and ctime < datetime.time(22)):
    playokay=1
    # if the schedule says roll dice to see if we wake up (20 in 100 chance)
    if playokay==1:
    i = random.randint(1, 100)
    # 20 in 100 chance of waking up every 5 minutes
    if i >= 80:
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: leaving charger, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    robot.play_anim("anim_launch_altwakeup_01").wait_for_completed()
    robot.drive_off_charger_contacts().wait_for_completed()
    robot.drive_off_charger_contacts().wait_for_completed()
    robot.drive_off_charger_contacts().wait_for_completed()
    time.sleep(2)
    robot.move_lift(-3)
    robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
    robot.drive_straight(distance_mm(100), speed_mmps(90)).wait_for_completed()
    # we're out of schedule or didn't make the dice roll, snore occasionally for the next 5 minutes and check if we're still in the charger
    x = 1
    while x < 30 and (robot.is_on_charger == 1):
    os.system('cls' if os.name == 'nt' else 'clear')
    if playokay == 1:
    print("State: charged, scheduled OK but not active, sleep loop %d of 30 before next check." % (x))
    else:
    print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
    i = random.randint(1, 100)
    if i >= 99:
    robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
    elif i >= 85:
    robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
    time.sleep(5)
    if (robot.is_on_charger == 0):
    break
    robot.set_all_backpack_lights(cozmo.lights.green_light)
    time.sleep(5)
    if (robot.is_on_charger == 0):
    break
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    x += 1
    #State 3: not on charger, good battery
    if (robot.battery_voltage > 6) and (robot.is_on_charger == 0):
    if freeplay==0:
    robot.world.connect_to_cubes()
    robot.enable_all_reaction_triggers(True)
    robot.start_freeplay_behaviors()
    freeplay=1
    print("freeplay active")
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: playtime, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep(5)
    #State 4: not on charger, low battery
    if (robot.battery_voltage <= 6) and (robot.is_on_charger == 0):
    #TODO: need some code here to smooth out the raw battery data which can bounce around a given value
    #if chargedip < 3:
    # chargedip +=1
    # break
    #
    #
    #back off from whatever we were doing
    if freeplay==1:
    robot.abort_all_actions(log_abort_messages=False)
    robot.wait_for_all_actions_completed()
    robot.stop_freeplay_behaviors()
    robot.enable_all_reaction_triggers(False)
    robot.world.disconnect_from_cubes()
    freeplay=0
    robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed()
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    robot.set_head_angle(degrees(0)).wait_for_completed()
    robot.move_lift(-3)
    robot.drive_straight(distance_mm(-30), speed_mmps(90)).wait_for_completed()
    robot.set_all_backpack_lights(cozmo.lights.blue_light)
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: finding charger, battery %s" % str(round(robot.battery_voltage, 2)))
    # charger location search
    charger = None
    # see if we already know where the charger is
    if robot.world.charger:
    if robot.world.charger.pose.origin_id == robot.pose.origin_id:
    #we know where the charger is
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: charger position known, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
    robot.move_lift(-3)
    robot.set_head_angle(degrees(0)).wait_for_completed()
    charger = robot.world.charger
    else:
    # we know where the charger is but we have been delocalized
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: robot delocalized recently, battery %s" % str(round(robot.battery_voltage, 2)))
    pass
    # we don't know where the charger is
    if not charger:
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.play_anim_trigger(cozmo.anim.Triggers.SparkIdle, ignore_body_track=True).wait_for_completed()
    robot.move_lift(-3)
    robot.set_head_angle(degrees(0)).wait_for_completed()
    robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
    # randomly drive around for a bit and see if we can spot the charger
    #TODO: better search AI and beter code
    loops=1
    while loops>0:
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    turns=1
    while turns < 10:
    robot.turn_in_place(degrees(36)).wait_for_completed()
    i = random.randint(1, 100)
    if robot.world.charger:
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    if i >= 95:
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep( 1 )
    turns+=1
    robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    cube1 = robot.world.get_light_cube(cozmo.objects.LightCube1Id)
    if cube1:
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    print("going to cube1!")
    robot.go_to_object(cube1, distance_mm(80)).wait_for_completed()
    robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
    print("going to random point +/-150 in X+Y")
    x= random.randrange(-150, 151, 300)
    y= random.randrange(-150, 151, 300)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    robot.turn_in_place(degrees(36)).wait_for_completed()
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    i = random.randint(1, 100)
    if i >= 95:
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep( 1 )
    turns+=1
    robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    cube2 = robot.world.get_light_cube(cozmo.objects.LightCube2Id)
    if cube2:
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    print("going to cube2!")
    robot.go_to_object(cube2, distance_mm(80)).wait_for_completed()
    robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
    print("going to random point +/-150 in X+Y")
    x= random.randrange(-150, 151, 300)
    y= random.randrange(-150, 151, 300)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    robot.turn_in_place(degrees(36)).wait_for_completed()
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    i = random.randint(1, 100)
    if i >= 95:
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep( 1 )
    turns+=1
    robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    cube3 = robot.world.get_light_cube(cozmo.objects.LightCube3Id)
    if cube3:
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    print("going to cube3!")
    robot.go_to_object(cube3, distance_mm(80)).wait_for_completed()
    robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
    print("going to random point +/-150 in X+Y")
    x= random.randrange(-150, 151, 300)
    y= random.randrange(-150, 151, 300)
    robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
    turns=1
    while turns < 10:
    robot.turn_in_place(degrees(36)).wait_for_completed()
    i = random.randint(1, 100)
    if i >= 95:
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep(1)
    turns+=1
    robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    if robot.world.charger:
    loops=0
    charger = robot.world.charger
    print("breaking charger loop as charger is known")
    break
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    loops=loops-1
    print("locator loop complete.")
    time.sleep(1)
    # Charger location and docking handling here
    #TODO: improve this spaghetti code
    if charger:
    while (robot.is_on_charger == 0):
    robot.set_lift_height(0.8,0.8,0.8,0.1).wait_for_completed()
    # drive near to the charger, and then stop.
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: moving to charger, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabChatty, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
    robot.move_lift(-3)
    robot.set_head_angle(degrees(0)).wait_for_completed()
    action = robot.go_to_pose(charger.pose)
    action.wait_for_completed()
    robot.drive_straight(distance_mm(-120), speed_mmps(50)).wait_for_completed()
    action = robot.go_to_pose(charger.pose)
    action.wait_for_completed()
    robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
    # we should be right in front of the charger, can we see it?
    if (charger.is_visible==False):
    #No we can't see it. Remove charger from navigation map and quit this loop.
    robot.world.charger = None
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    print("State: charger not found, clearing map. battery %s" % str(round(robot.battery_voltage, 2)))
    break
    i = random.randint(1, 100)
    if i >= 85:
    robot.play_anim_trigger(cozmo.anim.Triggers.FeedingReactToShake_Normal, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    # Dock. Turn around and drive backwards
    print("State: docking, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.turn_in_place(degrees(95)).wait_for_completed()
    robot.turn_in_place(degrees(95)).wait_for_completed()
    time.sleep( 1 )
    robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake, ignore_body_track=True).wait_for_completed()
    robot.set_head_angle(degrees(0)).wait_for_completed()
    robot.drive_straight(distance_mm(-145), speed_mmps(150)).wait_for_completed()
    time.sleep( 1 )
    # check if we're now docked
    if robot.is_on_charger:
    # Yes! we're docked!
    chargedip=0
    robot.play_anim("anim_sparking_success_02").wait_for_completed()
    robot.set_head_angle(degrees(0)).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: docked, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.set_all_backpack_lights(cozmo.lights.off_light)
    robot.play_anim("anim_gotosleep_getin_01").wait_for_completed()
    robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed()
    # No, we missed. Back off and try again
    else:
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: failed to dock with charger, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.world.charger = None
    robot.play_anim_trigger(cozmo.anim.Triggers.AskToBeRightedRight, ignore_body_track=True).wait_for_completed()
    robot.move_lift(-3)
    robot.set_head_angle(degrees(0)).wait_for_completed()
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: backing off to attempt docking, battery %s" % str(round(robot.battery_voltage, 2)))
    robot.drive_straight(distance_mm(50), speed_mmps(90)).wait_for_completed()
    robot.turn_in_place(degrees(-3)).wait_for_completed()
    robot.drive_straight(distance_mm(150), speed_mmps(90)).wait_for_completed()
    robot.turn_in_place(degrees(95)).wait_for_completed()
    robot.turn_in_place(degrees(96)).wait_for_completed()
    robot.set_head_angle(degrees(0)).wait_for_completed()
    time.sleep( 1 )
    else:
    # we have not managed to find the charger. Falling back to freeplay with occasional checks
    #robot.world.charger = None
    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
    #robot.world.connect_to_cubes()
    robot.enable_all_reaction_triggers(True)
    robot.start_freeplay_behaviors()
    freeplay=1
    x=0
    while x<20:
    if not robot.world.charger:
    time.sleep( 5 )
    print("State: charger not found, falling back to freeplay for a bit, battery %s" % str(round(robot.battery_voltage, 2)))
    x+=1
    #after 100 seconds or spotting the charger end freeplay
    robot.enable_all_reaction_triggers(False)
    robot.stop_freeplay_behaviors()
    freeplay=0
    print("total charger loop complete.")
    time.sleep(1)
    print("state cycle complete, looping")
    time.sleep(1)
    os.system('cls' if os.name == 'nt' else 'clear')
    print("State: program loop complete (we should not ever reach this state), battery %s" % str(round(robot.battery_voltage, 2)))
    time.sleep( 3 )

    cozmo.robot.Robot.drive_off_charger_on_connect = False
    #cozmo.run_program(cozmo_unleashed, use_viewer=True)
    # if you have freeglut in the same folder as this script you can change the above line to
    cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
    # which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus