Last active
January 25, 2022 14:23
-
-
Save acidzebra/c02ff8c8ccb0e3a057ae0b48a5082a68 to your computer and use it in GitHub Desktop.
Cozmo Unleashed: Attempting to create a perpetuum cozmobile for Anki's awesome robot - https://www.anki.com/en-us/cozmo
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 | |
| # | |
| # NOTE I HAVE MOVED ON TO USING A CUSTOM MARKER PRINTED OUT AND PASTED ON TOP OF MY CHARGER | |
| # SIZED AT 4CM/SIDE COZMO HAS A MUCH BETTER CHANCE OF FINDING THE CHARGER - SIMILAR TO THE NEW VECTOR ROBOT | |
| # the last "general purpose" version is https://gist.github.com/acidzebra/c02ff8c8ccb0e3a057ae0b48a5082a68/ad4ff9d686f7e2a1b197c4a26c6290d51a7c4380 | |
| # if you want to print out your own you can find a version of marker CustomObjectMarkers.Hexagons2 used here | |
| # http://cozmosdk.anki.com/docs/generated/cozmo.objects.html?highlight=hexagons2#cozmo.objects.CustomObjectMarkers.Hexagons2 | |
| # | |
| # WHAT THIS PROGRAM DOES | |
| # | |
| # SHORT VERSION: this program makes Cozmo play until he runs low on battery, then makes him find and dock with his charger. | |
| # Once his battery is fully recharged, he will leave the dock and start playing again. Rinse, repeat. | |
| # | |
| # You can run this program as-is but it is likely some things will need to be tweaked to suit your Cozmo and environment. | |
| # | |
| # AUTOMATIC DOCK FINDING AND DOCKING | |
| # this is highly reliant on a number of factors, including having enough ambient light, how big the area Cozmo has to explore is, | |
| # and just sheer blind luck sometimes. The built-in camera of Cozmo is not very high rez, and the dock icon is a little small. | |
| # You can improve this in several ways - better lights, custom printed large battery icon (and some fiddling with the docking routing) | |
| # | |
| # The surface Cozmo moves on can be more slippery or less than my environment, this will impact things like docking. | |
| # On my table's surface, I do two 95 degree turns, this makes it so Cozmo has a high rate of successful dockings. | |
| # Look for these two lines in the code and adjust as necessary | |
| # robot.turn_in_place(degrees(95)).wait_for_completed() | |
| # robot.turn_in_place(degrees(95)).wait_for_completed() | |
| # | |
| # Your Cozmo's battery will have slightly different upper and lower limits. | |
| # there is a lower threshold at which Cozmo will switch states and start looking for his charger. | |
| # The variable is called lowbatvoltage, 3.7 is the setting that works for me. | |
| # a higher value will cause Cozmo to start looking for his dock sooner. | |
| # | |
| # have a look at the section called | |
| # CONFIGURABLE VARIABLES | |
| # for more stuff | |
| # | |
| # DETAILED VERSION: | |
| # This program defines a number of 'states' for Cozmo: | |
| # State 1: on charger, charging | |
| # State 2: on charger, fully charged | |
| # State 3: not on charger, battery starting to get low | |
| # State 4: not on charger, good battery - freeplay active | |
| # State 5: battery low, looking for charger | |
| # State 6: battery low and we know where the charger is, moving to dock and docking | |
| # State 9: Cozmo is on its side or is currently picked up | |
| # State 0: recovery state - used to smooth state switching and to reassess which state Cozmo should be in | |
| # state 99: "fake" state used when running a random animation | |
| # state 98: "fake" state used when cozmo is looking around for his charger | |
| # | |
| # States dictate what Cozmo does. In state 4 Cozmo will enter freeplay mode, exploring and interacting with his environment: | |
| # stack cubes, build pyramids, offer people he sees fistbumps and games of peekaboo, and more. When his battery drops below | |
| # a certain threshold (set in the variable lowbatvoltage), he will start looking for his charger (state 5). | |
| # If he finds it he will attempt to dock (state 6) and start charging (state 1). Once fully charged (state 2), he drives off | |
| # the charger and the cycle repeats (state 4). State 3 is used as a 'delay' so a single drop in battery level won't | |
| # immediately cause him to seek out his charger. States 9 and 0 handle being picked up, falling, being on his side, and eventual | |
| # recovery. | |
| # | |
| # The battery level of Cozmo is also used to manipulate his three "needs" settings: repair, energy, play (the three metrics for | |
| # 'happiness' that you see in the main screen of the Anki Cozmo app). As his battery level gets lower so will his overall mood | |
| # and innate reactions to events. (TLDR: he gets grumpier the lower his battery level is) | |
| # | |
| # An event monitor running in a seperate thread watches for events like being picked up, seeing a face, detecting a cliff, etc. | |
| # Some events are just logged, others trigger different Cozmo states or actions. | |
| # | |
| #import required functions | |
| import sys, os, datetime, random, time, math, re, threading | |
| ##import logging | |
| import asyncio, cozmo, cozmo.objects, cozmo.util | |
| from cozmo.util import degrees, distance_mm, speed_mmps, Pose | |
| from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes | |
| #external libraries used for camera annotation | |
| #you will need to add these using pip/pip3 | |
| from PIL import ImageDraw, ImageFont | |
| import numpy as np | |
| #logging.basicConfig(filename='cozmo.log',level=logging.WARN) | |
| # set up global variables | |
| global robot | |
| global freeplay | |
| global tempfreeplay | |
| global needslevel | |
| global cozmostate | |
| global scheduler_playokay | |
| global msg | |
| global start_time | |
| global use_cubes | |
| global charger | |
| global maxbatvoltage | |
| global highbatvoltage | |
| global lowbatvoltage | |
| global batlightcounter | |
| global use_scheduler | |
| global camera | |
| global foundcharger | |
| global lightstate | |
| global robotvolume | |
| global debugging | |
| global batcounter | |
| global chargermarker1 | |
| # initialize needed variables | |
| freeplay = 0 | |
| lightstate = 0 | |
| batcounter = 0 | |
| robot = cozmo.robot.Robot | |
| msg = 'No status' | |
| q = None # dependency on queue variable for messaging instead of printing to event-content directly | |
| thread_running = False # starting thread for custom events | |
| # | |
| #============================ | |
| # CONFIGURABLE VARIABLES HERE | |
| #============================ | |
| # | |
| # BATTERY THRESHOLDS | |
| # | |
| # low battery voltage - when voltage drops below this Cozmo will start looking for his charger | |
| # high battery voltage - when cozmo comes off your charger fully charge, this value will self-calibrate | |
| # maxbatvoltage - the maximum battery level as recorded when cozmo is on charger but no longer charging | |
| # tweak this to suit your cozmo | |
| lowbatvoltage = 3.7 | |
| highbatvoltage= 4.14 | |
| maxbatvoltage = 4.8 | |
| # | |
| # 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 | |
| use_cubes = 1 | |
| # | |
| # COZMO VOLUME | |
| # what volume Cozmo should play sounds at, value between 0 and 1 | |
| robotvolume = 0.2 | |
| # | |
| # 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 | |
| use_scheduler = 0 | |
| # | |
| # DEBUGGING | |
| # when disabled, clears the screen status updates every cycle | |
| debugging = 1 | |
| # END OF CONFIGURABLE VARIABLES | |
| # | |
| # | |
| # CAMERA ANNOTATOR | |
| # | |
| # | |
| @cozmo.annotate.annotator | |
| def camera_info(image, scale, annotator=None, world=None, **kw): | |
| global camera | |
| d = ImageDraw.Draw(image) | |
| bounds = [3, 0, image.width, image.height] | |
| camera = world.robot.camera | |
| text_to_display = 'Exposure: %s ms\n' % camera.exposure_ms | |
| text_to_display += 'Gain: %.3f\n' % camera.gain | |
| text = cozmo.annotate.ImageText(text_to_display,position=cozmo.annotate.TOP_LEFT,line_spacing=2,color="white",outline_color="black", full_outline=True) | |
| text.render(d, bounds) | |
| # | |
| # MAIN PROGRAM LOOP START | |
| # | |
| def cozmo_unleashed(robot: cozmo.robot.Robot): | |
| cozmo.setup_basic_logging(general_log_level='WARN', protocol_log_level='WARN', protocol_log_messages='all', deprecated_filter='default') | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| global cozmostate,chargermarker1,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, batlightcounter, lowbatvoltage, highbatvoltage, maxbatvoltage, use_scheduler,msg,objmsg,facemsg,camera, foundcharger, tempfreeplay | |
| #robot.world.charger = None | |
| #charger = None | |
| foundcharger = 0 | |
| robot.set_robot_volume(robotvolume) | |
| if use_cubes == 1: | |
| robot.enable_freeplay_cube_lights(enable=True) | |
| #robot.enable_device_imu(enable_raw=False, enable_user=True, enable_gyro=True) | |
| # set up some camera stuff | |
| robot.world.image_annotator.add_annotator('camera_info', camera_info) | |
| camera = robot.camera | |
| camera.enable_auto_exposure() | |
| robot.enable_facial_expression_estimation(enable=True) | |
| robot.camera.image_stream_enabled = True | |
| robot.camera.color_image_enabled = False | |
| if use_cubes == 0: | |
| robot.world.disconnect_from_cubes() | |
| else: | |
| robot.world.connect_to_cubes() | |
| robot.enable_all_reaction_triggers(True) | |
| robot.enable_stop_on_cliff(True) | |
| q = None # dependency on queue variable for messaging instead of printing to event-content directly | |
| thread_running = False # starting thread for custom events | |
| robot.set_needs_levels(repair_value=1, energy_value=1, play_value=1) | |
| needslevel = 1 | |
| tempfreeplay = 0 | |
| lowbatcount=0 | |
| batlightcounter=0 | |
| cozmostate = 0 | |
| #some custom objects that I printed out and use as virtual walls, if you don't have them don't worry about it, it won't affect the program | |
| # wall_obj1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType01, CustomObjectMarkers.Circles2, 340, 120, 44, 44, True) | |
| # wall_obj2 = robot.world.define_custom_wall(CustomObjectTypes.CustomType02, CustomObjectMarkers.Circles4, 340, 120, 44, 44, True) | |
| # wall_obj3 = robot.world.define_custom_wall(CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles5, 340, 120, 44, 44, True) | |
| # wall_obj4 = robot.world.define_custom_wall(CustomObjectTypes.CustomType04, CustomObjectMarkers.Triangles2,340, 120, 44, 44, True) | |
| # wall_obj5 = robot.world.define_custom_wall(CustomObjectTypes.CustomType05, CustomObjectMarkers.Triangles3,340, 120, 44, 44, True) | |
| # #wall_obj6 = robot.world.define_custom_wall(CustomObjectTypes.CustomType06, CustomObjectMarkers.Hexagons2, 120, 340, 44, 44, True) | |
| # wall_obj6 = robot.world.define_custom_wall(CustomObjectTypes.CustomType06, CustomObjectMarkers.Hexagons2, 44, 44, 44, 44, True) | |
| # wall_obj7 = robot.world.define_custom_wall(CustomObjectTypes.CustomType07, CustomObjectMarkers.Circles3, 120, 340, 44, 44, True) | |
| # wall_obj8 = robot.world.define_custom_wall(CustomObjectTypes.CustomType08, CustomObjectMarkers.Hexagons3, 120, 340, 44, 44, True) | |
| chargermarker1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType01, CustomObjectMarkers.Hexagons2, 40, 40, 40, 40, True) | |
| # initialize event monitoring thread | |
| q = None | |
| monitor(robot, q) | |
| start_time = time.time() | |
| msg = 'initialization complete' | |
| robot_print_current_state('entering main loop') | |
| # ENTERING STATE LOOP | |
| while True: | |
| #robot_backbackbatteryindicator() | |
| #robot_print_current_state('main loop checkpoint') | |
| # | |
| #State 1: on charger, charging | |
| if (robot.is_on_charger == 1) and (robot.is_charging == 1): | |
| if cozmostate != 1: # 1 is charging | |
| robot_print_current_state('switching to state 1') | |
| cozmostate = 1 | |
| start_time = time.time() | |
| foundcharger = 0 | |
| if robot.is_freeplay_mode_active: | |
| ######robot.enable_all_reaction_triggers(False) | |
| robot.stop_freeplay_behaviors() | |
| freeplay = 0 | |
| if use_cubes == 1: | |
| robot.world.disconnect_from_cubes() | |
| lowbatcount=0 | |
| cozmostate = 1 | |
| msg = 'state 1 checkpoint' | |
| robot_print_current_state('state 1 - charging') | |
| # once in a while make random snoring noises | |
| robot_check_sleep_snoring() | |
| # | |
| #State 2: on charger, fully charged | |
| # | |
| if (robot.is_on_charger == 1) and (robot.is_charging == 0): | |
| cozmostate = 2 | |
| robot_print_current_state('switching to state 2 - pausing 30 secs') | |
| time.sleep(30) | |
| if cozmostate != 2: # 2 is fully charged | |
| maxbatvoltage = robot.battery_voltage | |
| robot_print_current_state('switching to state 2') | |
| cozmostate = 2 | |
| lowbatcount=0 | |
| foundcharger = 0 | |
| if use_cubes == 1: | |
| robot.world.connect_to_cubes() | |
| msg = 'state 2 checkpoint' | |
| robot_print_current_state('state 2 - charged') | |
| robot.set_needs_levels(repair_value=1, energy_value=1, play_value=1) | |
| # robot.drive_off_charger_contacts().wait_for_completed() | |
| # robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() | |
| robot_check_scheduler() | |
| # | |
| #State 3: not on charger, battery starting to get low | |
| # | |
| # basic 'trigger guard' so Cozmo doesn't go to charger immediately if the voltage happens to dip below 3.7 | |
| if (robot.battery_voltage <= lowbatvoltage) and (robot.is_on_charger == 0) and cozmostate != 5 and cozmostate != 6 and cozmostate !=98 and cozmostate != 1 and cozmostate != 2: | |
| lowbatcount += 1 | |
| robot_set_needslevel() | |
| msg = 'state 3 checkpoint' | |
| robot_print_current_state('state 3 - low battery threshold breach %s' % str(lowbatcount)) | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabTakaTaka,1,False,False,False) | |
| # print("Event log : %s" % str(msg)) | |
| time.sleep(0.5) | |
| # if we dip below the threshold three times we switch to state 5 | |
| if lowbatcount > 2 and (robot.is_on_charger == 0) and cozmostate !=5 and cozmostate !=6 and cozmostate !=99 and cozmostate !=98 and cozmostate != 1 and cozmostate != 2: | |
| if use_cubes == 1: | |
| robot.world.disconnect_from_cubes() | |
| robot_set_needslevel() | |
| msg = 'state 3 exit' | |
| robot_print_current_state('state 3 - low battery - switching to state 5') | |
| if cozmostate != 1 and cozmostate != 6: | |
| cozmostate = 5 | |
| # | |
| #State 4: not on charger, good battery - freeplay active | |
| # | |
| if (robot.battery_voltage > lowbatvoltage) and (robot.is_on_charger == 0) and cozmostate != 9 and cozmostate != 5 and cozmostate != 6 and cozmostate != 3 and lowbatcount < 3 and cozmostate != 99 and cozmostate !=98: | |
| if cozmostate != 4: # 4 is freeplay | |
| msg = 'state 4 checkpoint' | |
| robot_print_current_state('freeplay - switching to state 4') | |
| cozmostate = 4 | |
| if freeplay == 0: | |
| freeplay = 1 | |
| start_time = time.time() | |
| try: | |
| robot.drive_wheels(40, 40, l_wheel_acc=50, r_wheel_acc=50, duration=1) | |
| except: | |
| robot_print_current_state('state 4 - failed to drive wheels') | |
| robot_reaction_chance(cozmo.anim.Triggers.OnSpeedtapGameCozmoWinHighIntensity,1,True,False,False) | |
| if use_cubes == 1: | |
| robot.world.connect_to_cubes() | |
| if not robot.is_freeplay_mode_active: | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| if not robot.is_freeplay_mode_active and cozmostate == 4: | |
| robot_print_current_state('state 4 - re-enabling freeplay') | |
| freeplay = 1 | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| robot_set_needslevel() | |
| if cozmostate == 4: | |
| robot_check_randomreaction() | |
| time.sleep(0.5) | |
| # | |
| # state 5: battery low, looking for charger | |
| # | |
| if cozmostate == 5 and tempfreeplay != 1: | |
| robot_print_current_state('switching to state 5') | |
| if robot.is_freeplay_mode_active: | |
| freeplay = 0 | |
| robot.stop_freeplay_behaviors() | |
| #robot.enable_all_reaction_triggers(True) | |
| robot_locate_dock() | |
| # if cozmostate == 6: | |
| # pass | |
| # else: | |
| # cozmostate = 0 | |
| # | |
| # state 6: battery low and we know where the charger is, moving to dock and docking | |
| # | |
| if cozmostate == 6: | |
| robot_print_current_state('switching to state 6') | |
| if robot.is_freeplay_mode_active: | |
| #####robot.enable_all_reaction_triggers(False) | |
| robot.stop_freeplay_behaviors() | |
| freeplay = 0 | |
| robot_print_current_state('state 6 - aborting freeplay') | |
| robot.abort_all_actions(log_abort_messages=True) | |
| robot.wait_for_all_actions_completed() | |
| freeplay = 0 | |
| ##robot_set_needslevel() | |
| robot_start_docking() | |
| # | |
| # state 9: we're on our side or are currently picked up | |
| # | |
| if cozmostate == 9: | |
| robot_print_current_state('switching to state 9') | |
| robot_flash_backpacklights(4278190335) # 4278190335 is red | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabUnhappy,100,False,False,False) | |
| while cozmostate == 9: | |
| robot_print_current_state('state 9 - anger loop') | |
| robot_set_needslevel() | |
| if not robot.is_falling and not robot.is_picked_up: | |
| robot_print_current_state('state 9 reset - switching to 0') | |
| cozmostate = 0 | |
| lightstate = 0 | |
| break | |
| if robot.is_freeplay_mode_active: | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.stop_freeplay_behaviors() | |
| freeplay = 0 | |
| robot.abort_all_actions(log_abort_messages=True) | |
| robot.clear_idle_animation() | |
| robot.wait_for_all_actions_completed() | |
| robot_reaction_chance(cozmo.anim.Triggers.AskToBeRightedLeft,100,False,False,False) | |
| robot_print_current_state('picked annoyed response 1') | |
| time.sleep(0.5) | |
| if not robot.is_falling and not robot.is_picked_up: | |
| robot_print_current_state('state reset - switching to 0') | |
| cozmostate = 0 | |
| lightstate = 0 | |
| break | |
| robot_reaction_chance(cozmo.anim.Triggers.TurtleRoll,100,False,False,False) | |
| robot_print_current_state('picked annoyed response 2') | |
| time.sleep(0.5) | |
| if not robot.is_falling and not robot.is_picked_up: | |
| robot_print_current_state('state reset - switching to 0') | |
| cozmostate = 0 | |
| lightstate = 0 | |
| break | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabUnhappy,100,False,False,False) | |
| robot_print_current_state('picked annoyed response 3') | |
| time.sleep(0.5) | |
| robot_print_current_state('state 9 - loop complete') | |
| # | |
| # state 0: recovery state | |
| # | |
| if cozmostate == 0: | |
| robot_print_current_state('state 0') | |
| #robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,True,True) | |
| #robot.set_all_backpack_lights(cozmo.lights.white_light) | |
| lightstate = 0 | |
| time.sleep(0.5) | |
| # | |
| # state 98: lookaround loop trying to find charger | |
| # | |
| # | |
| # state 99: animation is playing | |
| # | |
| # | |
| # we have looped through the states | |
| # | |
| #robot_set_needslevel() | |
| #robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,1,True,True,True) | |
| #msg = 'state loop complete' | |
| #robot_check_randomreaction() | |
| #robot_print_current_state('cozmo_unleashed state program loop complete') | |
| time.sleep(0.5) | |
| # | |
| # | |
| # END OF STATE LOOP | |
| # | |
| ##robot_set_needslevel() | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabTakaTaka,1,True,True,False) | |
| robot_print_current_state('exiting main loop - how did we get here?') | |
| # | |
| # ROBOT FUNCTIONS | |
| # | |
| def robot_set_backpacklights(color): | |
| global robot | |
| robot.set_backpack_lights_off() | |
| color1=cozmo.lights.Color(int_color=color, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=2000, off_period_ms=1000, transition_on_period_ms=1500, transition_off_period_ms=500) | |
| light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=2000) | |
| light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=2000, transition_on_period_ms=500, transition_off_period_ms=1500) | |
| robot.set_backpack_lights(None, light1, light2, light3, None) | |
| def robot_flash_backpacklights(color): | |
| global robot | |
| robot.set_backpack_lights_off() | |
| color1=cozmo.lights.Color(int_color=color, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=250, transition_on_period_ms=375, transition_off_period_ms=125) | |
| light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=250, off_period_ms=250, transition_on_period_ms=250, transition_off_period_ms=500) | |
| light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=250, off_period_ms=500, transition_on_period_ms=125, transition_off_period_ms=375) | |
| robot.set_backpack_lights(None, light1, light2, light3, None) | |
| def robot_backbackbatteryindicator(): | |
| global robot,highbatvoltage,lowbatvoltage,maxbatvoltage,lightstate,batlightcounter | |
| batmultiplier = ((highbatvoltage - lowbatvoltage)/3)+0.1 | |
| chargebatmultiplier = ((maxbatvoltage - lowbatvoltage)/3)+0.1 | |
| critbatmultiplier = ((lowbatvoltage - 3.5)/3) | |
| robotvoltage=(robot.battery_voltage) | |
| if not lightstate: | |
| lightstate = 0 | |
| oldlightstate = lightstate | |
| if robotvoltage > (highbatvoltage-batmultiplier) and cozmostate==4: | |
| # bottom two lights on, third light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=1 and lightstate !=2: | |
| lightstate = 1 | |
| color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1) | |
| light2=cozmo.lights.Light(on_color=color1) | |
| light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| elif robotvoltage > (highbatvoltage-(batmultiplier*1.5)) and robotvoltage <= (highbatvoltage-batmultiplier) and cozmostate==4: | |
| #bottom one light on, second light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=2 and lightstate !=3: | |
| lightstate = 2 | |
| color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1) | |
| light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| light3=cozmo.lights.Light(on_color=color2) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| elif robotvoltage > (highbatvoltage-(batmultiplier*2.5)) and robotvoltage <= (highbatvoltage-(batmultiplier*1.5)) and cozmostate==4: | |
| # # bottom one light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=3: | |
| lightstate = 3 | |
| color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| light2=cozmo.lights.Light(on_color=color2) | |
| light3=cozmo.lights.Light(on_color=color2) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| elif robotvoltage >= lowbatvoltage and cozmostate==4: | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=4: | |
| lightstate = 4 | |
| color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500) | |
| light2=cozmo.lights.Light(on_color=color2) | |
| light3=cozmo.lights.Light(on_color=color2) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| #robot_set_backpacklights(65535) # 65535 is blue | |
| elif robotvoltage >= (maxbatvoltage-(chargebatmultiplier/2.5)) and robotvoltage <= (maxbatvoltage-(chargebatmultiplier/3.5)) and cozmostate==1: | |
| # # bottom one light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=7 and lightstate !=6 and lightstate !=5: | |
| lightstate = 7 | |
| color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| light2=cozmo.lights.Light(on_color=color2) | |
| light3=cozmo.lights.Light(on_color=color2) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| elif robotvoltage >= (maxbatvoltage-(chargebatmultiplier/1.0)) and robotvoltage <= (maxbatvoltage-chargebatmultiplier/2.5) and cozmostate==1: | |
| #bottom one light on, second light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=6 and lightstate !=5: | |
| lightstate = 6 | |
| color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1) | |
| light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| light3=cozmo.lights.Light(on_color=color2) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| elif robotvoltage >= maxbatvoltage and cozmostate==1: | |
| # bottom two lights on, third light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=5: | |
| lightstate = 5 | |
| color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1) | |
| light2=cozmo.lights.Light(on_color=color1) | |
| light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| # elif robotvoltage >= maxbatvoltage and cozmostate==1: | |
| # batlightcounter +=1 | |
| # if batlightcounter > 5 and lightstate !=8: | |
| # lightstate = 8 | |
| # color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None) | |
| # color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| # light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500) | |
| # light2=cozmo.lights.Light(on_color=color2) | |
| # light3=cozmo.lights.Light(on_color=color2) | |
| # robot.set_backpack_lights(None, light3, light2, light1, None) | |
| # batlightcounter = 0 | |
| # pass | |
| #robot_set_backpacklights(4278190335) # 4278190335 is red | |
| elif robotvoltage >= (lowbatvoltage+(critbatmultiplier*1.5)) and robotvoltage <= (lowbatvoltage+critbatmultiplier) and cozmostate==5: | |
| #bottom one light on, second light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=10 and lightstate !=9: | |
| lightstate = 10 | |
| color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1) | |
| light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| light3=cozmo.lights.Light(on_color=color2) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| elif robotvoltage >= (lowbatvoltage+(critbatmultiplier*2.5)) and robotvoltage <= (lowbatvoltage+(critbatmultiplier*1.5)) and cozmostate==5: | |
| # # bottom one light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=11 and lightstate !=10: | |
| lightstate = 11 | |
| color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| light2=cozmo.lights.Light(on_color=color2) | |
| light3=cozmo.lights.Light(on_color=color2) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| elif robotvoltage >= (lowbatvoltage+critbatmultiplier) and cozmostate==5: | |
| # bottom two lights on, third light blinking | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=9: | |
| lightstate = 9 | |
| color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1) | |
| light2=cozmo.lights.Light(on_color=color1) | |
| light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| elif robotvoltage >= lowbatvoltage and cozmostate==5: | |
| batlightcounter +=1 | |
| if batlightcounter > 5 and lightstate !=12: | |
| lightstate = 12 | |
| color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None) | |
| color2=cozmo.lights.Color(int_color=0, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500) | |
| light2=cozmo.lights.Light(on_color=color2) | |
| light3=cozmo.lights.Light(on_color=color2) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| batlightcounter = 0 | |
| pass | |
| if lightstate==0 or lightstate==99: | |
| lightstate=99 | |
| if robot.is_on_charger: | |
| robot_set_backpacklights(65535) # 16711935 is green | |
| else: | |
| if robot.battery_voltage > lowbatvoltage: | |
| robot_set_backpacklights(16711935) # 65535 is blue | |
| else : | |
| robot_set_backpacklights(4278190335) # 4278190335 is red | |
| def robot_set_needslevel(): | |
| global robot, needslevel, msg | |
| needslevel = 1 - (4.05 - robot.battery_voltage) | |
| if needslevel < 0.1: | |
| needslevel = 0.1 | |
| if needslevel > 1: | |
| needslevel = 1 | |
| # i = random.randint(1, 1000) | |
| # if i >= 990: | |
| #robot_print_current_state('updating needs levels') | |
| robot.set_needs_levels(repair_value=needslevel, energy_value=needslevel, play_value=needslevel) | |
| def robot_check_sleep_snoring(): | |
| global robot | |
| i = random.randint(1, 1000) | |
| if i >= 997 and cozmostate == 1 and not robot.is_animating: | |
| robot_print_current_state('check complete - snore') | |
| #robot.play_anim_trigger(Sleeping).wait_for_completed() | |
| robot_reaction_chance(cozmo.anim.Triggers.Sleeping,100,True,False,True) | |
| else: | |
| #robot_print_current_state('check complete - no snore') | |
| time.sleep(0.5) | |
| def robot_check_scheduler(): | |
| global robot,scheduler_playokay,use_cubes,use_scheduler, highbatvoltage | |
| # from 7pm on weekdays | |
| weekdaystartplay = 19 | |
| # to 11pm on weekdays | |
| weekdaystopplay = 23 | |
| # from 7am on weekends | |
| weekendstartplay = 7 | |
| # to 11pm on weekends | |
| weekendstopplay = 23 | |
| # scheduler - when battery is charged this represents the chance cozmo will get off his charger to play | |
| # chance is defined as a number between 1-99 with a higher number representing a lesser chance | |
| playchance = 1 | |
| robot_print_current_state('starting schedule check') | |
| # 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() | |
| scheduler_playokay=0 | |
| #it's weekend! Check for allowed times. | |
| if day_of_week > 4: | |
| if (ctime > datetime.time(weekendstartplay) and ctime < datetime.time(weekendstopplay)): | |
| scheduler_playokay=1 | |
| #it's a weekday! Check for allowed times. | |
| else: | |
| if (ctime > datetime.time(weekdaystartplay) and ctime < datetime.time(weekdaystopplay)): | |
| scheduler_playokay=1 | |
| # are we using the scheduler? | |
| if use_scheduler==0: | |
| scheduler_playokay=1 | |
| # if the schedule says OK roll dice to see if we wake up | |
| if scheduler_playokay==1: | |
| robot_print_current_state('schedule OK - random chance to wake up') | |
| i = random.randint(1, 100) | |
| # wake up chance | |
| if use_scheduler==0: | |
| i = 100 | |
| if i >= playchance: | |
| robot_print_current_state('waking up - leaving charger') | |
| #robot.world.connect_to_cubes() | |
| #robot_set_backpacklights(16711935) # 16711935 is green | |
| try: | |
| robot.play_anim("anim_gotosleep_getout_02").wait_for_completed() | |
| except: | |
| robot_print_current_state('wake up anim failed') | |
| #pass | |
| for _ in range(3): | |
| try: | |
| robot.drive_off_charger_contacts().wait_for_completed() | |
| robot_print_current_state('drive off charger OK') | |
| except: | |
| robot_print_current_state('drive off charger error') | |
| #pass | |
| time.sleep(0.5) | |
| highbatvoltage = robot.battery_voltage | |
| try: | |
| robot.move_lift(-3) | |
| except: | |
| robot_print_current_state('lift reset fail') | |
| #pass | |
| try: | |
| robot.drive_straight(distance_mm(60), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('drive straight fail') | |
| #pass | |
| try: | |
| robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('drive straight fail') | |
| #pass | |
| # we're out of schedule or didn't make the dice roll, back off and check again later. | |
| x = 1 | |
| while x < 20 and (robot.is_on_charger == 1): | |
| if scheduler_playokay == 1: | |
| robot_print_current_state('battery charged - schedule ok - not active') | |
| time.sleep(1) | |
| else: | |
| #print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x)) | |
| robot_print_current_state('battery charged - out of schedule') | |
| time.sleep(1) | |
| robot_check_sleep_snoring() | |
| if (robot.is_on_charger == 0): | |
| robot_print_current_state('cozmo was removed from charger') | |
| break | |
| time.sleep(2) | |
| def robot_check_randomreaction(): | |
| global robot,cozmostate,freeplay | |
| i = random.randint(1, 1000) | |
| #if i >= 980 and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and not robot.is_behavior_running and cozmostate==4: | |
| if i >= 970 and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and cozmostate==4: | |
| oldcozmostate=cozmostate | |
| cozmostate=99 | |
| #random action! | |
| robot_print_current_state('random animation starting') | |
| if robot.is_freeplay_mode_active: | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.stop_freeplay_behaviors() | |
| robot.abort_all_actions(log_abort_messages=True) | |
| #robot.clear_idle_animation() | |
| #robot.wait_for_all_actions_completed() | |
| #robot.wait_for_all_actions_completed() | |
| # grab a list of animation triggers | |
| all_animation_triggers = robot.anim_triggers | |
| # randomly shuffle the animations | |
| random.shuffle(all_animation_triggers) | |
| # select the first animation from the shuffled list | |
| triggers = 1 | |
| chosen_triggers = all_animation_triggers[:triggers] | |
| print('Playing {} random animations:'.format(triggers)) | |
| for trigger in chosen_triggers: | |
| if 'Onboarding' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'MeetCozmo' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'list' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'List' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'Severe' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'TakaTaka' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'Test' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'Loop' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'Sleep' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'Request' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'Singing' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'Drone' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| if 'SoundOnly' in trigger: | |
| trigger = 'cozmo.anim.Triggers.SparkSuccess' | |
| print("trigger %s" %str(trigger)," executed") | |
| robot_print_current_state('playing random animation') | |
| try: | |
| robot.play_anim_trigger(trigger).wait_for_completed() | |
| except: | |
| robot_print_current_state('random animation play failed') | |
| #pass | |
| robot_print_current_state('played random animation') | |
| #robot.wait_for_all_actions_completed() | |
| if freeplay == 1 and not robot.is_freeplay_mode_active: | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| if cozmostate==6: | |
| cozmostate=6 | |
| else: | |
| cozmostate=oldcozmostate | |
| time.sleep(0.5) | |
| def robot_reaction_chance(animation,chance,ignorebody,ignorehead,ignorelift): | |
| global robot, msg, freeplay,cozmostate | |
| i = random.randint(1, 100) | |
| if i >= chance and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and cozmostate !=99: | |
| robot_print_current_state('starting animation') | |
| oldcozmostate=cozmostate | |
| cozmostate=99 | |
| oldfreeplay = 0 | |
| if freeplay == 1: | |
| if robot.is_freeplay_mode_active: | |
| robot_print_current_state('disabling freeplay') | |
| #robot.enable_all_reaction_triggers(True) | |
| oldfreeplay = 1 | |
| freeplay = 0 | |
| robot.stop_freeplay_behaviors() | |
| robot.abort_all_actions(log_abort_messages=True) | |
| robot.stop_all_motors() | |
| robot_print_current_state('action queue aborted') | |
| #robot.clear_idle_animation() | |
| robot.wait_for_all_actions_completed() | |
| try: | |
| robot_print_current_state('playing animation') | |
| robot.play_anim_trigger(animation, ignore_body_track=ignorebody, ignore_head_track=ignorehead, ignore_lift_track=ignorelift).wait_for_completed() | |
| #print("reaction %s" %str(animation)," executed") | |
| msg = ("reaction %s" %str(animation)," executed") | |
| robot_print_current_state('animation completed') | |
| except: | |
| robot_print_current_state('animation failed') | |
| #print("reaction %s" %str(animation)," aborted") | |
| robot.wait_for_all_actions_completed() | |
| try: | |
| robot_print_current_state('resetting head angle') | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| robot_print_current_state('head angle reset failed') | |
| #print("reaction %s" %str(animation)," aborted") | |
| #robot.wait_for_all_actions_completed() | |
| robot.wait_for_all_actions_completed() | |
| try: | |
| robot_print_current_state('resetting lift') | |
| robot.move_lift(-3) | |
| except: | |
| robot_print_current_state('lift move down failed') | |
| #print("reaction %s" %str(animation)," aborted") | |
| robot.wait_for_all_actions_completed() | |
| if oldfreeplay == 1: | |
| oldfreeplay = 0 | |
| freeplay = 1 | |
| robot_print_current_state('re-enabling freeplay') | |
| if not robot.is_freeplay_mode_active: | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| if cozmostate == 6: | |
| cozmostate =6 | |
| elif cozmostate == 1: | |
| cozmostate = 1 | |
| else: | |
| cozmostate=oldcozmostate | |
| else: | |
| time.sleep(0.5) | |
| robot_print_current_state('animation check - no winner') | |
| def robot_locate_dock(): | |
| global robot,cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger, tempfreeplay | |
| #back off from whatever we were doing | |
| #robot_set_backpacklights(4278190335) # 4278190335 is red | |
| if robot.is_freeplay_mode_active: | |
| robot_print_current_state('disabling freeplay') | |
| robot.stop_freeplay_behaviors() | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.abort_all_actions(log_abort_messages=True) | |
| robot_print_current_state('starting locate dock sequence') | |
| robot.clear_idle_animation() | |
| #robot.wait_for_all_actions_completed() | |
| if use_cubes==1: | |
| robot.world.disconnect_from_cubes() | |
| freeplay = 0 | |
| robot_reaction_chance(cozmo.anim.Triggers.NeedsMildLowEnergyRequest,1,False,False,False) | |
| try: | |
| robot.drive_straight(distance_mm(-30), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('drive straight failed') | |
| robot_set_needslevel() | |
| robot_print_current_state('finding charger') | |
| # charger location search | |
| if robot.world.charger and cozmostate != 1 and cozmostate != 2 and cozmostate !=6: | |
| if robot.world.charger.pose.is_comparable(robot.pose): | |
| charger = robot.world.charger | |
| #we know where the charger is | |
| robot_print_current_state('finding charger, charger position known') | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,False) | |
| time.sleep(0.5) | |
| cozmostate = 6 | |
| foundcharger = 1 | |
| else: | |
| robot_print_current_state('finding charger, charger not in expected location') | |
| charger = None | |
| robot.world.charger = None | |
| if cozmostate != 1 and cozmostate != 2: | |
| cozmostate = 5 | |
| charger = robot.world.charger | |
| if not charger and cozmostate != 1 and cozmostate != 2 and cozmostate !=6: | |
| robot_print_current_state('looking for charger') | |
| cozmostate = 5 | |
| robot_reaction_chance(cozmo.anim.Triggers.SparkIdle,30,True,False,True) | |
| try: | |
| robot.move_lift(-3) | |
| except: | |
| robot_print_current_state('failed to move lift') | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to set head angle') | |
| try: | |
| robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to drive') | |
| # randomly drive around for a bit and see if we can spot the charger | |
| robot_drive_random_pattern() | |
| robot_print_current_state('looking for charger, random drive loop complete') | |
| time.sleep(0.5) | |
| def robot_drive_random_pattern(): | |
| global cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger, chargermarker1 | |
| loops=5 | |
| while loops>0 and cozmostate == 5: | |
| if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose): | |
| loops=0 | |
| charger = robot.world.charger | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,True) | |
| robot_print_current_state('found charger, breaking loop') | |
| cozmostate = 6 | |
| foundcharger = 1 | |
| break | |
| if cozmostate == 6 or cozmostate ==1 or cozmostate == 2: | |
| robot_print_current_state('breaking out of drive loop') | |
| loops=0 | |
| break | |
| # drive to a random point and orientation | |
| counter=0 | |
| while counter < 1 and cozmostate ==5 and cozmostate !=6: | |
| if cozmostate == 6 or cozmostate ==1 or cozmostate == 2: | |
| robot_print_current_state('breaking out of drive loop') | |
| loops=0 | |
| break | |
| if random.choice((True, False)): | |
| x=150 | |
| else: | |
| x=-150 | |
| if random.choice((True, False)): | |
| y=150 | |
| else: | |
| y=-150 | |
| z= random.randrange(-40, 41, 80) | |
| robot_print_current_state('looking for charger, going to random pose') | |
| try: | |
| robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(z)), relative_to_robot=True).wait_for_completed() | |
| robot.set_head_light(False) | |
| time.sleep(0.25) | |
| robot.set_head_light(True) | |
| time.sleep(0.25) | |
| robot.set_head_light(False) | |
| except: | |
| robot_print_current_state('failed to go to pose') | |
| if cozmostate == 6 or cozmostate ==1 or cozmostate == 2: | |
| loops=0 | |
| break | |
| if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose): | |
| loops=0 | |
| charger = robot.world.charger | |
| cozmostate = 6 | |
| foundcharger = 1 | |
| robot_print_current_state('found charger, breaking') | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,True) | |
| break | |
| # else: | |
| robot_check_randomreaction() | |
| counter+=1 | |
| if cozmostate == 6 or cozmostate ==1 or cozmostate == 2: | |
| loops=0 | |
| break | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,1,True,False,True) | |
| # turn around for a bit | |
| time.sleep(0.5) | |
| counter=0 | |
| oldcozmostate = cozmostate | |
| cozmostate = 98 | |
| robot_print_current_state('start lookaround behavior') | |
| robot.set_head_angle(degrees(20)).wait_for_completed() | |
| # if chargermarker1.pose.is_comparable(robot.pose): | |
| # action = robot.go_to_pose(pose=chargermarker1.pose) | |
| # action.wait_for_completed() | |
| # else: | |
| look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) | |
| try: | |
| custom_objects = robot.world.wait_until_observe_num_objects(num=1, object_type = CustomObject, timeout=15) | |
| robot_print_current_state('custom object found') | |
| except asyncio.TimeoutError: | |
| robot_print_current_state('lookaround search timeout') | |
| finally: | |
| robot_print_current_state('stop lookaround') | |
| look_around.stop() | |
| if len(custom_objects) > 0: | |
| robot_print_current_state('custom object in array') | |
| found_object = custom_objects[0] | |
| if str(found_object.object_type) == "CustomObjectTypes.CustomType01": | |
| robot_print_current_state('custom object found, traveling') | |
| action = robot.go_to_pose(pose=found_object.pose) | |
| action.wait_for_completed() | |
| robot.set_head_angle(degrees(20)).wait_for_completed() | |
| robot.drive_straight(distance_mm(-80), speed_mmps(50)).wait_for_completed() | |
| robot.drive_straight(distance_mm(-80), speed_mmps(50)).wait_for_completed() | |
| else: | |
| #print("no object to drive to") | |
| robot_print_current_state('custom object not in array') | |
| #pass | |
| # try: | |
| # charger = robot.world.wait_for_observed_charger(timeout=8) | |
| # #print("Found charger: %s" % charger) | |
| # robot_print_current_state('found charger, breaking') | |
| # cozmostate=6 | |
| # except asyncio.TimeoutError: | |
| # #print("Didn't see the charger") | |
| # robot_print_current_state('charger not found') | |
| # cozmostate = 5 | |
| # finally: | |
| # # whether we find it or not, we want to stop the behavior | |
| # #robot_print_current_state('lookaround behavior ending') | |
| # look_around.stop() | |
| #cozmostate = oldcozmostate | |
| if cozmostate == 6 or cozmostate ==1 or cozmostate == 2: | |
| break | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,1,True,False,True) | |
| robot_print_current_state('ended lookaround behavior') | |
| # while counter <2 and cozmostate == 5: | |
| # a= random.randrange(8, 17, 8) | |
| # t= random.randrange(2, 4, 1) | |
| # if random.choice((True, False)): | |
| # rx=50 | |
| # else: | |
| # rx=-50 | |
| # ry=-rx | |
| # robot_print_current_state('looking for charger, rotating') | |
| # try: | |
| # robot.set_head_light(False) | |
| # time.sleep(0.2) | |
| # robot.set_head_light(True) | |
| # time.sleep(0.2) | |
| # robot.set_head_light(False) | |
| # robot.drive_wheels(rx, ry, l_wheel_acc=a, r_wheel_acc=a, duration=t) | |
| # time.sleep(0.5) | |
| # except: | |
| # pass | |
| # if cozmostate == 6: | |
| # break | |
| if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose): | |
| loops=0 | |
| charger = robot.world.charger | |
| robot_print_current_state('found charger') | |
| foundcharger = 1 | |
| robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,True) | |
| break | |
| # else: | |
| robot_check_randomreaction() | |
| # counter+=1 | |
| # if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose): | |
| # loops=0 | |
| # charger = robot.world.charger | |
| # cozmostate = 6 | |
| # foundcharger = 1 | |
| # robot_print_current_state('found charger') | |
| # robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,False,False,False) | |
| # break | |
| #robot_set_needslevel() | |
| robot_print_current_state('looking for charger, looping through random poses') | |
| loops=loops-1 | |
| if cozmostate == 6: | |
| cozmostate = 6 | |
| else: | |
| cozmostate = oldcozmostate | |
| if cozmostate == 6: | |
| cozmostate = 6 | |
| elif cozmostate == 1: | |
| cozmostate = 1 | |
| elif cozmostate == 2: | |
| cozmostate = 2 | |
| else: | |
| cozmostate = oldcozmostate | |
| robot_print_current_state('looking for charger, broke out of drive loop') | |
| #return charger | |
| def robot_start_docking(): | |
| global robot,cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger | |
| charger = robot.world.charger | |
| #action = robot.go_to_object(charger, distance_mm(65.0)) | |
| #action.wait_for_completed() | |
| robot_print_current_state('go to object complete') | |
| if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose): | |
| action = robot.go_to_pose(robot.world.charger.pose) | |
| action.wait_for_completed() | |
| robot_print_current_state('go to pose complete') | |
| robot.drive_straight(distance_mm(-50), speed_mmps(50)).wait_for_completed() | |
| robot_print_current_state('drove back a little bit') | |
| else: | |
| robot_print_current_state('charger pose not known') | |
| robot.world.charger = None | |
| charger = None | |
| if cozmostate != 1 and cozmostate != 2: | |
| cozmostate = 5 | |
| 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: | |
| robot_print_current_state('failed to play anim') | |
| robot_print_current_state('charger not found, clearing map') | |
| if cozmostate != 1 and cozmostate != 2: | |
| cozmostate = 5 | |
| dockloop = 0 | |
| while dockloop < 2 and cozmostate == 6 and robot.world.charger: | |
| try: | |
| action = robot.go_to_pose(robot.world.charger.pose) | |
| action.wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to go to pose') | |
| robot_print_current_state('I should be in front of the charger') | |
| 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) | |
| try: | |
| robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to drive') | |
| robot.set_head_light(False) | |
| time.sleep(0.5) | |
| robot.set_head_light(True) | |
| time.sleep(0.5) | |
| robot.set_head_light(False) | |
| try: | |
| action = robot.go_to_pose(robot.world.charger.pose) | |
| action.wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to go to pose') | |
| try: | |
| robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to back up a little bit') | |
| charger = robot.world.charger | |
| robot_reaction_chance(cozmo.anim.Triggers.FeedingReactToShake_Normal,85,True,False,False) | |
| robot_print_current_state('docking') | |
| try: | |
| robot.turn_in_place(degrees(95)).wait_for_completed() | |
| robot.turn_in_place(degrees(95)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to do a 180') | |
| time.sleep(0.5) | |
| robot_reaction_chance(cozmo.anim.Triggers.CubePounceFake,1,True,False,False) | |
| try: | |
| robot.drive_straight(distance_mm(-147), speed_mmps(150)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to drive onto charger') | |
| time.sleep(0.5) | |
| # check if we're now docked | |
| if robot.is_on_charger: | |
| robot_reaction_chance(cozmo.anim.Triggers.SparkSuccess,1,True,False,True) | |
| dockloop = 3 | |
| cozmostate = 1 | |
| break | |
| # No, we missed. Back off and try again | |
| robot_print_current_state('failed to dock') | |
| robot_reaction_chance(cozmo.anim.Triggers.AskToBeRightedRight,1,True,False,False) | |
| try: | |
| robot.move_lift(-3) | |
| except: | |
| robot_print_current_state('failed to move lift') | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to set head angle') | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| robot_print_current_state('failed to dock, retrying') | |
| try: | |
| robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to drive') | |
| try: | |
| robot.turn_in_place(degrees(-3)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to turn') | |
| try: | |
| robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to drive') | |
| try: | |
| robot.turn_in_place(degrees(94)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to turn') | |
| try: | |
| robot.turn_in_place(degrees(94)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to turn') | |
| try: | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to set head angle') | |
| time.sleep(0.5) | |
| dockloop+=1 | |
| # exited loop, check current state: | |
| if cozmostate != 1 and cozmostate != 2: | |
| cozmostate = 5 | |
| charger= None | |
| robot.world.charger=None | |
| # express frustration | |
| try: | |
| robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to drive') | |
| try: | |
| robot.turn_in_place(degrees(-3)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to turn') | |
| try: | |
| robot.drive_straight(distance_mm(80), speed_mmps(50)).wait_for_completed() | |
| except: | |
| robot_print_current_state('failed to drive') | |
| robot_drive_random_pattern() | |
| robot_reaction_chance(cozmo.anim.Triggers.MemoryMatchPlayerWinGame,1,True,False,False) | |
| x=0 | |
| while x<11 and cozmostate == 5: | |
| tempfreeplay = 1 | |
| if freeplay==0: | |
| freeplay = 1 | |
| robot_print_current_state('charger not found, falling back to freeplay') | |
| #robot_set_backpacklights(16711935) # green | |
| if use_cubes==1: | |
| robot.world.connect_to_cubes() | |
| if not robot.is_freeplay_mode_active: | |
| robot_print_current_state('freeplay enabled') | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| if cozmostate != 5: | |
| break | |
| robot_print_current_state('charger not found, falling back to freeplay') | |
| time.sleep(1) | |
| if robot.world.charger: | |
| robot_print_current_state('found charger while in temporary freeplay') | |
| charger = robot.world.charger | |
| cozmostate = 6 | |
| break | |
| time.sleep(5) | |
| x+=1 | |
| #after time expires or spotting the charger end freeplay | |
| tempfreeplay = 0 | |
| if robot.is_freeplay_mode_active: | |
| #robot.enable_all_reaction_triggers(True) | |
| robot.stop_freeplay_behaviors() | |
| if use_cubes==1: | |
| robot.world.disconnect_from_cubes() | |
| #robot_set_backpacklights(4278190335) # red | |
| freeplay = 0 | |
| if cozmostate != 1: | |
| cozmostate = 5 | |
| #os.system('cls' if os.name == 'nt' else 'clear') | |
| robot_print_current_state('temporary freeplay ended') | |
| time.sleep(1) | |
| # | |
| # END OF ROBOT FUNCTIONS | |
| # | |
| # | |
| # EVENT MONITOR FUNCTIONS | |
| # | |
| class CheckState (threading.Thread): | |
| global robot,cozmostate,freeplay,msg,camera,objmsg,facemsg | |
| def __init__(self, thread_id, name, _q): | |
| threading.Thread.__init__(self) | |
| self.threadID = thread_id | |
| self.name = name | |
| self.q = _q | |
| # main thread | |
| def run(self): | |
| global robot,cozmostate,freeplay,msg,camera,highbatvoltage,lowbatvoltage,lightstate | |
| delay = 10 | |
| is_picked_up = False | |
| is_falling = False | |
| is_on_charger = False | |
| is_cliff_detected = False | |
| is_moving = False | |
| is_carrying_block = False | |
| is_localized = False | |
| is_picking_or_placing = False | |
| is_pathing = False | |
| is_behavior_running = False | |
| while thread_running: | |
| # event monitor: robot is picked up detection | |
| #robot_backbackbatteryindicator() | |
| if robot.is_picked_up: | |
| delay = 0 | |
| if not is_picked_up: | |
| is_picked_up = True | |
| robot_flash_backpacklights(4278190335) # 4278190335 is red | |
| cozmostate = 9 | |
| robot_print_current_state('switching to state 9 - picked up') | |
| lightstate=0 | |
| elif is_picked_up and delay > 9: | |
| cozmostate = 0 | |
| lightstate=0 | |
| is_picked_up = False | |
| robot_print_current_state('no longer picked up - state 0') | |
| elif delay <= 9: | |
| delay += 1 | |
| # event monitor: robot is carrying a block | |
| if robot.is_carrying_block: | |
| if not is_carrying_block: | |
| is_carrying_block = True | |
| robot_print_current_state('cozmo.robot.Robot.is_carrying_block: True') | |
| elif not robot.is_carrying_block: | |
| if is_carrying_block: | |
| is_carrying_block = False | |
| robot_print_current_state('cozmo.robot.Robot.is_carrying_block: False') | |
| # event monitor: robot is localized (I don't think this is working right now) | |
| # if robot.is_localized: | |
| # if not is_localized: | |
| # is_localized = True | |
| # robot_print_current_state('cozmo.robot.Robot.is_localized: True') | |
| # elif not robot.is_localized: | |
| # if is_localized: | |
| # is_localized = False | |
| # robot_print_current_state('cozmo.robot.Robot.is_localized: False') | |
| # event monitor: robot is falling | |
| if robot.is_falling: | |
| if not is_falling: | |
| is_falling = True | |
| robot.stop_all_motors() | |
| cozmostate = 9 | |
| robot_print_current_state('Switching to state 9 - Falling!') | |
| lightstate=0 | |
| elif not robot.is_falling: | |
| if is_falling: | |
| is_falling = False | |
| cozmostate = 0 | |
| lightstate=0 | |
| robot_print_current_state('no longer falling switching to state 0') | |
| # event monitor: robot moves onto charger | |
| if robot.is_on_charger: | |
| if not is_on_charger: | |
| is_on_charger = True | |
| #freeplay = 0 | |
| cozmostate = 1 | |
| # if robot.is_freeplay_mode_active: | |
| # #robot.enable_all_reaction_triggers(True) | |
| # robot.stop_freeplay_behaviors() | |
| # robot.abort_all_actions(log_abort_messages=True) | |
| # robot.wait_for_all_actions_completed() | |
| # robot.clear_idle_animation() | |
| # robot.stop_all_motors() | |
| robot_print_current_state('moved onto the charger') | |
| color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1) | |
| light2=cozmo.lights.Light(on_color=color1) | |
| light3=cozmo.lights.Light(on_color=color1) | |
| robot.set_backpack_lights(None, light3, light2, light1, None) | |
| # robot_set_backpacklights(65535) # 65535 is blue | |
| lightstate = 0 | |
| # #robot.play_anim_trigger(cozmo.anim.Triggers.Sleeping, loop_count=1, in_parallel=True, num_retries=0, ignore_body_track=True, ignore_head_track=False, ignore_lift_track=True).wait_for_completed() | |
| # try: | |
| # robot.play_anim_trigger(cozmo.anim.Triggers.GoToSleepGetIn).wait_for_completed() | |
| # except: | |
| # pass | |
| #robot_set_backpacklights(65535) # blue | |
| # if cozmostate==6: | |
| # try: | |
| # #robot.play_anim("anim_sparking_success_02").wait_for_completed() | |
| # robot_reaction_chance(cozmo.anim.Triggers.SparkSuccess,1,True,False,False) | |
| # except: | |
| # pass | |
| # try: | |
| # robot.set_head_angle(degrees(0)).wait_for_completed() | |
| # except: | |
| # pass | |
| # robot_print_current_state('docked') | |
| # try: | |
| # #robot.play_anim("anim_gotosleep_getin_01").wait_for_completed() | |
| # robot_reaction_chance(cozmo.anim.Triggers.GoToSleepGetIn,1,True,False,False) | |
| # except: | |
| # pass | |
| # try: | |
| # robot_reaction_chance(cozmo.anim.Triggers.CodeLabSleep,1,True,False,False) | |
| # #robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed() | |
| # except: | |
| # pass | |
| #robot.play_anim_trigger(cozmo.anim.Triggers.StartSleeping, loop_count=1, in_parallel=True, num_retries=0, ignore_body_track=True, ignore_head_track=False, ignore_lift_track=True).wait_for_completed() | |
| if robot.is_charging: | |
| cozmostate = 1 | |
| robot_print_current_state('switching to state 1') | |
| else: | |
| cozmostate = 2 | |
| maxbatvoltage = robot.battery_voltage | |
| robot_print_current_state('on charger, not charging') | |
| #print(msg) | |
| elif not robot.is_on_charger: | |
| if is_on_charger: | |
| #robot_set_backpacklights(16711935) # 16711935 is green | |
| color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None) | |
| light1=cozmo.lights.Light(on_color=color1) | |
| light2=cozmo.lights.Light(on_color=color1) | |
| light3=cozmo.lights.Light(on_color=color1) | |
| is_on_charger = False | |
| cozmostate = 0 | |
| robot_print_current_state('switching to state 0 - moved off charger') | |
| #print(msg) | |
| # event monitor: robot has detected cliff | |
| if robot.is_cliff_detected and not robot.is_falling and not robot.is_picked_up and cozmostate != 6: | |
| if not is_cliff_detected: | |
| robot.stop_all_motors() | |
| is_cliff_detected = True | |
| wasinfreeplay = 0 | |
| robot_print_current_state('cliff detected') | |
| #print(msg) | |
| if freeplay == 1: | |
| freeplay = 0 | |
| wasinfreeplay = 1 | |
| if robot.is_freeplay_mode_active: | |
| ##robot.enable_all_reaction_triggers(True) | |
| robot.stop_freeplay_behaviors() | |
| #robot.wait_for_all_actions_completed() | |
| robot.abort_all_actions(log_abort_messages=True) | |
| robot.clear_idle_animation() | |
| try: | |
| robot.drive_wheels(-40, -40, l_wheel_acc=30, r_wheel_acc=30, duration=1.5) | |
| except: | |
| robot_print_current_state('failed to drive') | |
| try: | |
| robot.drive_wheels(-40, -40, l_wheel_acc=30, r_wheel_acc=30, duration=1.5) | |
| except: | |
| robot_print_current_state('failed to drive') | |
| is_cliff_detected = False | |
| robot_print_current_state('cliff no longer detected') | |
| #print(msg) | |
| elif not robot.is_cliff_detected: | |
| if is_cliff_detected: | |
| robot_print_current_state('switching from cliff mode') | |
| is_cliff_detected = False | |
| if wasinfreeplay == 1: | |
| freeplay = 1 | |
| wasinfreeplay = 0 | |
| if robot.is_freeplay_mode_active: | |
| ##robot.enable_all_reaction_triggers(True) | |
| robot_print_current_state('re-enabling freeplay') | |
| robot.start_freeplay_behaviors() | |
| # event monitor: robot is picking or placing something | |
| if robot.is_picking_or_placing: | |
| if not is_picking_or_placing: | |
| is_picking_or_placing = True | |
| msg = 'cozmo.robot.Robot.is_picking_or_placing: True' | |
| #print(msg) | |
| robot_print_current_state('Robot.is_picking_or_placing: True') | |
| elif not robot.is_picking_or_placing: | |
| if is_picking_or_placing: | |
| is_picking_or_placing = False | |
| msg = 'cozmo.robot.Robot.is_picking_or_placing: False' | |
| robot_print_current_state('Robot.is_picking_or_placing: False') | |
| #print(msg) | |
| # event monitor: robot is pathing (traveling to a target) | |
| if robot.is_pathing: | |
| if not is_pathing: | |
| is_pathing = True | |
| msg = 'cozmo.robot.Robot.is_pathing: True' | |
| #print(msg) | |
| robot_print_current_state('Robot.is_pathing: True') | |
| elif not robot.is_pathing: | |
| if is_pathing: | |
| is_pathing = False | |
| msg = 'cozmo.robot.Robot.is_pathing: False' | |
| robot_print_current_state('Robot.is_pathing: False') | |
| #print(msg) | |
| # event monitor (behavior is running) | |
| if robot.is_behavior_running: | |
| if not is_behavior_running: | |
| is_behavior_running = True | |
| msg = 'cozmo.robot.Robot.is_behavior_running: True' | |
| robot_print_current_state('Robot.is_behavior_running: True') | |
| elif not robot.is_behavior_running: | |
| if is_behavior_running: | |
| is_behavior_running = False | |
| msg = 'cozmo.robot.Robot.is_behavior_running: False' | |
| robot_print_current_state('Robot.is_behavior_running: False') | |
| # event monitor: robot is moving | |
| # too spammy/unreliable | |
| # if robot.is_moving: | |
| # if not is_moving: | |
| # is_moving = True | |
| # robot_print_current_state('Robot.is_moving: True') | |
| # elif not robot.is_moving: | |
| # if is_moving: | |
| # is_moving = False | |
| # robot_print_current_state('Robot.is_moving: False') | |
| # end of detection loop | |
| # # camera IR control | |
| # if camera.exposure_ms < 60: | |
| # # robot.say_text("light").wait_for_completed() | |
| # robot.set_head_light(False) | |
| # elif camera.exposure_ms >= 60: | |
| # # robot.say_text("dark").wait_for_completed() | |
| # robot.set_head_light(True) | |
| time.sleep(0.1) | |
| def print_prefix(evt): | |
| msg = evt.event_name + ' ' | |
| return msg | |
| def print_object(obj): | |
| if isinstance(obj,cozmo.objects.LightCube): | |
| cube_id = next(k for k,v in robot.world.light_cubes.items() if v==obj) | |
| msg = 'LightCube-' + str(cube_id) | |
| else: | |
| r = re.search('<(\w*)', obj.__repr__()) | |
| msg = r.group(1) | |
| return msg | |
| def monitor_generic(evt, **kwargs): | |
| global robot,cozmostate,freeplay,msg,camera,objmsg,facemsg | |
| msg = print_prefix(evt) | |
| if 'behavior' in kwargs or 'behavior_type_name' in kwargs: | |
| msg += kwargs['behavior_type_name'] + ' ' | |
| msg += kwargs['behavior'] + ' ' | |
| elif 'obj' in kwargs: | |
| msg += print_object(kwargs['obj']) + ' ' | |
| elif 'action' in kwargs: | |
| action = kwargs['action'] | |
| if isinstance(action, cozmo.anim.Animation): | |
| msg += action.anim_name + ' ' | |
| elif isinstance(action, cozmo.anim.AnimationTrigger): | |
| msg += action.trigger.name + ' ' | |
| else: | |
| msg += str(set(kwargs.keys())) | |
| robot_print_current_state('generic monitor data incoming') | |
| # | |
| # event monitor: robot is experiencing unexpected movement | |
| # | |
| def monitor_EvtUnexpectedMovement(evt, **kwargs): | |
| global robot,cozmostate,freeplay,msg,camera | |
| msg = 'Unexpected Movement' | |
| robot_print_current_state('unexpected movement') | |
| #print(msg) | |
| if cozmostate != 3 and cozmostate !=9 and cozmostate !=6: | |
| robot_print_current_state('unexpected behavior during action; aborting') | |
| #print("unexpected behavior during action; aborting") | |
| robot.abort_all_actions(log_abort_messages=True) | |
| robot.stop_all_motors() | |
| robot.wait_for_all_actions_completed() | |
| #print("unexpected behavior during action; aborting") | |
| robot_print_current_state('unexpected behavior during action; aborting') | |
| # | |
| # event monitor: an object was tapped | |
| # | |
| def monitor_EvtObjectTapped(evt, *, obj, tap_count, tap_duration, tap_intensity, **kwargs): | |
| msg = print_prefix(evt) | |
| msg += print_object(obj) | |
| msg += ' count=' + str(tap_count) + ' duration=' + str(tap_duration) + ' intensity=' + str(tap_intensity) | |
| robot_print_current_state('object tapped') | |
| # | |
| # event monitor: a face was detected | |
| # | |
| def monitor_face(evt, face, **kwargs): | |
| msg = print_prefix(evt) | |
| name = face.name if face.name is not '' else '[unknown face]' | |
| expression = face.expression if face.expression is not '' else '[unknown expression]' | |
| msg += name + ' face_id=' + str(face.face_id) + ' looking ' + str(face.expression) | |
| #print(msg) | |
| robot_print_current_state('face module') | |
| # | |
| # event monitor: an object started moving | |
| # | |
| def monitor_EvtObjectMovingStarted(evt, *, obj, acceleration, **kwargs): | |
| msg = print_prefix(evt) | |
| msg += print_object(obj) + ' ' | |
| msg += ' accleration=' + str(acceleration) | |
| robot_print_current_state('object started moving') | |
| # | |
| # event monitor: an object stopped moving | |
| # | |
| def monitor_EvtObjectMovingStopped(evt, *, obj, move_duration, **kwargs): | |
| msg = print_prefix(evt) | |
| msg += print_object(obj) + ' ' | |
| msg += ' move_duration ' + str(move_duration) + 'secs' | |
| #print(msg) | |
| robot_print_current_state('object stopped moving') | |
| # | |
| # event monitor: an object appeared in our vision | |
| # | |
| def monitor_EvtObjectAppeared(evt, **kwargs): | |
| global cozmostate,robot,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger,bhvmsg,facemsg,objmsg | |
| msg = print_prefix(evt) | |
| msg += print_object(kwargs['obj']) + ' ' | |
| if print_object(kwargs['obj']) == "Charger": | |
| charger = robot.world.charger | |
| if print_object(kwargs['obj']) == "Charger" and (cozmostate == 5 or cozmostate == 98): | |
| robot_print_current_state('FOUND THE CHARGER') | |
| #print("it's the charger and we're looking for it!") | |
| cozmostate = 6 | |
| charger = robot.world.charger | |
| robot_print_current_state('object appeared') | |
| dispatch_table = { | |
| cozmo.objects.EvtObjectTapped : monitor_EvtObjectTapped, | |
| cozmo.objects.EvtObjectMovingStarted : monitor_EvtObjectMovingStarted, | |
| cozmo.objects.EvtObjectMovingStopped : monitor_EvtObjectMovingStopped, | |
| cozmo.objects.EvtObjectAppeared : monitor_EvtObjectAppeared, | |
| cozmo.faces.EvtFaceAppeared : monitor_face, | |
| cozmo.faces.EvtFaceDisappeared : monitor_face, | |
| cozmo.robot.EvtUnexpectedMovement : monitor_EvtUnexpectedMovement, | |
| } | |
| excluded_events = { # Occur too frequently to monitor by default | |
| cozmo.behavior.EvtBehaviorRequested, | |
| cozmo.objects.EvtObjectDisappeared, | |
| cozmo.faces.EvtFaceObserved, | |
| cozmo.objects.EvtObjectObserved | |
| } | |
| def monitor(_robot, _q, evt_class=None): | |
| if not isinstance(_robot, cozmo.robot.Robot): | |
| raise TypeError('First argument must be a Robot instance') | |
| if evt_class is not None and not issubclass(evt_class, cozmo.event.Event): | |
| raise TypeError('Second argument must be an Event subclass') | |
| global robot | |
| global q | |
| global thread_running | |
| robot = _robot | |
| q = _q | |
| thread_running = True | |
| if evt_class in dispatch_table: | |
| robot.world.add_event_handler(evt_class,dispatch_table[evt_class]) | |
| elif evt_class is not None: | |
| robot.world.add_event_handler(evt_class,monitor_generic) | |
| else: | |
| for k,v in dispatch_table.items(): | |
| if k not in excluded_events: | |
| robot.world.add_event_handler(k,v) | |
| thread_is_state_changed = CheckState(1, 'ThreadCheckState', q) | |
| thread_is_state_changed.start() | |
| def unmonitor(_robot, evt_class=None): | |
| if not isinstance(_robot, cozmo.robot.Robot): | |
| raise TypeError('First argument must be a Robot instance') | |
| if evt_class is not None and not issubclass(evt_class, cozmo.event.Event): | |
| raise TypeError('Second argument must be an Event subclass') | |
| global robot | |
| global thread_running | |
| robot = _robot | |
| thread_running = False | |
| try: | |
| if evt_class in dispatch_table: | |
| robot.world.remove_event_handler(evt_class,dispatch_table[evt_class]) | |
| elif evt_class is not None: | |
| robot.world.remove_event_handler(evt_class,monitor_generic) | |
| else: | |
| for k,v in dispatch_table.items(): | |
| robot.world.remove_event_handler(k,v) | |
| except Exception: | |
| pass | |
| def robot_print_current_state(currentstate): | |
| global robot,needslevel,start_time,cozmostate,msg,highbatvoltage,maxbatvoltage,objmsg,facemsg,bhvmsg,lightstate,batlightcounter,debugging, batcounter | |
| if not batcounter: | |
| batcounter = 0 | |
| if batcounter > 5: | |
| robot_backbackbatteryindicator() | |
| batcounter = 0 | |
| batcounter += 1 | |
| robot_set_needslevel() | |
| currentbehavior = robot.current_behavior | |
| if currentbehavior == None and cozmostate == 4: | |
| currentbehavior = 'freeplay' | |
| runtime = round(((time.time() - start_time)/60),2) | |
| "{:.2f}".format(runtime) | |
| if debugging==1: | |
| #logging.warn('istate %s' % cozmostate,'| battery %s' % str(round(robot.battery_voltage, 2)),'| energy %s' % round(needslevel, 2),'| anim %s' % str(robot.is_animating),'| behav %s' % str(robot.is_behavior_running),'| bkpk %s' % lightstate,'| state: %s' % str(currentstate),'| msg: %s' % str(msg)) | |
| #print("state %s" % cozmostate,"| battery %s" % str(round(robot.battery_voltage, 2)),"| energy %s" % round(needslevel, 2),"| anim %s" % str(robot.is_animating),"| behav %s" % str(robot.is_behavior_running),"| bkpk %s" % lightstate,"| action: %s" % str(currentstate),"| msg: %s" % str(msg),"accel: %s" % str(robot.accelerometer)) | |
| print("state %s" % cozmostate,"| battery %s" % str(round(robot.battery_voltage, 2)),"| energy %s" % round(needslevel, 2),"| anim %s" % str(robot.is_animating),"| behav %s" % str(robot.is_behavior_running),"| bkpk %s" % lightstate,"| action: %s" % str(currentstate),"| msg: %s" % str(msg),"curbehav: %s" % str(currentbehavior)) | |
| else: | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| # | |
| # commented out thingies either didn't do what I expected or didn't work | |
| # | |
| #print("cozmo sees : %s" % str(robot.world.connected_light_cubes)) | |
| #print("wheelie : %s" % str(robot.PopAWheelie)) | |
| #print("Cubes connected: %s" % robot.world.World.active_behavior.connected_light_cubes) | |
| #print("Behavior : %s" % str(cozmo.behavior.Behavior)) | |
| #print("Max Battery : %s" % str(round(maxbatvoltage, 2))) | |
| #print("Max off charger: %s" % str(round(highbatvoltage, 2))) | |
| #print("idle anim : %s" % str(robot.is_animating_idle)) | |
| #print("actions : %s" % str(robot.has_in_progress_actions)) | |
| # print("Object log : %s" %objmsg) | |
| # print("Face log : %s" %facemsg) | |
| # print("Behavior log : %s" %bhvmsg) | |
| print("State : %s" % str(currentstate)) | |
| print("Internal state : %s" % str(cozmostate)) | |
| print("Battery : %s" % (str(round(robot.battery_voltage, 2)))) | |
| print("Energy : %s" % (round(needslevel, 2))) | |
| print("Runtime : %s" % str(runtime)) | |
| print("running behav : %s" % (str(robot.is_behavior_running))) | |
| print("animating : %s" % (str(robot.is_animating))) | |
| print("Event log : %s" % str(msg)) | |
| print("Lightstate : %s" % str(lightstate)) | |
| # | |
| # END OF EVENT MONITOR FUNCTIONS | |
| # | |
| # START THE SHOW! | |
| # | |
| cozmo.robot.Robot.drive_off_charger_on_connect = False | |
| # | |
| #uncomment the below line to load the viewer | |
| #cozmo.run_program(cozmo_unleashed, use_viewer=True) | |
| # | |
| # you may need to install a freeglut library, the cozmo SDK has documentation for this. If you don't have it comment the below line and uncomment the one above. | |
| 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 | |
| # | |
| # below is just the program running without any camera view or 3d maps | |
| #cozmo.run_program(cozmo_unleashed) |
Oh, and I guess at some point I built some "walls" with markers I printed out, in order to make a little cozmo corral. It worked okay-ish.
stripped out a bunch of code, added "needs" modifiers to decrease cozmo's mood as his battery levels drop. Created variables for the scheduler for easy config.
cleaned up some code and fixed several bugs. Also introduced new and better bugs.
did some more work on sanitizing and clearly defining the different states Cozmo can be in, and any actions that Cozmo should take as a result of being in a specific state.
state guards, state guards everywhere. And still cozmo finds way to end up in unexpected states.
Hi, I cannot make it go to the charger. It finds it and tries to go for it but with his front side, so, he can never get inside of it. I tried to print the Hexagons2 marker (as a substitute of the charger icon or not hiding the icon and in top of it) but not working. Is there another reason why?, the program evidently detects the charger, it even makes a diagram of it in 3D, but I can't get cozmo inside by his own
Hello. I love your script. Fantastic job. I made a few changes of my own as we all do. Nothing major. Except one thing, that I am thinking is an error of random.randrang number selection.
Line 958
old: z= random.randrange(-40, 41, 80)
new: z= random.randrange(-40, 41, 5)
When I looked it up so I was not guessing at what you were trying to do, I think I realized that the reason Cozmo kept turning the same way until he backed itself into the same corner every time while looking for the charger, was because of this "80" Step in the randrange. I'm sure you guessed it by now after I pointed it out. -40 to 40 is 80, if you maintain a Step of 80 it will always turn the same way: +40. So I set it to 5 which seems to me more reasonable. I hope this helps. I have not tested it yet to see if it speeds up his ability to find the dock in robot_drive_random_pattern() while he's in State 5 and not finding the dock.
I was struggling for a while trying to figure out why my first printed marker worked and Cozmo parked himself 1 time. Then when I printed out a second marker that had more black ink on it, because the first was in economy mode then I printed in highquality, but then Cozmo never attempted to park himself again.
Welp, I believe I have cured the problem that many might be having. While the marker is printed at 40x40 millimeters, my actual paper "object" size of the paper is cut out by adding 10 millimeters on either side. So the object is 60x60. But Cozmo still was having troubles, so I used a ruler and measured the paper sign I cut out. The object/sign is actually 1 millimeter off from 60 in both axis directions, and the marker is 40 by 40. So even using scissors is not perfect.
Basically, cutting 10mm on all sides of the paper was out by 1mm on all sides, making the paper 62mm on both axis.
Making that small change from 60x60 to 62x62 was the breaking point where Cozmo finally parked himself again for the 2nd time.
For Cozmo, knowing the marker size is 40mm when he backs up or forward will tell him his distance from that marker. Though with a marker at 40,40,40,40 Cozmo only knows the distance when he tries to do triangulate stuff. Now if we set the object size to the size of the paper cutout, Cozmo will get a 3d Radar Angle Scan of his distance and directional relation to that object when he compares it against the background of the paper size in relation to how the marker size grows when he moves back or forth.
I felt I had to say that and stress the importance of setting that paper cutout size along with the marker printout size. It's a robot, this is very important. I suggest not guessing either, pull out a ruler and measure it right down to the millimeter and type in that number first before you attempt to tweak it later.
CustomObjectMarkers.Hexagons2, 62, 62, 40, 40
Thank you thank you very much for all the preceeding information. I’ll try to check the code with this new suggestions. 🙌🙌🙌🙌
Can you do a YouTube video on how to do this plz it would mean alot
Greetings. It's been a while since I worked on Cozmo.
Just over the past month, I have been working on a Fork from "VictorTagayun" for your Cozmo Unleashed program. His program has been stripped down to just make Cozmo focus on docking, so I have labeled the file Cozmo_Unleashed_Simple_Dock.py
I have rewritten the turning towards dock script, so that he performs one single 180 degree turn that is exact with a tolerance of zero degrees and I can control the speed of his turns to go as fast or slow as needed for precision.
Also, I can get him to go to a position from a specific Angle in relation to the object. Like if I wanted him to face the dock at a 45 degree angle I can use a Gotoboject with an Angle=45. That will put him at whatever distance and he'll be at the 45 corner of the dock facing it at a 45 or Angle=0 to get him right in front of it. Then run it again but decrease the distance. Then do the 180 turn really slow & run the pre-existing backup onto dock command he already knows.
I'm writing to let you know I'm working on this and making progress learning the advanced functions of how to get this done. When I get some more experience and clean up my script more, I will implement it into your full Unleashed program. Once I have the lines of code I need to get him on the dock 90% of the time, I will upload them here as a fork to all your hard work.
Stay tuned.
Instead of making you wait, I will show you the two lines of code that I figured out.
This line is how I get him in front of the dock at a specific angle in relation to the dock. Can use say 300mm first, then do run it again at say 60mm, he'll travel in a straight line right up to the dock. I tried to use robot.go_to_pose but it doesn't seem to work right for this line of code. With pose he seems to end up in strange places & the distance he travels is way larger for some reason. Using go_to_object seems to have more control over getting him into position with the Angle perameter.
action = robot.go_to_object(charger, distance_mm(100), Angle(0), num_retries=2)
Then, this line of code replaces the two 90s and instead, whatever direction he happens to be facing, he will turn towards the back of the dock which is returned as Zero Degrees in relation to the dock, while controlling his turn acceleration at 50 degrees of accel per second.
turn_in_place on it's own, will make Cozmo turn left that many degrees in relation to his current posotion.
turn_in_place on it's own with is_absolute, will make Cozmo turn to that degrees as a heading in relation to his pose.origin_id
turn_in_place with a degrees pulled from an object (as seen below) without is_absolute, will make Cozmo turn left that many degrees in the difference to what degree he is facing how and what degree to object returns.
turn_in_place with degrees pulled from object & with is_absolute, will make Cozmo turn to that heading which is the back of the object so that Cozmo faces the object by default.
robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(50), angle_tolerance=None, is_absolute=True, num_retries=2).wait_for_completed()
I hope that all made sense. Ok so, if we call charger.pose.rotation.angle_z.degrees and use is_absolute=True, it will return the degrees for the back of the dock, which means if you were in front of the dock, cozmo will face the dock.
So to turn him backwards from the dock, we simply subtract 180 like this:
robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees - 180), accel=degrees(50), angle_tolerance=None, is_absolute=True, num_retries=2).wait_for_completed()
This he will turn his back towards the dock. At which point you can run this next command like I do:
robot.backup_onto_charger(max_drive_time=6.3)
which will make him drive backwards slowly for a set duration for how ever long you need.
So far. I can get him on the dock over 50% of the time. When I can get him to do better. I will post a whole fork.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
I've had very little time to work on this since October, I really need to start writing this from scratch as it's a mess. But... it kinda works.