Cozmo Unleashed + Simple Dock: Autonomous Cozmo Script, will find & park on Charger. Also includes configurable Scheduler for 2 time slots for weekdays & 2 time slots for weekends.

cozmo_unleashed_with_simple_dock.py

#!/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()
#
# New go_to_object, lookaround, playtime, & chargermarker routines were added by Banteel. This eliminates the double 90 turning and
# replaces it with a turn_in_place that retrieves a relative 0 heading from the charger variable. The go_to_object has also been
# equipped with a relative 0 heading, again retrieved from the charger. Playtime & Chargermarker routines have been added from
# Simple_Dock and have been even more optimized for this ultimate Cozmo Unleashed.
#
# 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.
#
# NOTE:
# Original perpetual Cozmo_Unleashed script credits go to AcidZebra for this script.
# 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.
# All the bells and whistles in AcidZebra's script now supports Simple_Dock docking routines.
#
# Last Update: July/19/2023
#
#import required functions
import sys, os, random, time, math, re, threading
##import logging
import asyncio, cozmo, cozmo.objects, cozmo.util, cozmo.world
from cozmo.run import connect_on_loop
from cozmo.util import Angle, degrees, distance_mm, speed_mmps, Pose, pose_z_angle
from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes
from cozmo.robot_alignment import RobotAlignmentTypes
#from asyncio import current_task, events
from datetime import datetime as Datetime
#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, camera, msg, needslevel, cozmostate, lightstate, lowbatvoltage, highbatvoltage, maxbatvoltage, debugging, robotvolume, chance, wfaac
global charger, foundcharger, foundmarker, found_object, chargermarker1, batcounter, batlightcounter, start_time, main_loop_count
global freeplay, tempfreeplay, use_scheduler, scheduler_playokay, scheduled_days, use_cubes, use_marker, use_walls
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

# initialize needed variables
chance = 90
freeplay = 0
lightstate = 0
batcounter = 0
main_loop_count = 0
scheduled_days="None"
scheduler_playokay=0
robot = cozmo.robot.Robot
wfaac = robot.wait_for_all_actions_completed(cozmo.robot.Robot)
# init birthday variables
bday_year = 0
bday_month = 0
bday_day = 0

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 lowbatvoltage to suit your cozmo. default value is 3.65
lowbatvoltage = 3.63
highbatvoltage= 4.04
maxbatvoltage = 4.57
#
# 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 between 0 and 1
use_cubes = 0
#
# CHARGER MARKER USAGE
#
# whether or not to activate the printed out "Hexagons2" (link is at the top ^) marker
# used by placing it over the charger to help Cozmo locate the area from a greater distance
# value between 0 and 1
use_marker = 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
#
# DEBUGGING
# when disabled, clears the screen status updates every cycle
# value between 0 and 1
debugging = 1
#
# SCHEDULER USAGE
#
# whether or not to use the schedule to define allowed "play times"
# this code will do 2 time slots: 2 slots for global weekdays, 2 slots for global weekend days
# this code will also do overnight scheduling and wake up or put cozmo to sleep on time 24-7
# value between 0 and 1
use_scheduler = 1
#
# SCHEDULE START & STOP SETTING IN 24HR TIME FORMAT ex. 0 to 23
#
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 = 7		# Week (D)ay's (S)tarting time hour, default is 7AM
	wkDSm =	0		# Week (D)ay's (S)tarting time minute
	wkDE = 8		# Week (D)ay's (E)nding time hour, default is 8AM
	wkDEm = 30		# 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 = 0		# Week (N)ight's (S)tarting time minute
	wkNE = 23		# Week (N)ight's (E)nding time hour, default is 11PM
	wkNEm = 30		# 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 = 30		# 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 = 0		# 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 = 30		# 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"
bday_year1 = 2000
bday_month1 = 1
bday_day1 = 2
# Enter User2 Birthday Event Time Here:
bday_name2 = "User2"
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)

#
# 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 = cozmo.robot.camera.Camera
	text_to_display = 'Exposure: %s ms\n' % camera.exposure_ms
	text_to_display += 'Gain: %s 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):
	global msg, objmsg, facemsg, camera, thread_running, needslevel, batlightcounter, lowbatvoltage, highbatvoltage, maxbatvoltage, lowbatcount
	global chargermarker1, foundmarker, charger, foundcharger, found_object, cozmostate
	global freeplay, start_time, use_cubes, tempfreeplay, lightstate, robotvolume, main_loop_count, scheduled_days, scheduler_playokay
	global use_scheduler
	if use_scheduler == 1:
		global nowtime, wday
		global wkDS, wkDSm, wkDE, wkDEm, wkNS, wkNSm, wkNE, wkNEm, wkEDS, wkEDSm, wkEDE, wkEDEm, wkENS, wkENSm, wkENE, wkENEm, dayornight
		nowtime = Datetime.now().timetuple()
	if (scheduled_days=="None" or use_scheduler == 0):
		cozmo.setup_basic_logging(general_log_level='WARN', protocol_log_level='WARN', protocol_log_messages='all', deprecated_filter='default')
		charger = None
		robot.world.charger = None
		found_object = None
		foundcharger = 0
		foundmarker = 0
		if use_scheduler == 1:
			nowtime = Datetime.now().timetuple()
			scheduler_playokay = 0
		robot.set_robot_volume(robotvolume)
		if use_cubes == 1:
			robot.enable_freeplay_cube_lights(enable=True)
			time.sleep(1)
		#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(True)
		robot.camera.enable_auto_exposure(True)
		robot.enable_facial_expression_estimation(enable=True)
		robot.camera.image_stream_enabled = True
		robot.camera.color_image_enabled = False
		robot.enable_all_reaction_triggers(False)
		robot.stop_all_motors()
		wfaac
		time.sleep(2)
		robot.clear_idle_animation()
		wfaac
		time.sleep(2)
		robot.abort_all_actions()
		wfaac
		time.sleep(2)
		wfaac
		time.sleep(1)
		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
		if use_marker == 1:
			#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
			# define the chargermarker printout as wall for the dock
			robot.world.define_custom_wall(CustomObjectTypes.CustomType01, CustomObjectMarkers.Hexagons2, 62, 62, 40, 40, True)
		if use_walls == 1:
			# these define the 4 walls of my play field, it helps Cozmo not bump into the walls
			# you can comment them out if your play field doesn't use walls
			# Marker Parameters:
			# (WallSize_X_Axis, WallSize_Y_Axis, ExactPrintedMarkerSize_X_Axis, ExactPrintedMarkerSize_Y_Axis, Unique True or not)
			# WallSize is the imaginary wall size that you want to create does not necessarily have to be same size as the paper
			# it's printed on. You can just invent any WallSize you want.
			# ExactPrintedMarkerSize must be measured from the colored area size of the printed marker, from edge to edge
			robot.world.define_custom_wall(CustomObjectTypes.CustomType02, CustomObjectMarkers.Diamonds2, 300, 60, 36, 36, False)
			robot.world.define_custom_wall(CustomObjectTypes.CustomType03, CustomObjectMarkers.Diamonds3, 300, 60, 36, 36, False)
			robot.world.define_custom_wall(CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 300, 60, 36, 36, False)
			robot.world.define_custom_wall(CustomObjectTypes.CustomType05, CustomObjectMarkers.Diamonds5, 300, 60, 36, 36, False)
		# initialize event monitoring thread
		q = None
		monitor(robot, q)
		start_time = time.time()
		msg = 'initialization complete'
		robot_print_current_state('entering main loop')
		time.sleep(4)
# 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):
			robot.enable_all_reaction_triggers(False)
			if cozmostate != 1: # 1 is charging
				robot_print_current_state('switching to state 1')
				cozmostate = 1
				start_time = time.time()
				if foundcharger!=0:
					foundcharger = 0
				if foundmarker!=0:
					foundmarker = 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()
			if robot.is_animating==True:
				robot.enable_all_reaction_triggers(False)
			time.sleep(0.1)
			lowbatcount=0
			#cozmostate = 1
			msg = 'state 1 checkpoint'
			if use_scheduler == 1 and scheduled_days == "None":
				robot_check_scheduler(robot)
			elif use_scheduler == 1 and scheduler_playokay == 1 and (scheduled_days == "Resting" or robot.is_charging == 1):
				cozmo_resting(robot)
			elif use_scheduler == 1 and scheduler_playokay == 1:
				robot_check_scheduler(robot)
			elif use_scheduler == 0:
				robot_print_current_state('state 1 - charging')
			# once in a while make random snoring noises
			robot_check_sleep_snoring()
			robot.clear_idle_animation()
			robot.abort_all_actions()
			wfaac
			robot_check_scheduler(robot)
			robot.clear_idle_animation()
			robot.abort_all_actions()
			wfaac

#
#State 2: on charger, fully charged, get off charger
#
		if (robot.is_on_charger == 1) and (robot.is_charging == 0) and (scheduled_days != "Resting" and scheduled_days != "None") and (scheduler_playokay == 1 or use_scheduler == 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
				if foundcharger!=0:
					foundcharger = 0
				if foundmarker!=0:
					foundmarker = 0
				if use_cubes == 1:
					robot.world.connect_to_cubes()
					time.sleep(5)
			msg = 'state 2 checkpoint'
			robot_print_current_state('state 2 - charged')
			robot.set_needs_levels(repair_value=1, energy_value=1, play_value=1)
			time.sleep(4)
			robot.play_anim("anim_launch_altwakeup_01").wait_for_completed()
			time.sleep(4)
			robot.clear_idle_animation()
			robot.drive_off_charger_contacts(num_retries=3, in_parallel=False).wait_for_completed()
			time.sleep(4)
			robot.move_lift(-3)
			try:
				if (robot.is_on_charger == 1):
					robot.drive_straight(distance_mm(100), speed_mmps(50), num_retries=2).wait_for_completed()
			finally:
				pass
			time.sleep(3)
			try:
				if (robot.is_on_charger == 1):
					robot.drive_straight(distance_mm(100), speed_mmps(50), num_retries=2).wait_for_completed()
			finally:
				pass
			time.sleep(2)
			wfaac

			#if scheduler_playokay == 1 or use_scheduler == 0:
			#	for _ in range(3):
			#		try:
			#			robot.drive_off_charger_contacts().wait_for_completed()
			#			robot_print_current_state('drive off charger OK')
			#			time.sleep(4)
			#		except: 
			#			robot_print_current_state('drive off charger error')
			#	robot_print_current_state('waking up - leaving charger')
			#	try:
			#		robot.drive_straight(distance_mm(80), speed_mmps(50)).wait_for_completed()
			#		time.sleep(4)
			#	except:
			#		robot_print_current_state('drive straight fail')
			#	try:
			#		robot.drive_straight(distance_mm(80), speed_mmps(50)).wait_for_completed()
			#		time.sleep(4)
			#	except:
			#		robot_print_current_state('drive straight fail')
			#	try:
			#		robot.play_anim("anim_gotosleep_getout_02").wait_for_completed()
			#		time.sleep(5)
			#	except:
			#		robot_print_current_state('wake up anim failed')
				#robot.world.connect_to_cubes()
				#robot_set_backpacklights(16711935)  # 16711935 is green
			time.sleep(3)
			highbatvoltage = robot.battery_voltage
		elif use_scheduler == 1 and scheduled_days == "None":
			robot_check_scheduler(robot)
		elif use_scheduler == 1 and scheduled_days == "Resting":
			cozmo_resting(robot)
			
#
#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,99,True,False,False)
			# print("Event log      : %s" % str(msg))
			time.sleep(0.2)
		# 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_scheduler == 1 and scheduled_days == "None":
				robot_check_scheduler(robot)
			if use_cubes == 1:
				robot.world.disconnect_from_cubes()
				time.sleep(1)
			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
			time.sleep(1)
#			
#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 and (scheduler_playokay == 1 or use_scheduler == 0):
			robot.enable_all_reaction_triggers(True)
			if robot.battery_voltage > lowbatvoltage + 0.4:
				lowbatcount == 0
			if use_scheduler == 1 and scheduled_days == "None":
				cozmostate = 4
				robot_check_scheduler(robot)
			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=2)
					except:
						robot_print_current_state('state 4 - failed to drive wheels')
						robot_reaction_chance(cozmo.anim.Triggers.OnSpeedtapGameCozmoWinHighIntensity,99,True,False,False)
					if use_cubes == 1:
						robot.world.connect_to_cubes()
						time.sleep(5)
					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.start_freeplay_behaviors()
			robot_set_needslevel()
		if cozmostate == 4:
			robot_check_randomreaction()
			time.sleep(1)
		if scheduler_playokay == 0 and cozmostate == 4 and cozmostate != 98 and cozmostate != 99:
			cozmostate == 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_or_not()
			# 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)
				time.sleep(1)
				robot.stop_freeplay_behaviors()
				freeplay = 0
				robot_print_current_state('state 6 - aborting freeplay')
				robot.abort_all_actions(log_abort_messages=True)
				wfaac
			freeplay = 0
			##robot_set_needslevel()
			robot_start_docking()
#
# state 9: we're on our side or are currently picked up
#
		if cozmostate == 9:
			robot.enable_all_reaction_triggers(True)
			robot_print_current_state('switching to state 9')
			robot_flash_backpacklights(4278190335)  # 4278190335 is red
			robot_reaction_chance(cozmo.anim.Triggers.CodeLabUnhappy,15,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()
				wfaac
				robot_reaction_chance(cozmo.anim.Triggers.AskToBeRightedLeft,15,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,15,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,15,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,99,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
#
		if cozmostate == 98:
			if foundcharger == 1 or foundmarker == 1:
				cozmostate = 6
#
# state 99: animation is playing
#

#
# we have looped through the states
#
		#robot_set_needslevel()
		#robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,99,True,True,True)
		#msg = 'state loop complete'
		#robot_check_randomreaction()
		#robot_print_current_state('cozmo_unleashed state program loop complete')
		time.sleep(0.5)
#
# Get Main Event Loop
#
		#if asyncio.Task.get_name(current_task()) != "Task-0":
		#	print (asyncio.Task.get_name(current_task()))
		#	asyncio.Task.cancel()
#
# if use_scheduler == 1 and is enabled
#
		if use_scheduler == 1 and (cozmostate == 4 or cozmostate == 1):
			nowtime = Datetime.now().timetuple()
			if scheduled_days == "None" and scheduler_playokay == 0:
				robot_check_scheduler(robot)
			# if main_loop_count >= 3:
			# 	robot_print_current_state('Main Loop Exceeded 2 Counts')
			# 	main_loop_count == main_loop_count - 1
			# 	break
			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)
					)
				):
				robot_print_current_state('Weekday '+str(dayornight)+' time Scheduled Time')
				robot_check_scheduler(robot)
				# break
			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)
					)
				):
				robot_print_current_state('Weekday '+str(dayornight)+' time Schedule Time')
				robot_check_scheduler(robot)
				# break
			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)
					)
				):
				robot_print_current_state('Weekend '+str(dayornight)+' time Scheduled Time')
				robot_check_scheduler(robot)
				# break
			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)
					)
				):
				robot_print_current_state('Weekend '+str(dayornight)+' time Schedule Time')
				robot_check_scheduler(robot)
				# break
			elif scheduled_days == "Resting" or scheduler_playokay == 0:
				if use_cubes == 3:
					robot.enable_freeplay_cube_lights(enable=True)
				if robot.is_on_charger == 0:
					cozmostate = 5
					robot.stop_freeplay_behaviors()
					time.sleep(3)
					robot_print_current_state('Resting Time Schedule, Find Charger')
					robot_locate_dock_or_not()
		if use_scheduler == 1:
			robot_check_scheduler(robot)
				# break
		robot_print_current_state('Main Loop Completed')
#
#
# END OF STATE LOOP
# 
	##robot_set_needslevel()
	robot_reaction_chance(cozmo.anim.Triggers.CodeLabTakaTaka,99,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()
	time.sleep(0.1)
	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,oldlightstate
	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, highbatvoltage
	needslevel = 1 - (highbatvoltage - 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, cozmostate
	robot.clear_idle_animation()
	time.sleep(1)
	robot.abort_all_actions(log_abort_messages=True)
	time.sleep(1)
	wfaac
	time.sleep(1)
	i = random.randint(1, 100)
	#robot_print_current_state('snore check int='+str(i))
	#if i >= 97 and cozmostate == 1 and not robot.is_animating:
	if i >= 97 and cozmostate == 1:
		robot_print_current_state('check complete - snore')
		#robot.play_anim_trigger(Sleeping).wait_for_completed()
		robot_reaction_chance(cozmo.anim.Triggers.Sleeping,100,False,False,False)
		wfaac
		time.sleep(4)
	else:
		#robot_print_current_state('check complete - no snore')
		time.sleep(2)

def cozmo_resting(robot: cozmo.robot.Robot):
	global nowtime, bday_name1, bday_name2, bday_year1, bday_year2, bday_month1, bday_month2, bday_day1, bday_day2, say_age1, say_age2, cozmostate
	global bday_year, bday_month, bday_day, bday_name, Hdays, robotvolume
	
	i = random.randint(1, 100)
	# 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):
		oldcozmostate=cozmostate
		cozmostate=99
		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)
		cozmostate=oldcozmostate
		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)):
			oldcozmostate=cozmostate
			cozmostate=99
			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)
		
			cozmostate=oldcozmostate
			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 scheduler_playokay == 1:
			robot_print_current_state('charging, inside schedule')
		else:
			robot_print_current_state('charging, outside schedule')

		#i = random.randint(1, 100)
		if i >= 97:
			robot.clear_idle_animation()
			robot.abort_all_actions()
			try:
				robot_reaction_chance(cozmo.anim.Triggers.GoToSleepGetIn,100,False,False,False)
				#robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
				wfaac
			except:
				robot_print_current_state('failed to play anim')
		elif i >= 90:
			robot.clear_idle_animation()
			robot.abort_all_actions()
			try:
				robot_reaction_chance(cozmo.anim.Triggers.Sleeping,100,False,False,False)
				#robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
				wfaac
			except:
				robot_print_current_state('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 scheduler_playokay == 1:
			robot_print_current_state('Charged! Starting scheduled playtime')
		else:
			robot_print_current_state('Charged! waiting for scheduled playtime')

		#i = random.randint(1, 100)
		if i >= 97:
			try:
				robot_reaction_chance(cozmo.anim.Triggers.GoToSleepGetIn,100,False,False,False)
				#robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
			except:
				robot_print_current_state('failed to play anim')
		elif i >= 85:
			try:
				robot_reaction_chance(cozmo.anim.Triggers.Sleeping,100,False,False,False)
				#robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
			except:
				robot_print_current_state('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()
#
# Define Check Scheduler if it is enabled
#
def robot_check_scheduler(robot: cozmo.robot.Robot):
	global use_scheduler, nowtime, wday, scheduled_days, scheduler_playokay, cozmostate, main_loop_count
	global wkDS, wkDSm, wkDE, wkDEm, wkNS, wkNSm, wkNE, wkNEm, wkEDS, wkEDSm, wkEDE, wkEDEm, wkENS, wkENSm, wkENE, wkENEm, dayornight
	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
	#robot_print_current_state('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:
	robot_print_current_state('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"
		scheduler_playokay = 1
		robot_print_current_state('Weekday '+str(dayornight)+' time is: '+str(nowtime.tm_hour)+':'+str(nowtime.tm_min))
		# if main_loop_count >=1 and (cozmostate == 4 or cozmostate == 1):
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	#print("scheduler run loop " + str(main_loop_count))
		# 	main_loop_count = main_loop_count - 1
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	robot_print_current_state('weekday '+str(dayornight)+' scheduled loop complete')
		# 	# break
		# main_loop_count = main_loop_count + 1
		# cozmo_unleashed(robot)				
	#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"
		scheduler_playokay = 1
		robot_print_current_state('Weekday '+str(dayornight)+' time is: '+str(nowtime.tm_hour)+':'+str(nowtime.tm_min))
		# if main_loop_count >=1 and (cozmostate == 4 or cozmostate == 1):
		# 	#print("scheduler run loop " + str(main_loop_count))
		# 	main_loop_count = main_loop_count - 1
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	robot_print_current_state('weekday '+str(dayornight)+' scheduled loop complete')
		# main_loop_count = main_loop_count + 1
		# cozmo_unleashed(robot)			
	#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"
		scheduler_playokay = 1
		robot_print_current_state('Weekend '+str(dayornight)+' time is: '+str(nowtime.tm_hour)+':'+str(nowtime.tm_min))
		# if main_loop_count >=1 and (cozmostate == 4 or cozmostate == 1):
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	#print("scheduler run loop " + str(main_loop_count))
		# 	main_loop_count = main_loop_count - 1
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	robot_print_current_state('weekend '+str(dayornight)+' scheduled loop complete')
		# 	# break
		# main_loop_count = main_loop_count + 1
		# cozmo_unleashed(robot)
	#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"
		scheduler_playokay = 1
		robot_print_current_state('Weekend '+str(dayornight)+' time is: '+str(nowtime.tm_hour)+':'+str(nowtime.tm_min))
		# if main_loop_count >=1 and (cozmostate == 4 or cozmostate == 1):
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	#print("scheduler run loop " + str(main_loop_count))
		# 	main_loop_count = main_loop_count - 1
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	robot_print_current_state('weekend '+str(dayornight)+' scheduled loop complete')
		# 	# break
		# main_loop_count = main_loop_count + 1
	# Schedule not set or in Resting state
	elif scheduled_days == "None" or scheduled_days == "Resting":
		nowtime = Datetime.now().timetuple()
		scheduled_days = "Resting"
		scheduler_playokay = 0
		if cozmostate == 4:
			cozmostate = 5
		# if main_loop_count >=1 and (cozmostate == 4 or cozmostate == 1 or scheduler_playokay == 0):
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	#print("scheduler run loop " + str(main_loop_count))
		# 	main_loop_count = main_loop_count - 1
		# 	#os.system('cls' if os.name == 'nt' else 'clear')
		# 	robot_print_current_state('resting loop complete')
		# 	# break
		cozmo_resting(robot)
	else:
		nowtime = Datetime.now().timetuple()
		scheduled_days = "Resting"
		scheduler_playokay = 0

def robot_check_randomreaction():
	global robot,cozmostate,freeplay
	i = random.randint(1, 2000)
	#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 >= 1991 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()
		time.sleep(1)
		robot.wait_for_all_actions_completed()
		time.sleep(1)
		robot.abort_all_actions(log_abort_messages=True)
		time.sleep(1)
		#robot.clear_idle_animation()
		#wfaac
		# 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:
				trig = robot.play_anim_trigger(trigger)
				trig.wait_for_completed()
				time.sleep(3)
				robot_print_current_state('played random animation')
			except:
				robot_print_current_state('random animation play failed')
				pass
			wfaac
		#wfaac	
		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(1)

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:
	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()
			wfaac
		time.sleep(2)
		robot_print_current_state('playing animation')
		robot.clear_idle_animation()
		time.sleep(0.1)
		robot.abort_all_actions()
		time.sleep(0.1)
		try:
			trig = robot.play_anim_trigger(animation, ignore_body_track=ignorebody, ignore_head_track=ignorehead, ignore_lift_track=ignorelift)
			trig.wait_for_completed()
			#time.sleep(4)
			#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")
		wfaac
		#time.sleep(2)
		#robot.clear_idle_animation()
		robot_print_current_state('resetting head angle')
		try:
			robot.set_head_angle(degrees(0)).wait_for_completed()
		except:
			robot_print_current_state('head angle reset failed')
			#print("reaction %s" %str(animation)," aborted")
		#wfaac
		wfaac
		time.sleep(2)
		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")
		wfaac
		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(2)
		robot_print_current_state('animation check - no winner')
		robot.clear_idle_animation()

# goto chargermarker pose routine after marker is found
def goto_chargermarker_pose():
	global cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes,charger,lowbatvoltage, use_scheduler,msg,camera,foundcharger,foundmarker,custom_objects,found_object,tempfreeplay
	markerloop=0
	while robot.world.charger == None:
		time.sleep(1)
		if str(found_object.object_type) == "CustomObjectTypes.CustomType01" and charger==None:
			robot.stop_all_motors()
			robot.clear_idle_animation()
			robot.abort_all_actions(log_abort_messages=True)
			wfaac
			time.sleep(3)
			try:
				robot_print_current_state('custom object found, traveling')
				robot.go_to_pose(pose_z_angle(found_object.pose.position.x, found_object.pose.position.y, 0, angle_z=degrees(found_object.pose.rotation.angle_z.degrees)), in_parallel=False, num_retries=2).wait_for_completed()
			except:
				robot_print_current_state('failed to goto pose')
			wfaac
			try:
				robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
			except:
				robot_print_current_state('failed to turn in place')
			try:
				robot.set_head_angle(degrees(0)).wait_for_completed()
			except:
				robot_print_current_state('failed to set head angle')
			wfaac
			time.sleep(3)
			if robot.world.charger != None:
				charger = robot.world.charger
				robot_print_current_state('found charger after goto chargermarker')
				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:
					robot_print_current_state('failed to turn in place')
				try:
					robot.drive_straight(distance_mm(-120), speed_mmps(40)).wait_for_completed()
					wfaac
				except:
					robot_print_current_state('failed to drive straight')
				try:
					robot.go_to_object(charger, distance_mm(100), RobotAlignmentTypes.Body, num_retries=3)
					wfaac
				except:
					robot_print_current_state('failed to goto object')
				try:
					robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=3)
					wfaac
				except:
					robot_print_current_state('failed to goto object')
				try:
					robot.go_to_object(charger, distance_mm(60), RobotAlignmentTypes.Body, num_retries=3)
					wfaac
				except:
					robot_print_current_state('failed to goto object')
				finally:
					cozmostate=6
					foundcharger=1
					freeplay=0
					break
			time.sleep(1)
			if not robot.world.charger:
				try:
					robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), num_retries=1, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
					wfaac
				except:
					robot_print_current_state('failed to turn in place')
				if found_object != None and found_object.is_visible == False:
					try:
						robot_print_current_state('cannot see marker, backup a bit')
						robot.drive_straight(distance_mm(-80), speed_mmps(40)).wait_for_completed()
						wfaac
					except:
						robot_print_current_state('failed to drive straight')
				try:
					robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
				except:
					robot_print_current_state('failed to turn in place')
			wfaac
			time.sleep(2)
			try:
				charger = robot.world.wait_for_observed_charger(timeout=3, include_existing=True)
				robot.world.charger = charger
				robot_print_current_state('found charger after backup!')
				cozmostate=6
				foundcharger=1
				freeplay=0
				break
			except asyncio.TimeoutError:
				robot_print_current_state('i do not see the charger')
			time.sleep(0.5)
			if not robot.world.charger:
				if found_object.is_visible == False:
					try:
						robot_print_current_state('cannot see marker, backup a bit more')
						robot.drive_straight(distance_mm(-60), speed_mmps(40)).wait_for_completed()
						wfaac
					except:
						robot_print_current_state('failed to drive straight')
				try:
					robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
				except:
					robot_print_current_state('failed to turn in place')
				try:
					charger = robot.world.wait_for_observed_charger(timeout=3, include_existing=True)
					robot.world.charger = charger
					robot_print_current_state('found charger after backup!')
					cozmostate=6
					foundcharger=1
					freeplay=0
					break
				except asyncio.TimeoutError:
					robot_print_current_state('i still do not see the charger')
			time.sleep(1)
			robot.set_head_angle(degrees(10)).wait_for_completed()
			wfaac
			time.sleep(1)
			if found_object != None:
				try:
					robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
				except:
					robot_print_current_state('failed to turn in place')
				try:
					robot_print_current_state('cannot see chargermarker, backup a lot')
					robot.drive_straight(distance_mm(-100), speed_mmps(40)).wait_for_completed()
				except:
					robot_print_current_state('failed to drive straight')
				try:
					robot_print_current_state('custom object still known, traveling')
					robot.go_to_pose(pose_z_angle(found_object.pose.position.x, found_object.pose.position.y, 0, angle_z=degrees(found_object.pose.rotation.angle_z.degrees)), in_parallel=False, num_retries=2).wait_for_completed()
				except:
					robot_print_current_state('failed to goto pose')
				wfaac
				try:
					charger = robot.world.wait_for_observed_charger(timeout=3, include_existing=True)
					robot_print_current_state('found charger after backup!')
					cozmostate=6
					foundcharger=1
					freeplay=0
					break
				except asyncio.TimeoutError:
					robot_print_current_state('did not find charger, try again')
					cozmostate=5
			wfaac
			time.sleep(3)
			#if charger == None and robot.world.charger == None:
			#	markerloop+=1
			#	robot_print_current_state('chargermarker goto retry: '+str(markerloop))
			#if markerloop > 1:
			#	robot_print_current_state('chargermarker goto failed, resetting')
			#	break
	found_object = None
	custom_objects = None
	foundmarker = 0

def robot_locate_dock_or_not():
	global robot,cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger, foundmarker,tempfreeplay,custom_objects
	#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()
	#wfaac
	if use_cubes==1:
		robot.world.disconnect_from_cubes()
	freeplay = 0
	robot_reaction_chance(cozmo.anim.Triggers.NeedsMildLowEnergyRequest,95,False,False,False)
	robot_set_needslevel()
	robot_print_current_state('begin finding charger')
	# charger location search
	if foundcharger == 1 and cozmostate != 1 and cozmostate != 2 and cozmostate !=6:
		#we know where the charger is
		robot_print_current_state('charger position known, travelling')
		robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,95,True,False,False)
		wfaac
		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:
			action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.Body, num_retries=3)
			action.wait_for_completed()
		except:
			robot_print_current_state('failed to goto charger')
		try:
			action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=3)
			action.wait_for_completed()
		except:
			robot_print_current_state('failed to goto charger')
		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:
			robot_print_current_state('failed to turn in place')
		wfaac
		cozmostate = 6
		time.sleep(1)
	elif foundcharger == 1 and cozmostate == 6:
		robot_start_docking()
	elif use_marker==1 and foundmarker==1 and foundcharger==0 and str(found_object.object_type)=="CustomObjectTypes.CustomType01" and cozmostate !=1 and cozmostate !=2 and cozmostate !=6:
		robot_print_current_state('chargermarker still known')
		goto_chargermarker_pose()
		custom_objects = None
		#cozmostate = 6
		time.sleep(1)
	elif foundcharger==0 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,70,True,False,True)
		wfaac
		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_find_charger_pattern()
		robot_print_current_state('locate dock sequence completed')
	elif charger != None:
		cozmostate = 6
	elif robot.is_on_charger == 1:
		cozmostate = 1
	time.sleep(0.5)
						
def robot_find_charger_pattern():
	global cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes,charger,lowbatvoltage, use_scheduler,msg,camera,foundcharger,foundmarker,custom_objects,found_object,tempfreeplay,use_marker
	oldcozmostate = cozmostate
	freeplay=0
	while foundcharger == 0 and foundmarker == 0 and cozmostate == 5:
		#if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose):
		if foundcharger == 1:
			#charger = robot.world.charger
			robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,99,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')
			break
		if use_marker==1 and foundmarker==1 and foundcharger==0 and found_object != None and str(found_object.object_type) == "CustomObjectTypes.CustomType01":
			robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop()
			robot_print_current_state('chargermarker still known')
			#robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
			goto_chargermarker_pose()
			custom_objects = None
			#cozmostate = 6
			break
		cozmostate = 98
		robot.enable_all_reaction_triggers(True)
		robot_print_current_state('start lookaround behavior')
		robot.set_head_angle(degrees(0)).wait_for_completed()
		robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
		current_time = time.time()
		end_time = time.time() + 32.0
		if use_marker == 0:
			charger_timeout = 35
		elif use_marker != 0:
			charger_timeout = 0.1
		while current_time < end_time:
			try:
				charger = robot.world.wait_for_observed_charger(timeout=charger_timeout, include_existing=True)
				robot_print_current_state('found charger in lookaround!')
				if charger != None:
					robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop()
					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:
						robot_print_current_state('failed to turn in place')
					finally:
						cozmostate = 6
						foundcharger=1
						break
			except asyncio.TimeoutError or time.time() >= end_time:
				pass
			time.sleep(0.1)
			if use_marker == 1:
				try:
					custom_objects = robot.world.wait_until_observe_num_objects(num=1, object_type=CustomObject, timeout=0.1, include_existing=True)
				except asyncio.TimeoutError:
					pass
				finally:
					time.sleep(0.5)
					if custom_objects != None:
						if len(custom_objects) > 0 and str(custom_objects[0].object_type) == "CustomObjectTypes.CustomType01":
							robot_print_current_state('found chargermarker in lookaround!')
							robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop()
							found_object = custom_objects[0]
							custom_objects=None
							#cozmostate = 6
							foundmarker = 1
							break
					if custom_objects != None:
						custom_objects=None
			if time.time() > end_time:
				robot_print_current_state('lookaround time ended, did not find charger or marker')
				robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop()
				break
			current_time = time.time()
			time.sleep(0.1)
		# lookaround is stopped
		time.sleep(1)
		robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,99,True,False,True)
		robot_print_current_state('ended lookaround behavior')

		if use_marker == 1 and foundmarker == 1 and foundcharger == 0:
			time.sleep(1)
			goto_chargermarker_pose()
			custom_objects=None
		time.sleep(0.5)

		if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
			break

		if cozmostate==98 and foundcharger==0:
			tempfreeplay=1
			robot_print_current_state('begin freeplay behavior')
			robot.start_freeplay_behaviors()
			#try freeplay for 90 seconds, if found charger finally stop freeplay
			current_time = time.time()
			end_time = time.time() + 92.0
			if use_marker == 0:
				charger_timeout = 95
			elif use_marker != 0:
				charger_timeout = 0.1
			while current_time < end_time:
				if use_marker == 1 and foundmarker == 1 and str(found_object.object_type) == "CustomObjectTypes.CustomType01":
					robot.stop_freeplay_behaviors()
					robot_print_current_state('chargermarker still known')
					tempfreeplay=0
					goto_chargermarker_pose()
					custom_objects = None
					break
				try:
					charger = robot.world.wait_for_observed_charger(timeout=charger_timeout, include_existing=True)
					robot_print_current_state('found the charger in 90sec playtime!')
					if charger != None:
						robot.stop_freeplay_behaviors()
						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:
							robot_print_current_state('failed to turn in place')
						try:
							robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed()
						except:
							robot_print_current_state('failed to drive straight')
						finally:
							tempfreeplay=0
							cozmostate = 6
							foundcharger=1
							break
				except asyncio.TimeoutError or time.time() > end_time:
					pass
				time.sleep(0.01)
				if use_marker == 1:
					try:
						custom_objects = robot.world.wait_until_observe_num_objects(num=1, object_type=CustomObject, timeout=0.1, include_existing=True)
					except asyncio.TimeoutError or time.time() > end_time:
						pass
					finally:
						if custom_objects != None:
							if len(custom_objects) > 0 and str(custom_objects[0].object_type) == "CustomObjectTypes.CustomType01":
								robot_print_current_state('found the chargermarker in 90sec playtime!')
								robot.stop_freeplay_behaviors()
								found_object = custom_objects[0]
								custom_objects=None
								tempfreeplay=0
								#cozmostate = 6
								foundmarker = 1
								break
						if custom_objects != None:
							custom_objects=None
				if time.time() > end_time:
					robot_print_current_state('freeplay time ended, did not find charger or marker')
					robot.stop_freeplay_behaviors()
					tempfreeplay=0
					break
				current_time = time.time()
				time.sleep(0.1)
			#after 90 seconds end freeplay

		if use_marker == 1 and foundmarker == 1 and charger==None:
			time.sleep(1)
			tempfreeplay=0
			goto_chargermarker_pose()
			custom_objects=None
		time.sleep(0.5)

		if cozmostate == 6 or cozmostate == 1 or cozmostate == 2 or foundmarker == 1:
			break
		cozmostate = oldcozmostate
		robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,99,True,False,True)
		robot_print_current_state('ended freeplay 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):
			charger = robot.world.charger
			robot_print_current_state('found charger')
			cozmostate = 6
			foundcharger = 1
			robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,99,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,99,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 foundmarker == 1 and foundcharger == 0:
		time.sleep(1)
		tempfreeplay=0
		goto_chargermarker_pose()
		custom_objects=None
	elif cozmostate == 6:
		cozmostate = 6
	elif cozmostate == 1:
		cozmostate = 1
	elif cozmostate == 2:
		cozmostate = 2
	else:
		cozmostate = oldcozmostate
	robot_print_current_state('locate charger pattern completed')
	#return charger

def robot_start_docking():
	global robot,cozmostate,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger, foundmarker
	if foundcharger == 1 and charger !=None:
		if charger and charger.is_visible == False:
			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:
				robot_print_current_state('failed to turn in place')
			try:
				robot.drive_straight(distance_mm(-120), speed_mmps(50)).wait_for_completed()
			except:
				robot_print_current_state('failed to back up a little bit')
		action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=1)
		action.wait_for_completed()
		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:
			robot_print_current_state('failed to turn in place')
		robot_print_current_state('go to charger complete')
	else:
		robot_print_current_state('charger pose not known')
		charger = None
		#robot.world.charger = None
		foundcharger = 0
		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
	if charger and charger.is_visible == False:
		charger = None
		cozmostate = 5
	dockloop = 0
	while dockloop <= 3 and cozmostate == 6 and robot.is_on_charger == 0 and charger != None:
		robot.enable_all_reaction_triggers(False)
		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:
					robot_print_current_state('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:
				robot_print_current_state('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:
					robot_print_current_state('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:
			robot_print_current_state('failed to play anim')
		wfaac
		time.sleep(1)

		if charger and charger.is_visible == False:
			#we know where the charger is
			robot_print_current_state('can not see charger, position is still known')
			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:
				robot_print_current_state('failed to turn in place')
			finally:
				pass
			try:
				action = robot.drive_straight(distance_mm(-140), speed_mmps(40))
				action.wait_for_completed()
			except:
				robot_print_current_state('failed to drive straight')
			try:
				robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
			except:
				robot_print_current_state('failed to play anim')
			try:
				robot.set_head_angle(degrees(0)).wait_for_completed()
			except:
				robot_print_current_state('failed to set head angle')
			try:
				action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.Body, num_retries=2)
				action.wait_for_completed()
			except:
				robot_print_current_state('failed to go to charger')
			try:
				action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=2)
				action.wait_for_completed()
			except:
				robot_print_current_state('failed to go to charger')
			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:
				robot_print_current_state('failed to turn in place')
			try:
				action = robot.drive_straight(distance_mm(-50), speed_mmps(40))
				action.wait_for_completed()
			except:
				robot_print_current_state('failed to drive straight')
			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:
					robot_print_current_state('failed to drive straight')
				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:
					robot_print_current_state('failed to turn in place')
				robot_print_current_state('charger not found, clearing map')
				time.sleep(1)
				charger = None
				foundcharger = 0
				break
		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)
		time.sleep(0.5)
		robot.set_head_light(True)
		time.sleep(0.5)
		robot.set_head_light(False)
		robot_reaction_chance(cozmo.anim.Triggers.FeedingReactToShake_Normal,85,True,False,False)
		wfaac
		time.sleep(1)
		# robot.enable_all_reaction_triggers(False)
		robot_print_current_state('docking')
		try:
			robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees - 180), accel=degrees(75), angle_tolerance=None, is_absolute=True, num_retries=3).wait_for_completed()
		except:
			robot_print_current_state('failed to do a 180')
		wfaac
		time.sleep(1)
		robot_reaction_chance(cozmo.anim.Triggers.CubePounceFake,15,True,False,False)
		#robot_reaction_chance(cozmo.anim.Triggers.CubePounceFake,99,True,False,False)
		time.sleep(1)
		# robot.enable_all_reaction_triggers(False)
	
		backup_count = 0
		while not robot.is_on_charger:
			try:
				robot.backup_onto_charger(max_drive_time=2)
				backup_count+=1
				time.sleep(0.4)
				if robot.is_on_charger == 1:
					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:
				robot.stop_all_motors()
				robot.clear_idle_animation()
				robot.abort_all_actions(True)
				print("State: Contact! Stop all motors, battery %s" % str(round(robot.battery_voltage, 2)))
				break
			except robot._recv_msg_unexpected_movement:
				print("State: Robot bumped into something, battery %s" % str(round(robot.battery_voltage, 2)))
				break
			finally:
				robot.clear_idle_animation()
				robot.abort_all_actions(True)
				wfaac
				time.sleep(0.5)
				wfaac
		# try:
		# 	robot.backup_onto_charger(max_drive_time=6)
		# except:
		# 	robot_print_current_state('failed to drive onto charger')
		# #wfaac
		# time.sleep(2)			
		# try:
		# 	if robot.is_on_charger == 0:
		# 		robot_print_current_state('bumped into something, backup a bit more')
		# 		robot.backup_onto_charger(max_drive_time=3)
		# 		time.sleep(2)
		# 	elif robot.is_on_charger == 1:
		# 		pass
		# except:
		# 	robot_print_current_state('failed to drive onto charger')
		# finally:
		# 	robot.clear_idle_animation()
		# 	robot.abort_all_actions(True)
		# 	wfaac
		# 	time.sleep(0.5)
		time.sleep(2)
		# 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)
			robot_print_current_state('Docked!')
			#robot_reaction_chance(cozmo.anim.Triggers.SparkSuccess,60,True,True,True)
			#wfaac
			dockloop = 0
			cozmostate = 1
			robot.clear_idle_animation()
			robot.abort_all_actions(True)
			robot.enable_all_reaction_triggers(False)
			time.sleep(2)
			break
		else:
			# No, we missed. Back off and try again
			dockloop = dockloop + 1
			robot_print_current_state('failed to dock, go forward and try docking again')
			robot_reaction_chance(cozmo.anim.Triggers.AskToBeRightedRight,99,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')
			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:
				robot_print_current_state('failed to turn')
			try:
				robot.drive_straight(distance_mm(90), speed_mmps(90)).wait_for_completed()
			except:
				robot_print_current_state('failed to drive')
			try:
				robot.drive_straight(distance_mm(90), speed_mmps(90)).wait_for_completed()
			except:
				robot_print_current_state('failed to drive')
			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:
				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(1)
			if charger and charger.is_visible == False:
				#we know where the charger is
				robot_print_current_state('can not see charger after turnaround')
				try:
					robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
				except:
					robot_print_current_state('failed to play anim')
				try:
					robot.set_head_angle(degrees(0)).wait_for_completed()
				except:
					robot_print_current_state('failed to set head angle')
				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:
					robot_print_current_state('failed to turn')
				try:
					action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.Body, num_retries=2)
					action.wait_for_completed()
				except:
					robot_print_current_state('failed to go to charger')
				try:
					action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.Body, num_retries=2)
					action.wait_for_completed()
				except:
					robot_print_current_state('failed to go to charger')
				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:
					robot_print_current_state('failed to turn in place')
				time.sleep(1)
				if charger.is_visible == False:
					# we lost the charger, clearing all map variables
					charger = None
					foundcharger = 0
					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')
					try:
						robot.drive_straight(distance_mm(-120), speed_mmps(50)).wait_for_completed()
					except:
						robot_print_current_state('failed to go drive straight')
					robot_print_current_state('charger lost - docking loop re-initialized')
					time.sleep(1)
					break
			if dockloop < 3:
				robot_print_current_state('docking loop '+str(dockloop)+' completed')
			else:
				robot_print_current_state('loop 3 - docking re-initialized')
				foundcharger = 0				
				charger = None
				foundmarker = 0
				found_object = None
				dockloop = 0
				time.sleep(2)
		if foundmarker == 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:
				robot_print_current_state('failed to play anim')
			time.sleep(1)
			try:
				robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=1).wait_for_completed()
			except:
				robot_print_current_state('failed to go drive straight')
			robot_print_current_state('charger lost while docking')
			time.sleep(1)

	# exited loop, check current state:
	if cozmostate != 1 and cozmostate != 2:
		cozmostate = 5
		dockloop = 0
		robot_locate_dock_or_not()
		robot_reaction_chance(cozmo.anim.Triggers.MemoryMatchPlayerWinGame,99,True,False,False)
		time.sleep(1)
#
# END OF ROBOT FUNCTIONS
#
	
#
# EVENT MONITOR FUNCTIONS
#
class CheckState (threading.Thread):
	global robot,cozmostate,freeplay,msg,camera,objmsg,facemsg,oldcozmostate
	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,maxbatvoltage,lightstate,charger,found_object,foundcharger,foundmarker
		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 foundcharger==1:
					foundcharger = 0
				if foundmarker==1:
					foundmarker = 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 > 5:
				charger = None
				found_object = None
				#robot.world.charger = None
				if foundcharger==1:
					foundcharger = 0
				if foundmarker==1:
					foundmarker = 0
				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:
				time.sleep(0.5)
				if not is_on_charger:
					is_on_charger = True
					#freeplay = 0
					if cozmostate != 99:
						cozmostate = 1
					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,99,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,99,True,False,False)
					# except:
						# pass
					# try:
						# robot_reaction_chance(cozmo.anim.Triggers.CodeLabSleep,99,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:
						if cozmostate != 99:
							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
					if cozmostate != 99:
						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 not robot.is_picking_or_placing 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()
						#wfaac
					robot.abort_all_actions(log_abort_messages=True)
					robot.clear_idle_animation()
					wfaac
					time.sleep(0.1)
					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')
					if robot.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):
	global found_object, cozmostate
	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)
	elif isinstance(obj,cozmo.objects.CustomObject):
		msg = 'Object-' + str(obj.object_type)
	else:
		r = re.search('<(\w*)', obj.__repr__())
		msg = r.group(1)
	#time.sleep(0.01)
	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.stop_all_motors()
		robot.abort_all_actions(log_abort_messages=True)
		robot.clear_idle_animation()
		time.sleep(0.5)
		if robot.is_animating:
			#time.sleep(0.5)
			try:
				wfaac
			except:
				robot_print_current_state('all actions completed')
			#time.sleep(0.5)
		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 != '' else '[unknown face]'
	expression = face.expression if face.expression != '' 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,found_object,foundmarker
	msg = print_prefix(evt)
	msg += print_object(kwargs['obj']) + ' '
	if print_object(kwargs['obj']) == "Charger":
		tempCharger = (kwargs['obj'])
		if tempCharger.pose.is_comparable(kwargs['obj'].pose) == True:
			robot_print_current_state('updating charger location')
			charger = tempCharger
		if cozmostate == 5 or cozmostate == 98:
			robot_print_current_state('Found The Charger and I need it!')
		if foundcharger==0:
			foundcharger=1
	elif print_object(kwargs['obj']) == "Object-CustomObjectTypes.CustomType01" and use_marker==1:
		temp_object=(kwargs['obj'])
		if found_object == None:
			found_object = temp_object
		if temp_object.pose.is_comparable(found_object.pose) == True:
			robot_print_current_state('updating chargermarker location')
			found_object = temp_object
		if cozmostate == 5 or cozmostate == 98:
			robot_print_current_state('Found The ChargerMarker and I need it!')
		if foundmarker==0:
			foundmarker=1
	time.sleep(0.01)
	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 != 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 != 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 != 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 != 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, force_viewer_on_top=True, show_viewer_controls=True, exit_on_connection_error=True)
#if sys.platform =='win32' or sys.platform == 'win64':
#	loop = asyncio.ProactorEventLoop()
asyncio.get_event_loop_policy()
cozmo.run_program(cozmo_unleashed, use_viewer=True, force_viewer_on_top=True, show_viewer_controls=True, exit_on_connection_error=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, 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
#
# below is just the program running without any camera view or 3d maps
#cozmo.run_program(cozmo_unleashed)