-
-
Save VictorTagayun/219aa7dab61af6efef0214437e5de55a to your computer and use it in GitHub Desktop.
Attempting to create a perpetuum cozmobile
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/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 os | |
| import datetime | |
| import random | |
| import time | |
| import asyncio | |
| import cozmo | |
| from cozmo.util import degrees, distance_mm, speed_mmps | |
| from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes | |
| global freeplay | |
| freeplay=0 | |
| # main program and loops | |
| def cozmo_unleashed(robot: cozmo.robot.Robot): | |
| global freeplay | |
| 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 >= 97: | |
| 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(3) | |
| robot.set_all_backpack_lights(cozmo.lights.green_light) | |
| time.sleep(3) | |
| 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): | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charged, 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(50)).wait_for_completed() | |
| time.sleep(2) | |
| #State 3: not on charger, good battery | |
| if (robot.battery_voltage > 3.65) 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 | |
| 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.65) and (robot.is_on_charger == 0): | |
| 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(50)).wait_for_completed() | |
| 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.set_head_angle(degrees(0)).wait_for_completed() | |
| charger = robot.world.charger | |
| print (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: | |
| # we will look around in place for the charger for 30 seconds | |
| 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.set_head_angle(degrees(0)).wait_for_completed() | |
| look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) | |
| try: | |
| charger = robot.world.wait_for_observed_charger(timeout=30) | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: found charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| except asyncio.TimeoutError: | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: unable to find charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabUnhappy, ignore_body_track=True).wait_for_completed() | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| finally: | |
| # stop the behavior | |
| look_around.stop() | |
| # Charger location and docking handling here | |
| if charger: | |
| while (robot.is_on_charger == 0): | |
| robot.set_lift_height(0.8,0.8,0.8,0.1).wait_for_completed() | |
| # Yes! Attempt to 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(-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! | |
| 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(-2)).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. | |
| 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() | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charger not found, falling back to 90 secs freeplay, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| freeplay=1 | |
| time.sleep( 90 ) | |
| #after 90 seconds end freeplay | |
| robot.enable_all_reaction_triggers(False) | |
| robot.stop_freeplay_behaviors() | |
| freeplay=0 | |
| robot.world.disconnect_from_cubes() | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: program loop complete, battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep( 3 ) | |
| # scheduler | |
| #def scheduler: | |
| # day_of_week = datetime.date.today().weekday() # 0 is Monday, 6 is Sunday | |
| # time = datetime.datetime.now().time() | |
| # #weekend | |
| # if day_of_week >= 5 and (time > datetime.time(7,30) and time < datetime.time(23)): | |
| # do_something() | |
| #weekday mornings | |
| # if day_of_week < 5 and (time > datetime.time(7,30) and time < datetime.time(8)): | |
| # do_something() | |
| #weekday evenings | |
| # if day_of_week < 5 and (time > datetime.time(7,30) and time < datetime.time(8)): | |
| # do_something() | |
| 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 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment