-
-
Save Banteel/ad9f52ce5645784bd3dc094353ccee36 to your computer and use it in GitHub Desktop.
SIMPLE_DOCK: Creating a streamlined tactic to efficiently get Cozmo on the dock, No Printout Markers required, just plugin the charger and go, Simple_Dock is Python 3.6 safe for Cozmo robot
This file contains hidden or 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. | |
| # | |
| # | |
| # simple_dock | |
| # This script can use Walls although does not require walls | |
| # This script has FreePlay Lights options | |
| # These options can be turned on & off as needed | |
| # This script does not require any of these options to be turned on, all you need is a Charger | |
| # forked script from: cozmo_unleashed | |
| # | |
| # Original perpetual Cozmo_Unleashed script by AcidZebra | |
| # This Script is made by VictorTagayun and is heavily slimmed down to only docking | |
| # Script based on the Cozmo SDK drive_to_charger example script | |
| # and raproenca's modifications of same. | |
| # | |
| # Additional modifications by Banteel to streamline the two 90 degree turns in one simple | |
| # command that is linked to position.angle_z of object "charger" which contains all the info. | |
| # rewritten to support battery/charge states and all kinds of bells and whistles by AcidZebra | |
| # | |
| # The default Low Battery Level for Cozmo is 3.65. For testing pursposes I have set mine | |
| # to 3.88 so Cozmo gets on the dock more often. Please feel free to set both lines to 3.65 | |
| # | |
| # Last Update: June/20/2024 | |
| # | |
| #import required functions | |
| from socket import INADDR_ALLHOSTS_GROUP | |
| import sys, os, datetime, random, time, math, re, threading | |
| ##import logging | |
| import asyncio, cozmo, cozmo.objects, cozmo.util | |
| from cozmo.util import Angle, degrees, distance_mm, speed_mmps | |
| from cozmo.objects import CustomObjectMarkers, CustomObjectTypes, LightCube | |
| from cozmo.robot_alignment import RobotAlignmentTypes | |
| from datetime import datetime as Datetime | |
| from cozmo import connect_on_loop | |
| global robot, freeplay, lowbatvolt, use_cubes, use_walls, wfaac | |
| global use_scheduler, main_loop_count, scheduled_days, scheduled_time | |
| robot = cozmo.robot.Robot | |
| freeplay=0 | |
| main_loop_count=0 | |
| scheduled_days="None" | |
| scheduled_time=0 | |
| wfaac = robot.wait_for_all_actions_completed(cozmo.robot.Robot) | |
| # | |
| #============================ | |
| # CONFIGURABLE VARIABLES HERE | |
| #============================ | |
| # | |
| # BATTERY VOLTAGE THRESHOLDS | |
| # | |
| # lowbatvolt - when voltage drops below this Cozmo will start looking for his charger | |
| # highbatvolt - when cozmo comes off your charger fully charge, this value will self-calibrate | |
| # maxbatvolt - the maximum battery level as recorded when cozmo is on charger but no longer charging | |
| # tweak lowbatvolt to suit your cozmo. default value is 3.65 | |
| lowbatvolt = 3.65 | |
| #highbatvolt= 4.04 - currently not used | |
| #maxbatvolt = 4.54 - currently not used | |
| # | |
| # COZMO'S VOICE VOLUME | |
| # what volume Cozmo should play sounds at, value between 0 and 1 | |
| robotvolume = 0.05 | |
| # | |
| # CUBE USAGE | |
| # | |
| # whether or not to activate the cubes (saves battery if you don't) | |
| # I almost always leave this off, he will still stack them and mess around with them | |
| # some games like "block guard dog" will not come up unless the blocks are active | |
| # | |
| # value of 0: cubes off & cube's lights off | |
| # value of 1: cubes on & enables all games | |
| # value of 2: cubes on & cube's freeplay lights on & in-schedule breathing lights off | |
| # value of 3: cubes on & cube's freeplay lights on & in-schedule breathing lights on | |
| # value between 0 and 3 | |
| use_cubes = 0 | |
| # | |
| # MARKER WALLS USAGE | |
| # | |
| # whether or not you are using printed out markers for walls on your play field | |
| # if you are using walls, you will likely need to configure wall sizes to your play area | |
| # values between 0 and 1 | |
| use_walls = 0 | |
| # | |
| # SCHEDULER USAGE | |
| # | |
| # whether or not to use the schedule to define allowed "play times" | |
| # this code is a bit rough, use at your own risk | |
| # value between 0 and 1 | |
| use_scheduler = 1 | |
| # | |
| # SCHEDULE START & STOP SETTING IN 24HR TIME FORMAT ex. 0 to 23 | |
| # If you are new at learning 24HR time format, try this: | |
| # If the Time is past 12PM, add 12 to it. | |
| # For example; if Time is 4PM, then do 4 + 12 = 16 Hours. | |
| # Or for example; if Time is 11PM, do 11 + 12 = 23 Hours. | |
| # 24 Hours/Midnight is always 0 Hours even in script. | |
| # | |
| if use_scheduler == 1: | |
| # Initialize Scheduler Variables | |
| global wkDS, wkDSm, wkDE, wkDEm, wkNS, wkNSm, wkNE, wkNEm, wkEDS, wkEDSm, wkEDE, wkEDEm, wkENS, wkENSm, wkENE, wkENEm, wday, dayornight | |
| nowtime = Datetime.now().timetuple() | |
| dayornight="None" | |
| # Day And Night Time Settings For Time Scheduler Starts Here: | |
| # Week Day Time Slot 1 | |
| wkDS = 17 # Week (D)ay's (S)tarting time hour, default is 7AM | |
| wkDSm = 45 # Week (D)ay's (S)tarting time minute | |
| wkDE = 18 # Week (D)ay's (E)nding time hour, default is 8AM | |
| wkDEm = 45 # Week (D)ay's (E)nding time minute | |
| # Week Day Time Slot 2 | |
| wkNS = 19 # Week (N)ight's (S)tarting time hour, default is 3PM(1500hrs) | |
| wkNSm = 25 # Week (N)ight's (S)tarting time minute | |
| wkNE = 22 # Week (N)ight's (E)nding time hour, default is 11PM(2300hrs) | |
| wkNEm = 55 # Week (N)ight's (E)nding time minute | |
| # Week End Time Slot 1 | |
| wkEDS = 13 # Week(E)nd (D)ay's (S)tarting time hour, default is 9AM | |
| wkEDSm = 0 # Week(E)nd (D)ay's (S)tarting time minute | |
| wkEDE = 20 # Week(E)nd (D)ay's (E)nding time hour, default is 12PM(1200hrs) | |
| wkEDEm = 2 # Week(E)nd (D)ay's (E)nding time minute | |
| # Week End Time Slot 2 | |
| wkENS = 22 # Week(E)nd (N)ight's (S)tarting time hour, default is 3PM(1500hrs) | |
| wkENSm = 30 # Week(E)nd (N)ight's (S)tarting time minute | |
| wkENE = 1 # Week(E)nd (N)ight's (E)nding time hour, default is 11PM(2300hrs) | |
| wkENEm = 58 # Week(E)nd (N)ight's (E)nding time minute | |
| # | |
| #COZMO SCHEDULER DEFAULT TIME SETTINGS: | |
| # | |
| # The Rule for 24hr Time Format is: If it's 1PM or greater than, add 12 to the number | |
| # | |
| ## Week Day Time Slot 1 | |
| #wkDS = 7 # Week (D)ay's (S)tarting time hour, default is 7AM | |
| #wkDSm = 15 # Week (D)ay's (S)tarting time minute | |
| #wkDE = 8 # Week (D)ay's (E)nding time hour, default is 8AM | |
| #wkDEm = 35 # Week (D)ay's (E)nding time minute | |
| # | |
| ## Week Day Time Slot 2 | |
| #wkNS = 15 # Week (N)ight's (S)tarting time hour, default is 3PM | |
| #wkNSm = 35 # Week (N)ight's (S)tarting time minute | |
| #wkNE = 23 # Week (N)ight's (E)nding time hour, default is 11PM | |
| #wkNEm = 15 # Week (N)ight's (E)nding time minute | |
| # | |
| ## Week End Time Slot 1 | |
| #wkEDS = 9 # Week(E)nd (D)ay's (S)tarting time hour, default is 9AM | |
| #wkEDSm = 0 # Week(E)nd (D)ay's (S)tarting time minute | |
| #wkEDE = 12 # Week(E)nd (D)ay's (E)nding time hour, default is 12PM | |
| #wkEDEm = 20 # Week(E)nd (D)ay's (E)nding time minute | |
| # | |
| ## Week End Time Slot 2 | |
| #wkENS = 15 # Week(E)nd (N)ight's (S)tarting time hour, default is 3PM | |
| #wkENSm = 35 # Week(E)nd (N)ight's (S)tarting time minute | |
| #wkENE = 23 # Week(E)nd (N)ight's (E)nding time hour, default is 11PM | |
| #wkENEm = 58 # Week(E)nd (N)ight's (E)nding time minute | |
| # | |
| # BIRTHDAY EVENT USAGE: | |
| # | |
| # Set this bday value to use either set 1 user birthday or 2 user's birthdays | |
| # | |
| # Enter User1 Birthday Event Time Here: | |
| bday_name1 = "User1" # Change "User1" to "owner's name" | |
| bday_year1 = 2000 | |
| bday_month1 = 1 | |
| bday_day1 = 2 | |
| # Enter User2 Birthday Event Time Here: | |
| bday_name2 = "User2" # Change "User2" to "second person's name" | |
| bday_year2 = 1975 | |
| bday_month2 = 6 | |
| bday_day2 = 28 | |
| # Have Cozmo Say How Old You Are On Your Birthday Or Not: say_age = 0-No, 1-Yes | |
| # User1 | |
| say_age1 = 1 | |
| # User2 | |
| say_age2 = 1 | |
| # | |
| # HOLIDAY EVENT USAGE: | |
| # | |
| # Add your Holidays with the Month first then the Day second like this: Hdays.append([Month,Day]) | |
| # You can add dates to the list in any order you want, it doesn't have to be sorted by date | |
| # Remember to scroll down to Cozmo_Resting definition and add your say_text message for your Holiday | |
| # | |
| # Declaring Vars | |
| Hdays = [] | |
| # New Years Days [Month,Day] | |
| Hdays = [[12,31], [1,1]] | |
| # Holloween Holiday | |
| Hdays.append([10,31]) | |
| # Movember Days [Month,Day] | |
| Hdays.extend([[11,10], [11,11], [11,12], [11,13], [11,14], [11,15], [11,16], [11,17], [11,18], [11,19], [11,20]]) | |
| # ^^ "extend" to add a tuple of days for 1 Holiday | |
| # Canada Day | |
| Hdays.append([7,1]) | |
| # Independence Day | |
| Hdays.append([7,4]) | |
| # for (x,y) in Hdays: | |
| # print(x,y) | |
| # | |
| # | |
| # main program and loops | |
| def cozmo_unleashed(robot: robot): | |
| global charger, freeplay, robotvolume, use_walls, use_cubes, use_scheduler, scheduled_days | |
| if use_scheduler == 1: | |
| global nowtime, wday, scheduled_time, scheduled_time, dayornight | |
| global wkDS, wkDSm, wkDE, wkDEm, wkNS, wkNSm, wkNE, wkNEm, wkEDS, wkEDSm, wkEDE, wkEDEm, wkENS, wkENSm, wkENE, wkENEm | |
| if scheduled_days=="None" or use_scheduler == 0: | |
| # | |
| # DEFINING MARKERS | |
| # | |
| # 1st 2 numbers are the width/height of the paper the printed marker is on | |
| # for the 1st dimension: measure the side of the width/height, from the printed marker's edge, then | |
| # multiply it by 2 then add it to the 2nd dimension | |
| # 2nd 2 numbers are the width/height of the printed marker on the paper | |
| # I suggest using a ruler to measure these dimensions, by the millimeter, after you print & cut them to get exact numbers | |
| # these define the 4 walls of my play field, it helps Cozmo not bump into the walls | |
| # you can set the use_walls variable at the top of the script to 0 if you are not using walls | |
| # If you are only using one copy of each printed marker then change is_unique= to True | |
| if use_walls >= 1: | |
| robot.world.define_custom_wall(CustomObjectTypes.CustomType02, CustomObjectMarkers.Diamonds2, 300, 60, 36, 36, is_unique=False) | |
| robot.world.define_custom_wall(CustomObjectTypes.CustomType03, CustomObjectMarkers.Diamonds3, 300, 60, 36, 36, is_unique=False) | |
| robot.world.define_custom_wall(CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 300, 60, 36, 36, is_unique=False) | |
| robot.world.define_custom_wall(CustomObjectTypes.CustomType05, CustomObjectMarkers.Diamonds5, 300, 60, 36, 36, is_unique=False) | |
| robot.world.charger = None | |
| charger = None | |
| if use_scheduler == 1: | |
| nowtime = datetime.datetime.now().timetuple() | |
| scheduled_time = 0 | |
| wfaac | |
| robot.clear_idle_animation() | |
| robot.stop_all_motors() | |
| robot.abort_all_actions(log_abort_messages=False) | |
| robot.enable_all_reaction_triggers(False) | |
| robot.enable_stop_on_cliff(True) | |
| robot.world.auto_disconnect_from_cubes_at_end(True) | |
| robot.set_robot_volume(robotvolume) | |
| time.sleep(1) | |
| if use_cubes >= 2: | |
| robot.enable_freeplay_cube_lights(enable=True) | |
| while True: | |
| #State 1: on charger, charging | |
| if (robot.is_on_charger == 1) and (robot.is_charging == 1): | |
| freeplay=0 | |
| if scheduled_time == 1: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charging, in scheduled playtime, battery %s" % str(round(robot.battery_voltage, 2))) | |
| else: | |
| #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() | |
| wfaac | |
| elif i >= 85: | |
| robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
| wfaac | |
| time.sleep(2) | |
| robot.set_all_backpack_lights(cozmo.lights.green_light) | |
| time.sleep(2) | |
| robot.set_all_backpack_lights(cozmo.lights.off_light) | |
| #State 2: on charger, fully charged | |
| if use_scheduler == 1 and scheduled_days == "None": | |
| robot_check_scheduler(robot) | |
| elif (robot.is_on_charger == 1) and (robot.is_charging == 0) and (scheduled_days != "Resting") and (scheduled_time == 1 or use_scheduler == 0): | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charged, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.set_needs_levels(1, 1, 1) | |
| robot.clear_idle_animation() | |
| robot.set_all_backpack_lights(cozmo.lights.off_light) | |
| robot.play_anim("anim_launch_altwakeup_01").wait_for_completed() | |
| wfaac | |
| time.sleep(2) | |
| robot.drive_off_charger_contacts(num_retries=3, in_parallel=False).wait_for_completed() | |
| robot.move_lift(-3) | |
| try: | |
| if (robot.is_on_charger == 1): | |
| robot.drive_straight(distance_mm(80), speed_mmps(25), num_retries=1).wait_for_completed() | |
| finally: | |
| pass | |
| wfaac | |
| time.sleep(2) | |
| #State 3: not on charger, good time play battery | default low battery is 3.65 | |
| if (robot.battery_voltage > lowbatvolt) and (robot.is_on_charger == 0) and (scheduled_time == 1 or use_scheduler == 0): | |
| if use_scheduler == 1 and scheduled_days == "None": | |
| robot_check_scheduler(robot) | |
| elif freeplay == 0: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: turn on good time play battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.move_lift(-3) | |
| if use_cubes == 1: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: turn on cubes %s" % str(round(robot.battery_voltage, 2))) | |
| robot.world.connect_to_cubes() | |
| time.sleep(4) | |
| elif use_cubes >= 2: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: turn on cubes and freeplay lights %s" % str(round(robot.battery_voltage, 2))) | |
| robot.enable_freeplay_cube_lights(True) | |
| robot.world.connect_to_cubes() | |
| time.sleep(4) | |
| robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| freeplay=1 | |
| if robot.is_picked_up == True: | |
| charger = None | |
| robot.world.charger = None | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: robot picked up during freeplay, resetting everything, battery %s" % str(round(robot.battery_voltage, 2))) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: good battery playtime, battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep(3) | |
| #State 4: not on charger, low battery | default low battery is 3.65 | |
| if (robot.battery_voltage <= lowbatvolt) and (robot.is_on_charger == 0): | |
| if freeplay == 1: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: turn off good battery freeplay, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.enable_all_reaction_triggers(False) | |
| robot.stop_all_motors() | |
| robot.clear_idle_animation() | |
| #robot.abort_all_actions(False) | |
| wfaac | |
| time.sleep(1) | |
| robot.stop_freeplay_behaviors() | |
| if use_cubes == 1 or use_cubes == 3: | |
| robot.world.disconnect_from_cubes() | |
| time.sleep(4) | |
| elif use_cubes == 2: | |
| robot.enable_freeplay_cube_lights(enable=False) | |
| robot.world.disconnect_from_cubes() | |
| time.sleep(4) | |
| freeplay=0 | |
| robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed() | |
| wfaac | |
| time.sleep(2) | |
| robot.set_all_backpack_lights(cozmo.lights.blue_light) | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| robot.move_lift(-3) | |
| wfaac | |
| time.sleep(0.1) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: low battery, finding charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| # charger location search | |
| # see if we already know where the charger is | |
| if charger != None: | |
| #we know where the charger is | |
| robot.move_lift(-3) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: start docking loop, 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() | |
| wfaac | |
| time.sleep(0.01) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print(str(charger)) | |
| if charger != None: | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.Body, num_retries=3) | |
| action.wait_for_completed() | |
| finally: | |
| pass | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=3) | |
| action.wait_for_completed() | |
| finally: | |
| pass | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed() | |
| wfaac | |
| time.sleep(1) | |
| else: | |
| # we know where the charger is but we have been delocalized | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: did not find charger after clearing map, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.set_needs_levels(1, 1, 1) | |
| if use_cubes == 1 or use_cubes == 3: | |
| robot.world.disconnect_from_cubes() | |
| time.sleep(4) | |
| elif use_cubes == 2: | |
| robot.enable_freeplay_cube_lights(enable=False) | |
| robot.world.disconnect_from_cubes() | |
| time.sleep(4) | |
| ## decide if we need to run find charger routines or not | |
| robot_locate_dock_or_not(robot) | |
| ## end of loop routines | |
| if robot.world.charger != None: | |
| charger = robot.world.charger | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: updating charger location, battery %s" % str(round(robot.battery_voltage, 2))) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: "+str(charger)+", battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.world.charger = None | |
| if robot.is_picked_up == True: | |
| charger = None | |
| robot.world.charger = None | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: robot picked up during freeplay, resetting charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| #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 ) | |
| if use_scheduler == 1: | |
| nowtime = datetime.datetime.now().timetuple() | |
| if scheduled_days == "WeekDayTimeSlot-1" and ( | |
| (nowtime.tm_hour < wkDS) or (nowtime.tm_hour == wkDS and nowtime.tm_min < wkDSm) | |
| ) and ( | |
| (nowtime.tm_hour > wkDE) or (nowtime.tm_hour == wkDE and nowtime.tm_min >= wkDEm) | |
| ) or scheduled_days == "WeekDayTimeSlot-1" and ( | |
| wkDE < wkDS | |
| and ( | |
| (nowtime.tm_hour <= wkDS and nowtime.tm_hour >= wkDE) | |
| or (nowtime.tm_hour == wkDE and nowtime.tm_min >= wkDEm) | |
| ) | |
| ): | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('State: Weekday '+str(dayornight)+' time Scheduled Time') | |
| robot_check_scheduler(robot) | |
| elif scheduled_days == "WeekDayTimeSlot-2" and ( | |
| (nowtime.tm_hour < wkNS) or (nowtime.tm_hour == wkNS and nowtime.tm_min < wkNSm) | |
| ) and ( | |
| (nowtime.tm_hour > wkNE) or (nowtime.tm_hour == wkNE and nowtime.tm_min >= wkNEm) | |
| ) or scheduled_days == "WeekDayTimeSlot-2" and ( | |
| wkNE < wkNS | |
| and ( | |
| (nowtime.tm_hour <= wkNS and nowtime.tm_hour >= wkNE) | |
| or (nowtime.tm_hour == wkNE and nowtime.tm_min >= wkNEm) | |
| ) | |
| ): | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('State: Weekday '+str(dayornight)+' time Schedule Time') | |
| robot_check_scheduler(robot) | |
| elif scheduled_days == "WeekEndTimeSlot-1" and ( | |
| (nowtime.tm_hour < wkEDS) or (nowtime.tm_hour == wkEDS and nowtime.tm_min < wkEDSm) | |
| ) and ( | |
| (nowtime.tm_hour > wkEDE) or (nowtime.tm_hour == wkEDE and nowtime.tm_min >= wkEDEm) | |
| ) or scheduled_days == "WeekEndTimeSlot-1" and ( | |
| wkEDE < wkEDS | |
| and ( | |
| (nowtime.tm_hour <= wkEDS and nowtime.tm_hour >= wkEDE) | |
| or (nowtime.tm_hour == wkEDE and nowtime.tm_min >= wkEDEm) | |
| ) | |
| ): | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('State: Weekend '+str(dayornight)+' time Scheduled Time') | |
| robot_check_scheduler(robot) | |
| elif scheduled_days == "WeekEndTimeSlot-2" and ( | |
| (nowtime.tm_hour < wkENS) or (nowtime.tm_hour == wkENS and nowtime.tm_min < wkENSm) | |
| ) and ( | |
| (nowtime.tm_hour > wkENE) or (nowtime.tm_hour == wkENE and nowtime.tm_min >= wkENEm) | |
| ) or scheduled_days == "WeekEndTimeSlot-2" and ( | |
| wkENE < wkENS | |
| and ( | |
| (nowtime.tm_hour <= wkENS and nowtime.tm_hour >= wkENE) | |
| or (nowtime.tm_hour == wkENE and nowtime.tm_min >= wkENEm) | |
| ) | |
| ): | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('State: Weekend '+str(dayornight)+' time Schedule Time') | |
| robot_check_scheduler(robot) | |
| elif scheduled_days == "Resting" and scheduled_time == 0: | |
| if use_cubes == 3: | |
| robot.enable_freeplay_cube_lights(enable=True) | |
| if robot.is_on_charger == 0: | |
| robot.stop_freeplay_behaviors() | |
| time.sleep(3) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('State: Resting Time Schedule, Find Charger') | |
| robot_locate_dock_or_not(robot) | |
| else: | |
| robot_check_scheduler(robot) | |
| else: | |
| cozmo_resting(robot) | |
| # | |
| ## define if we need to find charger or not | |
| # | |
| def robot_locate_dock_or_not(robot: robot): | |
| global charger, freeplay, lowbatvolt, scheduled_time | |
| #back off from whatever we were doing | |
| #robot_set_backpacklights(4278190335) # 4278190335 is red | |
| if robot.is_freeplay_mode_active: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: disabling freeplay, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.stop_freeplay_behaviors() | |
| freeplay=0 | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.abort_all_actions(log_abort_messages=True) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: starting locate dock sequence, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.clear_idle_animation() | |
| #wfaac | |
| if use_cubes==1: | |
| robot.world.disconnect_from_cubes() | |
| freeplay = 0 | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: begin finding charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| # charger location search | |
| if charger: | |
| #we know where the charger is | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charger position known, travelling, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.move_lift(-3) | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to move lift, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to set head angle, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.Body, num_retries=3) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to go to charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=3) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to go to charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2))) | |
| wfaac | |
| time.sleep(1) | |
| robot_start_docking(robot) | |
| else: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.move_lift(-3) | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to move lift, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to set head angle, battery %s" % str(round(robot.battery_voltage, 2))) | |
| #try: | |
| # robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed() | |
| #except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| # print('failed to drive') | |
| # randomly drive around for a bit and see if we can spot the charger | |
| robot_find_charger_pattern(robot) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: locate dock sequence completed, battery %s" % str(round(robot.battery_voltage, 2))) | |
| # | |
| ## define look around for charger routine | |
| # | |
| def robot_find_charger_pattern(robot: robot): | |
| global charger, freeplay, lowbatvolt, scheduled_time | |
| while not charger: | |
| # we will look around in place for the charger for 30 seconds | |
| robot.enable_all_reaction_triggers(True) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: look around 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() | |
| time.sleep(0.5) | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| robot.move_lift(-3) | |
| time.sleep(1) | |
| robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) | |
| try: | |
| charger = robot.world.wait_for_observed_charger(timeout=45, include_existing=True) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: found charger in lookaround!, battery %s" % str(round(robot.battery_voltage, 2))) | |
| break | |
| except asyncio.TimeoutError: | |
| robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop() | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: unable to find charger in lookaround, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabUnhappy, in_parallel=True, ignore_body_track=True, ignore_head_track=True).wait_for_completed() | |
| robot.set_head_angle(degrees(0), in_parallel=True).wait_for_completed() | |
| wfaac | |
| time.sleep(2) | |
| finally: | |
| # stop the lookaround behavior | |
| robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop() | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: stop lookaround routine, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.clear_idle_animation() | |
| wfaac | |
| time.sleep(3) | |
| # we have not managed to find the charger. Falling back to freeplay. | |
| robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| wfaac | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: fallback to 90 seconds of freeplay, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.start_freeplay_behaviors() | |
| #try freeplay for 90 seconds, if found charger finally stop freeplay | |
| try: | |
| charger = robot.world.wait_for_observed_charger(timeout=90, include_existing=True) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: found the charger within 90 seconds of playtime!, battery %s" % str(round(robot.battery_voltage, 2))) | |
| break | |
| except asyncio.TimeoutError: | |
| robot.stop_freeplay_behaviors() | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charger not found after 90 seconds, 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() | |
| wfaac | |
| time.sleep(1) | |
| finally: | |
| #after 90 seconds end freeplay | |
| robot.stop_freeplay_behaviors() | |
| wfaac | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: stop freeplay routine, battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep(1) | |
| # | |
| ## define the begin docking pattern | |
| # | |
| def robot_start_docking(robot: robot): | |
| global charger, freeplay, lowbatvolt, scheduled_time | |
| # we don't know where the charger is | |
| # Charger location and docking handling here | |
| #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_lift_track=True, ignore_body_track=True, ignore_head_track=True).wait_for_completed() | |
| wfaac | |
| time.sleep(1) | |
| robot.move_lift(-3) | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| # If you are running Cozmo on carpet, try turning these num_retries up to 4 or 5 | |
| dockloop = 0 | |
| while dockloop <= 3 and robot.is_on_charger == 0 and charger != None: | |
| alignmentloop = 0 | |
| while alignmentloop < 3: | |
| charger_distance = 100 | |
| while charger_distance > 30: | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(charger_distance), RobotAlignmentTypes.Body, num_retries=4) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('failed to go to charger') | |
| wfaac | |
| time.sleep(2) | |
| charger_distance -= 20 | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('failed to turn in place') | |
| wfaac | |
| if alignmentloop < 2: | |
| time.sleep(1) | |
| robot.set_head_light(False) | |
| time.sleep(0.5) | |
| robot.set_head_light(True) | |
| time.sleep(0.5) | |
| robot.set_head_light(False) | |
| time.sleep(0.5) | |
| robot.set_head_light(True) | |
| time.sleep(0.5) | |
| robot.set_head_light(False) | |
| time.sleep(1) | |
| try: | |
| robot.drive_straight(distance_mm(-80), speed_mmps(50)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('failed to back up a little bit') | |
| wfaac | |
| time.sleep(1) | |
| alignmentloop += 1 | |
| time.sleep(1) | |
| time.sleep(1) | |
| try: | |
| robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('failed to play anim') | |
| wfaac | |
| time.sleep(1) | |
| if charger and charger.is_visible == False: | |
| #we know where the charger is | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: can not see charger, position is still known, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=2).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2))) | |
| finally: | |
| pass | |
| try: | |
| action = robot.drive_straight(distance_mm(-140), speed_mmps(40)) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to drive straight, battery %s" % str(round(robot.battery_voltage, 2))) | |
| finally: | |
| pass | |
| try: | |
| robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to play anim, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to set head angle, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.Body, num_retries=2) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to go to charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=2) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to go to charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| action = robot.drive_straight(distance_mm(-50), speed_mmps(40)) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to drive straight, battery %s" % str(round(robot.battery_voltage, 2))) | |
| finally: | |
| pass | |
| time.sleep(1) | |
| if charger and charger.is_visible == False: | |
| # we don't know where the charger is anymore | |
| #robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| try: | |
| robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to drive straight, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2))) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charger not found, clearing map, battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep(1) | |
| charger = None | |
| break | |
| if dockloop < 3: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('docking loop '+str(dockloop)+' completed') | |
| else: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('loop 3 - docking re-initialized') | |
| charger = None | |
| dockloop = 0 | |
| time.sleep(2) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: I should be in front of the charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| #robot.world.charger = None | |
| robot.set_head_light(False) | |
| time.sleep(0.5) | |
| robot.set_head_light(True) | |
| time.sleep(0.5) | |
| robot.set_head_light(False) | |
| time.sleep(0.5) | |
| robot.set_head_light(True) | |
| time.sleep(0.5) | |
| robot.set_head_light(False) | |
| wfaac | |
| time.sleep(1) | |
| robot.enable_all_reaction_triggers(False) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: docking, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees - 180), accel=degrees(75), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to do a 180, battery %s" % str(round(robot.battery_voltage, 2))) | |
| wfaac | |
| time.sleep(1) | |
| #robot_reaction_chance(cozmo.anim.Triggers.CubePounceFake,1,True,False,False) | |
| time.sleep(1) | |
| backup_count = 0 | |
| while robot.is_on_charger == 0: | |
| try: | |
| robot.backup_onto_charger(max_drive_time=2) | |
| backup_count+=1 | |
| time.sleep(1.5) | |
| if robot.is_on_charger == 1: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: Robot is on Charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| break | |
| elif backup_count == 6: | |
| break | |
| except robot.is_on_charger == 1: | |
| robot.stop_all_motors() | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: Contact! Stop all motors, battery %s" % str(round(robot.battery_voltage, 2))) | |
| wfaac | |
| wfaac | |
| time.sleep(0.5) | |
| # check if we're now docked | |
| if robot.is_on_charger: | |
| time.sleep(2) | |
| robot.play_anim("anim_sparking_success_02").wait_for_completed() | |
| time.sleep(2) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('Docked!') | |
| #wfaac | |
| dockloop = 0 | |
| robot.clear_idle_animation() | |
| robot.abort_all_actions(True) | |
| time.sleep(2) | |
| break | |
| else: | |
| # No, we missed. Back off and try again | |
| dockloop = dockloop + 1 | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to dock, go forward and try docking again, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.move_lift(-3) | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to move lift, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to set head angle, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees - 180), accel=degrees(90), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to turn, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.drive_straight(distance_mm(90), speed_mmps(90)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to drive, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.drive_straight(distance_mm(90), speed_mmps(90)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to drive, battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep(1) | |
| wfaac | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(90), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to turn, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to set head angle, battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep(1) | |
| if charger and charger.is_visible == False: | |
| #we know where the charger is | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: can not see charger after turnaround, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to play anim, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to set head angle, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(90), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to turn, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.Body, num_retries=2) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to go to charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=2) | |
| action.wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to go to charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep(1) | |
| if charger.is_visible == False: | |
| # we lost the charger, clearing all map variables | |
| charger = None | |
| try: | |
| robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to play anim, battery %s" % str(round(robot.battery_voltage, 2))) | |
| try: | |
| robot.drive_straight(distance_mm(-120), speed_mmps(50)).wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to go drive straight, battery %s" % str(round(robot.battery_voltage, 2))) | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charger lost - docking loop re-initialized, battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep(1) | |
| break | |
| if dockloop < 3: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: docking loop "+str(dockloop)+" completed, battery %s" % str(round(robot.battery_voltage, 2))) | |
| else: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: loop 3 - docking re-initialized, battery %s" % str(round(robot.battery_voltage, 2))) | |
| charger = None | |
| dockloop = 0 | |
| time.sleep(2) | |
| # | |
| # define Cozmo's Scheduler resting time | |
| # | |
| def cozmo_resting(robot: robot): | |
| global nowtime, bday_name1, bday_name2, bday_year1, bday_year2, bday_month1, bday_month2, bday_day1, bday_day2, say_age1, say_age2 | |
| global bday_year, bday_month, bday_day, bday_name, Hdays, robotvolume | |
| i = random.randint(1, 100) | |
| if (robot.is_on_charger == 1) and (robot.is_charging == 1): | |
| # | |
| # Birthdays | |
| if bday_month1 == nowtime.tm_mon and bday_day1 == nowtime.tm_mday: | |
| bday_name = bday_name1 | |
| bday_year = bday_year1 | |
| bday_month = bday_month1 | |
| bday_day = bday_day1 | |
| say_age = say_age1 | |
| elif bday_month2 == nowtime.tm_mon and bday_day2 == nowtime.tm_mday: | |
| bday_name = bday_name2 | |
| bday_year = bday_year2 | |
| bday_month = bday_month2 | |
| bday_day = bday_day2 | |
| say_age = say_age2 | |
| if i > 20 and i < 70 and bday_month == nowtime.tm_mon and bday_day == nowtime.tm_mday and (nowtime.tm_min == 15 or nowtime.tm_min == 45): | |
| tempvolume=robotvolume | |
| robotvolume=1 | |
| robot.set_robot_volume(robotvolume) | |
| if robot.is_on_charger: | |
| robot.drive_off_charger_contacts(in_parallel=True).wait_for_completed() | |
| time.sleep(3) | |
| #robot_reaction_chance(cozmo.anim.Triggers.CodeLabAmazed,100,False,False,False) | |
| #time.sleep(1) | |
| if say_age == 1: | |
| robot.say_text('Happy Birthday '+str(bday_name)+'! You are '+str(nowtime.tm_year - bday_year)+' years old!', play_excited_animation=True) | |
| else: | |
| robot.say_text('Happy Birthday '+str(bday_name)+'!', play_excited_animation=True) | |
| time.sleep(3) | |
| robot.clear_idle_animation() | |
| time.sleep(0.1) | |
| robot.backup_onto_charger() | |
| time.sleep(3) | |
| robotvolume=tempvolume | |
| robot.set_robot_volume(robotvolume) | |
| wfaac | |
| time.sleep(3) | |
| # | |
| # Holidays | |
| for (x,y) in Hdays: | |
| if i > 20 and i < 70 and x == nowtime.tm_mon and y == nowtime.tm_mday and ((nowtime.tm_min == 15 or nowtime.tm_min == 45) or (x == 1 and y == 1 and nowtime.tm_hour == 0 and nowtime.tm_min <= 4)): | |
| tempvolume=robotvolume | |
| robotvolume=1 | |
| robot.set_robot_volume(robotvolume) | |
| if robot.is_on_charger: | |
| robot.drive_off_charger_contacts(in_parallel=False, num_retries=2).wait_for_completed() | |
| robot.clear_idle_animation() | |
| robot.abort_all_actions() | |
| robot.stop_all_motors() | |
| time.sleep(3) | |
| wfaac | |
| # Remember to add your new Holiday say_text in here or Cozmo will say nothing | |
| # Also add your (elif x == month and y == day) for your holiday say_text | |
| if x == 11 and y >= 10 and y <=20: | |
| robot.say_text('Happy Movember!', play_excited_animation=True, in_parallel=False).wait_for_completed() | |
| elif x == 12 and y == 31: | |
| robot.say_text('Happy New Years-Eve!', play_excited_animation=True).wait_for_completed() | |
| elif x == 1 and y == 1: | |
| robot.say_text('Happy New Year!', play_excited_animation=True).wait_for_completed() | |
| elif x == 10 and y == 31: | |
| robot.say_text('Happy Scary Holloween!', play_excited_animation=True).wait_for_completed() | |
| elif x == 7 and y == 1: | |
| robot.say_text('Happy Canada day!', play_excited_animation=True).wait_for_completed() | |
| elif x == 7 and y == 4: | |
| robot.say_text('Happy Independence day!', play_excited_animation=True).wait_for_completed() | |
| time.sleep(3) | |
| if not robot.is_on_charger: | |
| robot.backup_onto_charger() | |
| time.sleep(3) | |
| robotvolume=tempvolume | |
| robot.set_robot_volume(robotvolume) | |
| wfaac | |
| time.sleep(3) | |
| # On charger random sounds and animations go here | |
| if (robot.is_on_charger == 1) and (robot.is_charging == 1): | |
| if scheduled_time == 1: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('charging, inside schedule') | |
| else: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('charging, outside schedule') | |
| #i = random.randint(1, 100) | |
| if i >= 97: | |
| try: | |
| robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('failed to play anim') | |
| elif i >= 90: | |
| try: | |
| robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('failed to play anim') | |
| time.sleep(3) | |
| robot.set_all_backpack_lights(cozmo.lights.green_light) | |
| time.sleep(3) | |
| robot.set_all_backpack_lights(cozmo.lights.off_light) | |
| elif (robot.is_on_charger == 1) and (robot.is_charging == 0): | |
| if scheduled_time == 1: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('Charged! Starting scheduled playtime') | |
| else: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('Charged! waiting for scheduled playtime') | |
| #i = random.randint(1, 100) | |
| if i >= 97: | |
| try: | |
| robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('failed to play anim') | |
| elif i >= 85: | |
| try: | |
| robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
| except: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('failed to play anim') | |
| time.sleep(3) | |
| robot.set_all_backpack_lights(cozmo.lights.green_light) | |
| time.sleep(3) | |
| robot.set_all_backpack_lights(cozmo.lights.off_light) | |
| elif (robot.is_on_charger == 0) and (robot.is_charging == 0) and scheduled_days == "Resting": | |
| robot.stop_freeplay_behaviors() | |
| robot_locate_dock_or_not(robot) | |
| # | |
| # define Scheduler | |
| # | |
| def robot_check_scheduler(robot: robot): | |
| global use_scheduler, nowtime, wday, scheduled_days, scheduled_time, main_loop_count, dayornight | |
| global wkDS, wkDSm, wkDE, wkDEm, wkNS, wkNSm, wkNE, wkNEm, wkEDS, wkEDSm, wkEDE, wkEDEm, wkENS, wkENSm, wkENE, wkENEm | |
| wday = nowtime.tm_wday # 0 is Monday, 6 is Sunday | |
| #TempTime = Datetime.now().timetuple() | |
| nowtime = Datetime.now().timetuple() | |
| # NOTICE: This Below Will Display Time Tuple for scripting usage purposes | |
| # It will reveal everything you need to work with the Scheduler Time Tuple "nowtime" variable | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| #print('Full timetuple() is: '+str(nowtime)) | |
| if nowtime.tm_hour >= 6 and nowtime.tm_hour <= 17 and dayornight != "day": | |
| dayornight = "day" | |
| elif dayornight != "night": | |
| dayornight = "night" | |
| # while True: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('Schedule is: '+str(scheduled_days)) | |
| time.sleep(1) | |
| #weekday mornings | |
| if wday < 5 and ( | |
| ( | |
| (nowtime.tm_hour > wkDS) | |
| or (nowtime.tm_hour == wkDS and nowtime.tm_min >= wkDSm) | |
| ) | |
| and ( | |
| (nowtime.tm_hour < wkDE) | |
| or (nowtime.tm_hour == wkDE and nowtime.tm_min < wkDEm) | |
| ) | |
| ) or wday < 5 and ( | |
| wkDE < wkDS | |
| and ( | |
| (nowtime.tm_hour > wkDE and nowtime.tm_hour > wkDS) | |
| or (nowtime.tm_hour == wkDS and nowtime.tm_min >= wkDSm) | |
| ) or ( | |
| wkDE < wkDS | |
| and ( | |
| (nowtime.tm_hour < wkDE and nowtime.tm_hour < wkDS) | |
| or (nowtime.tm_hour == wkDE and nowtime.tm_min < wkDEm) | |
| ) | |
| ) | |
| ): | |
| scheduled_days = "WeekDayTimeSlot-1" | |
| scheduled_time = 1 | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('Weekday '+str(dayornight)+' time is: '+str(nowtime.tm_hour)+':'+str(nowtime.tm_min)) | |
| #weekday evenings | |
| elif wday < 5 and ( | |
| ( | |
| (nowtime.tm_hour > wkNS) | |
| or (nowtime.tm_hour == wkNS and nowtime.tm_min >= wkNSm) | |
| ) | |
| and ( | |
| (nowtime.tm_hour < wkNE) | |
| or (nowtime.tm_hour == wkNE and nowtime.tm_min < wkNEm) | |
| ) | |
| ) or wday < 5 and ( | |
| wkNE < wkNS | |
| and ( | |
| (nowtime.tm_hour > wkNE and nowtime.tm_hour > wkNS) | |
| or (nowtime.tm_hour == wkNS and nowtime.tm_min >= wkNSm) | |
| ) or ( | |
| wkNE < wkNS | |
| and ( | |
| (nowtime.tm_hour < wkNE and nowtime.tm_hour < wkNS) | |
| or (nowtime.tm_hour == wkNE and nowtime.tm_min < wkNEm) | |
| ) | |
| ) | |
| ): | |
| scheduled_days = "WeekDayTimeSlot-2" | |
| scheduled_time = 1 | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('Weekday '+str(dayornight)+' time is: '+str(nowtime.tm_hour)+':'+str(nowtime.tm_min)) | |
| #weekend mornings | |
| elif wday >= 5 and ( | |
| ( | |
| (nowtime.tm_hour > wkEDS) | |
| or (nowtime.tm_hour == wkEDS and nowtime.tm_min >= wkEDSm) | |
| ) and ( | |
| (nowtime.tm_hour < wkEDE) | |
| or (nowtime.tm_hour == wkEDE and nowtime.tm_min < wkEDEm) | |
| ) | |
| ) or wday >= 5 and ( | |
| wkEDE < wkEDS | |
| and ( | |
| (nowtime.tm_hour > wkEDE and nowtime.tm_hour > wkEDS) | |
| or (nowtime.tm_hour == wkEDS and nowtime.tm_min >= wkEDSm) | |
| ) or ( | |
| wkEDE < wkEDS | |
| and ( | |
| (nowtime.tm_hour < wkEDE and nowtime.tm_hour < wkEDS) | |
| or (nowtime.tm_hour == wkEDE and nowtime.tm_min < wkEDEm) | |
| ) | |
| ) | |
| ): | |
| scheduled_days = "WeekEndTimeSlot-1" | |
| scheduled_time = 1 | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('Weekend '+str(dayornight)+' time is: '+str(nowtime.tm_hour)+':'+str(nowtime.tm_min)) | |
| #weekend evenings | |
| elif wday >= 5 and ( | |
| ( | |
| (nowtime.tm_hour > wkENS) | |
| or (nowtime.tm_hour == wkENS and nowtime.tm_min >= wkENSm) | |
| ) | |
| and ( | |
| (nowtime.tm_hour < wkENE) | |
| or (nowtime.tm_hour == wkENE and nowtime.tm_min < wkENEm) | |
| ) | |
| ) or wday >= 5 and ( | |
| wkENE < wkENS | |
| and ( | |
| (nowtime.tm_hour > wkENE and nowtime.tm_hour > wkENS) | |
| or (nowtime.tm_hour == wkENS and nowtime.tm_min >= wkENSm) | |
| ) | |
| or ( | |
| wkENE < wkENS | |
| and ( | |
| (nowtime.tm_hour < wkENE and nowtime.tm_hour < wkENS) | |
| or (nowtime.tm_hour == wkENE and nowtime.tm_min < wkENEm) | |
| ) | |
| ) | |
| ): | |
| scheduled_days = "WeekEndTimeSlot-2" | |
| scheduled_time = 1 | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print('Weekend '+str(dayornight)+' time is: '+str(nowtime.tm_hour)+':'+str(nowtime.tm_min)) | |
| # Schedule not set or in Resting state | |
| elif scheduled_days == "None" or scheduled_days == "Resting": | |
| nowtime = Datetime.now().timetuple() | |
| scheduled_days = "Resting" | |
| scheduled_time = 0 | |
| cozmo_resting(robot) | |
| # | |
| # | |
| # START THE SHOW! | |
| # | |
| #cozmo.robot.Robot.drive_off_charger_on_connect = False | |
| #cozmo.run_program(cozmo_unleashed, use_viewer=True, force_viewer_on_top=True, show_viewer_controls=True, exit_on_connection_error=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, force_viewer_on_top=True, show_viewer_controls=True, exit_on_connection_error=True) | |
| # which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus | |
| # --- Above ^^ is the common command for running the first def. | |
| # --- Below -- is a script from acidzebra for loading the main program, that I will | |
| # like to eventually use to turn into a SDK Loop that can stay running in an executed | |
| # program so that if the WiFi disconnects & reconnects again, the SDK Loop will kick | |
| # back in when it sees a phone with an open SDK connection again, without having to | |
| # stop and restart the script manually. | |
| # Run at your own RISK, using the AbstractEventLoop is dangerous. | |
| def run(sdk_conn_loop): | |
| global robot | |
| '''The run method runs once the Cozmo SDK is connected.''' | |
| robot = sdk_conn_loop.wait_for_robot(timeout=1) | |
| try: | |
| #cozmo_unleashed(robot) | |
| # ^^ this above line ^^ is the original safest call to use | |
| # this connect_on_loop seems to work, though i don't think it actually reconnects | |
| # the phone to cozmo as i intended it to. i'm still working on that part | |
| # this AbstractEventLoop line might cause issues, use at your own risk | |
| #connect_on_loop(AbstractEventLoop.run_forever(cozmo_unleashed(robot)), sdk_conn_loop) | |
| # this non-Abstract connect_on_loop is much safer, use this line below | |
| connect_on_loop(cozmo_unleashed(robot), sdk_conn_loop) | |
| except KeyboardInterrupt as k: | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("") | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| print("Exit requested by user") | |
| SystemExit("Keyboard interrupt: %s" % k) | |
| if __name__ == '__main__': | |
| cozmo.setup_basic_logging() | |
| cozmo.robot.Robot.drive_off_charger_on_connect = False # Cozmo can stay on charger for now | |
| try: | |
| cozmo.connect_with_tkviewer(run, force_on_top=True) | |
| except cozmo.ConnectionError as e: | |
| SystemExit("A connection error with viewer occurred: %s" % e) |
This sounds great! Next week I’ll try it on my Cozmo because I’m out of town. But thank you very very much: really.
You & the Cozmo Community are welcome. You may have noticed I have also included scripts that use the ChargerMarker printout. One script uses an Asyncio Gather to run the 2 processes, looking for the Charger & looking for the printer Marker, threaded at the same time. Where as the other Non_Asyncio script, I have figured out a way to get the same job done, within the same amount of time, while not using the threaded Gather to do it. This second Non_Asyncio script is safe for Python 3.6, whereas the Gather script needs 3.7 to 3.9 in order to run.
I've made all my Cozmo work public for the Cozmo Community to use & learn & make Cozmo do way more things than I ever could.
I tried to connect my Cozmo, but the cmd (using Windows / Android) gives me an "androidconnector.connect" error (because of the loop=loop argument), and when I correct it on the run.py file now it says: "No devices connected running cozmo in sdk mode" (even if the device appears when using "adb devices" command).
Can't make it work, but is not because of your code; can't run any .py file on Cozmo... I'll try again later on another computer.
I tried to connect my Cozmo, but the cmd (using Windows / Android) gives me an "androidconnector.connect" error (because of the loop=loop argument), and when I correct it on the run.py file now it says: "No devices connected running cozmo in sdk mode" (even if the device appears when using "adb devices" command).
Can't make it work, but is not because of your code; can't run any .py file on Cozmo... I'll try again later on another computer.
Cozmo seems to run on Python version 3.6 to 3.9, it will not run 3.10. I use Visual Studio 2022 to code with. I include, manually, the USB Connectivity package into the app which is not on by default.
I am unable to give you any real advice on this as Visual Studio 2022 is my main goto for running Cozmo and it always works for me.
Please give me an update and let me know if you have solved the issue yet.
Make sure you have the Android Tools Package "folder" with the adb.exe file in the Windows Environment path, or in the Path of whatever OS u happen to be using.
FYI to all Cozmo Owners, Python package "Pillow" is receiving some new updates that will cause your current Cozmo SDK version not to run.
I found the solution is to install version "Pillow==9.1.1" and do not go past this version until the Cozmo SDK has, at some point, been updated to accept these new python changes.
There is also another previous SPAM from the python packages, that no longer spams anymore, although you will also need to make this change, currently, to get your Cozmo up to date.
Without explaining all the details, you need to change these lines in the "annotate.py" file,
located in your "...\Python\Python##\site-packages\cozmo" folder: (## will be your python version)
Line 38: to ...collections.abc
Line 181: to ...collections.abc
After changing the Import & the call that uses it, your annotate file will be up to date.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
This sounds great! Next week I’ll try it on my Cozmo because I’m out of town. But thank you very very much: really.