Non-Asyncio Simple Dock with ChargerMarker: A non-asyncio Simple_Dock with Chargermarker that is Python 3.6 safe for Cozmo robot

simple_dock_with_chargermarker_non_asyncio.py

#!/usr/bin/env python3

# based on Copyright (c) 2016 Anki, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License in the file LICENSE.txt or at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
#
# simple_dock_with_chargermarker_non_asyncio
# This script can use Walls & a Charger Marker to locate the Charger
# The Walls, FreePlay Lights, & Charger Marker can be turned off & on as needed
# This script does not require any of these options to be turned on, all you need is a Charger
# This script is the Non Asyncio version of the simple_dock_with_ChargerMarker script
#
#
# forked script from: cozmo_unleashed
#
# Original perpetual Cozmo_Unleashed script by AcidZebra
# This Script is made by VictorTagayun and is heavily slimmed down to only docking
# Script based on the Cozmo SDK drive_to_charger example script 
# and raproenca's modifications of same.
#
# Additional modifications by Banteel to streamline the two 90 degree turns in one simple
# command that is linked to position.angle_z of object "charger" which contains all the info.
# rewritten to support battery/charge states and all kinds of bells and whistles by AcidZebra
#
# The default Low Battery Level for Cozmo is 3.65. For testing pursposes I have set mine
# to 3.88 so Cozmo gets on the dock more often. Please feel free to set both lines to 3.65
#
#import required functions
import sys, os, datetime, random, time, math, re, threading
##import logging
import asyncio, cozmo, cozmo.objects, cozmo.util, cozmo.world
from cozmo.util import Angle, degrees, distance_mm, speed_mmps, pose_z_angle, Pose
from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes, LightCube
from cozmo.robot_alignment import RobotAlignmentTypes
from cozmo import connect_on_loop
from asyncio import AbstractEventLoop

#external libraries used for camera annotation
#you will need to add these using pip/pip3
from PIL import ImageDraw, ImageFont
import numpy as np

global robot, charger, freeplay, robotvolume, found_object, custom_objects, marker_time, use_cubes, use_walls
global use_scheduler, scheduled_days, scheduled_time

robot = cozmo.robot.Robot
custom_objects=[]
found_object=None
freeplay=0
marker_time=0
scheduled_days="None"
scheduled_time=0
#
#============================
# CONFIGURABLE VARIABLES HERE
#============================
#
# BATTERY VOLTAGE THRESHOLDS
#
# lowbatvolt - when voltage drops below this Cozmo will start looking for his charger
# highbatvolt - when cozmo comes off your charger fully charge, this value will self-calibrate
# maxbatvolt - the maximum battery level as recorded when cozmo is on charger but no longer charging
# tweak lowbatvolt to suit your cozmo. default value is 3.65
lowbatvolt = 3.65
#highbatvolt= 4.04 - currently not used
#maxbatvolt = 4.54 - currently not used
#
# COZMO'S VOICE VOLUME
# what volume Cozmo should play sounds at, value between 0 and 1
robotvolume = 0.05
#
# CUBE USAGE
#
# whether or not to activate the cubes (saves battery if you don't)
# I almost always leave this off, he will still stack them and mess around with them
# some games like "block guard dog" will not come up unless the blocks are active
#
# value of 0: cubes off & cube's lights off
# value of 1: cubes on & enables all games
# value of 2: cubes on & cube's freeplay lights on & in-schedule breathing lights off
# value of 3: cubes on & cube's freeplay lights on & in-schedule breathing lights on
# value between 0 and 3
use_cubes = 3
# 
# 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 = 1
#
# SCHEDULER USAGE
#
# whether or not to use the schedule to define allowed "play times"
# this code is a bit rough, use at your own risk
# value between 0 and 1
use_scheduler = 1
#
# SCHEDULE START & STOP SETTING IN 24HR TIME FORMAT ex. 0 to 23
#
if use_scheduler == 1:
	# Initialize Scheduler Variables
	global wkDS, wkDSm, wkDE, wkDEm, wkNS, wkNSm, wkNE, wkNEm, wkEDS, wkEDSm, wkEDE, wkEDEm, wkENS, wkENSm, wkENE, wkENEm, wday
	# Week Days
	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 Nights
	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 Days
	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 Nights
	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
#
	#COZMO SCHEDULER DEFAULT TIME SETTINGS:
	## Week Days
	#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 Nights
	#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 Days
	#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 Nights
	#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
#
## define goto_chargermarker_pose behaviour
def goto_chargermarker_pose(robot: robot):
	global charger, custom_objects, found_object
	counter=0
	while robot.world.charger == None:
		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=False)
			try:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: custom object found, traveling, battery %s" % str(round(robot.battery_voltage, 2)))
				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:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: failed to goto pose, battery %s" % str(round(robot.battery_voltage, 2)))
			robot.wait_for_all_actions_completed()
			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:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2)))
			try:
				robot.set_head_angle(degrees(0)).wait_for_completed()
			except:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: failed to set head angle, battery %s" % str(round(robot.battery_voltage, 2)))
			robot.wait_for_all_actions_completed()
			time.sleep(3)
			if robot.world.charger != None:
				charger = robot.world.charger
				robot.world.charger = None
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: found charger after goto chargermarker, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
				except:
					print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					robot.drive_straight(distance_mm(-120), speed_mmps(40)).wait_for_completed()
					robot.wait_for_all_actions_completed()
				except:
					print("State: failed to drive straight, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					action = robot.go_to_object(charger, distance_mm(100), RobotAlignmentTypes.LiftPlate, num_retries=3)
					action.wait_for_completed()
					robot.wait_for_all_actions_completed()
				except:
					print("State: failed to goto object, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.LiftPlate, num_retries=3)
					action.wait_for_completed()
					robot.wait_for_all_actions_completed()
				except:
					print("State: failed to goto object, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					action = robot.go_to_object(charger, distance_mm(60), RobotAlignmentTypes.LiftPlate, num_retries=3)
					action.wait_for_completed()
					robot.wait_for_all_actions_completed()
				except:
					print("State: failed to goto object, battery %s" % str(round(robot.battery_voltage, 2)))
				finally:
					break
			time.sleep(1)
			if not robot.world.charger:
				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:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: can't see charger, backup a bit, battery %s" % str(round(robot.battery_voltage, 2)))
					robot.drive_straight(distance_mm(-80), speed_mmps(40)).wait_for_completed()
					robot.wait_for_all_actions_completed()
				except:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: failed to drive straight, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
				except:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2)))
			robot.wait_for_all_actions_completed()
			time.sleep(2)
			try:
				charger = robot.world.wait_for_observed_charger(timeout=3, include_existing=True)
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: found charger after backup!, battery %s" % str(round(robot.battery_voltage, 2)))
				break
			except asyncio.TimeoutError:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: i don't see the charger, battery %s" % str(round(robot.battery_voltage, 2)))
			time.sleep(0.5)
			if not robot.world.charger:
				try:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: can't see charger, backup a bit more, battery %s" % str(round(robot.battery_voltage, 2)))
					robot.drive_straight(distance_mm(-60), speed_mmps(40)).wait_for_completed()
					robot.wait_for_all_actions_completed()
				except:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: failed to drive straight, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					robot.turn_in_place(degrees(found_object.pose.rotation.angle_z.degrees), num_retries=2, accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
				except:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					charger = robot.world.wait_for_observed_charger(timeout=3, include_existing=True)
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: found charger after backup!, battery %s" % str(round(robot.battery_voltage, 2)))
					break
				except asyncio.TimeoutError:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: i still don't see the charger, battery %s" % str(round(robot.battery_voltage, 2)))
			time.sleep(1)
			robot.set_head_angle(degrees(10)).wait_for_completed()
			robot.wait_for_all_actions_completed()
			time.sleep(1)
			if found_object.is_visible==False:
				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:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: failed to turn in place, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: can't see chargermarker, backup a lot, battery %s" % str(round(robot.battery_voltage, 2)))
					robot.drive_straight(distance_mm(-100), speed_mmps(40)).wait_for_completed()
				except:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: failed to drive straight, battery %s" % str(round(robot.battery_voltage, 2)))
				try:
					charger = robot.world.wait_for_observed_charger(timeout=3, include_existing=True)
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: found charger after backup!, battery %s" % str(round(robot.battery_voltage, 2)))
					break
				except asyncio.TimeoutError:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: did not find charger, try again, battery %s" % str(round(robot.battery_voltage, 2)))
			robot.wait_for_all_actions_completed()
			time.sleep(3)
			if charger == None and robot.world.charger == None:
				counter+=1
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: chargermarker goto retry: "+str(counter))
			if counter > 1:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: chargermarker goto failed, resetting, battery %s" % str(round(robot.battery_voltage, 2)))
				break
	custom_objects = None
	found_object = None

## define look for charger and goto routine
def find_charger_and_goto(robot: robot):
	global charger, freeplay, found_object, custom_objects, temp_object, marker_time, lowbatvolt, scheduled_time
	# we don't know where the charger is
	if not charger:
		# we will look around in place for the charger for 30 seconds
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("State: look around for charger, battery %s" % str(round(robot.battery_voltage, 2)))
		robot.play_anim_trigger(cozmo.anim.Triggers.SparkIdle, ignore_body_track=True).wait_for_completed()
		robot.set_head_angle(degrees(0)).wait_for_completed()
		robot.move_lift(-3)
		robot.wait_for_all_actions_completed()
		time.sleep(0.1)
		robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
		current_time = time.time()
		end_time = time.time() + 35.0
		while current_time < end_time:
			if found_object != None and str(found_object.object_type) == "CustomObjectTypes.CustomType01":
				robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop()
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: chargermarker still known, battery %s" % str(round(robot.battery_voltage, 2)))
				goto_chargermarker_pose(robot)
				custom_objects = None
				break
			try:
				charger = robot.world.wait_for_observed_charger(timeout=0.1, include_existing=True)
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: found charger in lookaround!, battery %s" % str(round(robot.battery_voltage, 2)))
				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()
					finally:
						pass
					try:
						robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed()
					finally:
						break
			except asyncio.TimeoutError:
				pass
			time.sleep(0.01)
			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.1)
				if custom_objects != None:
					if len(custom_objects) > 0 and str(custom_objects[0].object_type) == "CustomObjectTypes.CustomType01":
						#os.system('cls' if os.name == 'nt' else 'clear')
						print("State: found chargermarker in lookaround!, battery %s" % str(round(robot.battery_voltage, 2)))
						robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop()
						found_object = custom_objects[0]
						custom_objects=None
						break
				if custom_objects != None:
					custom_objects=None
			if current_time > end_time:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: lookaround time ended, didn't find charger or marker, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop()
				break
			current_time = time.time()
			time.sleep(0.1)
		# lookaround is stopped
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("State: stop lookaround, battery %s" % str(round(robot.battery_voltage, 2)))
		#robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace).stop()
		robot.wait_for_all_actions_completed()
		time.sleep(1)
		if found_object != None and charger==None:
			time.sleep(1)
			goto_chargermarker_pose(robot)
			custom_objects=None
	time.sleep(1)
	# Docking routines & fallback routines are here
	if charger:
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("State: docking mode, moving to charger, battery %s" % str(round(robot.battery_voltage, 2)))
		while (robot.is_on_charger == 0):
			# check to make sure we didn't get here from a battery spike
			if (robot.battery_voltage > lowbatvolt + 0.03) and (scheduled_time == 1):
				break
			# Yes! Attempt to drive near to the charger, and then stop.
			robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabChatty, ignore_lift_track=True, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
			robot.wait_for_all_actions_completed()
			time.sleep(1)
			robot.move_lift(-3)
			robot.set_head_angle(degrees(0)).wait_for_completed()
			# If you are running Cozmo on carpet, try turning these num_retries up to 4 or 5
			try:
				action = robot.go_to_object(charger, distance_mm(160), RobotAlignmentTypes.LiftPlate, num_retries=2)
				action.wait_for_completed()
			finally:
				pass
			try:
				action = robot.go_to_object(charger, distance_mm(120), RobotAlignmentTypes.LiftPlate, num_retries=2)
				action.wait_for_completed()
			finally:
				pass
			try:
				action = robot.go_to_object(charger, distance_mm(100), RobotAlignmentTypes.LiftPlate, num_retries=3)
				action.wait_for_completed()
			finally:
				pass
			try:
				action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.LiftPlate, num_retries=3)
				action.wait_for_completed()
			finally:
				pass
			try:
				action = robot.go_to_object(charger, distance_mm(60), RobotAlignmentTypes.LiftPlate, num_retries=3)
				action.wait_for_completed()
			finally:
				pass
			time.sleep(2)
			try:
				robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=2).wait_for_completed()
			finally:
				pass
			try:
				robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed()
			finally:
				pass
			try:
				action = robot.go_to_object(charger, distance_mm(90), RobotAlignmentTypes.LiftPlate, num_retries=3)
				action.wait_for_completed()
			finally:
				pass
			try:
				action = robot.go_to_object(charger, distance_mm(60), RobotAlignmentTypes.LiftPlate, num_retries=3)
				action.wait_for_completed()
			finally:
				pass
			try:
				action = robot.go_to_object(charger, distance_mm(40), RobotAlignmentTypes.LiftPlate, num_retries=3)
				action.wait_for_completed()
			finally:
				pass
			# we should be right in front of the charger, can we see it?
			time.sleep(1)
			if (charger.is_visible == False):
				# we know where the charger is but can't see it
				# is robot flipped over or picked up?
				try:
					if robot.is_picked_up == True:
						robot.stop_all_motors()
						robot.abort_all_actions(False)
						robot.clear_idle_animation()
						robot.wait_for_all_actions_completed()
						time.sleep(1)
						charger = None
						robot.world.charger = None
						temp_object = None
						#os.system('cls' if os.name == 'nt' else 'clear')
						print("State: robot delocalized during docking, resetting, battery %s" % str(round(robot.battery_voltage, 2)))
						break
				finally:
					pass
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: can't see charger, position is still known, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
				robot.set_head_angle(degrees(0)).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(2)
				#os.system('cls' if os.name == 'nt' else 'clear')
				print(str(charger))
				# charger is not in front of us, backup and try again
				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()
				finally:
					pass
				try:
					action = robot.drive_straight(distance_mm(-140), speed_mmps(40))
					action.wait_for_completed()
				finally:
					pass
				try:
					action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.LiftPlate, num_retries=2)
					action.wait_for_completed()
				finally:
					pass
				try:
					action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.LiftPlate, num_retries=2)
					action.wait_for_completed()
				finally:
					pass
				robot.wait_for_all_actions_completed()
				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()
				robot.drive_straight(distance_mm(-50), speed_mmps(50)).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(1)
				if (charger.is_visible == False):
					# we don't know where the charger is, clear all locations
					robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
					robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed()
					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()
					robot.wait_for_all_actions_completed()
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: charger not found, clearing map, battery %s" % str(round(robot.battery_voltage, 2)))
					charger = None
					robot.world.charger = None
					temp_object = None
					time.sleep(1)
					break
				else:
					break
			robot.enable_all_reaction_triggers(False)
			i = random.randint(1, 100)
			if i >= 85:
				robot.play_anim_trigger(cozmo.anim.Triggers.FeedingReactToShake_Normal, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(2)
			# charger should be in front of us. Turn around, drive backwards, and dock
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: docking, battery %s" % str(round(robot.battery_voltage, 2)))
			time.sleep(2.5)
			if robot.is_animating:
				robot.clear_idle_animation()
				robot.abort_all_actions(log_abort_messages=True)
				robot.wait_for_all_actions_completed()
			robot.wait_for_all_actions_completed()
			robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees - 180), accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
			robot.wait_for_all_actions_completed()
			time.sleep(0.1)
			robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake, ignore_body_track=True, ignore_lift_track=True, ignore_head_track=True).wait_for_completed()
			robot.set_head_angle(degrees(0)).wait_for_completed()
			robot.wait_for_all_actions_completed()
			#robot.clear_idle_animation()
			time.sleep(1)
			backup_count = 0
			while robot.is_on_charger == 0:
				try:
					robot.backup_onto_charger(max_drive_time=2)
					backup_count+=1
					time.sleep(0.4)
					if robot.is_on_charger == 1:
						print("State: Robot is on Charger, battery %s" % str(round(robot.battery_voltage, 2)))
						robot.enable_all_reaction_triggers(False)
						break
					elif backup_count == 6:
						robot.enable_all_reaction_triggers(False)
						break
				except robot.is_on_charger == 1:
					robot.stop_all_motors()
					print("State: Contact! Stop all motors, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.wait_for_all_actions_completed()
			#try:
			#	robot.backup_onto_charger(max_drive_time=6)
			#except robot.is_on_charger == 1:
			#	robot.stop_all_motors()
			#finally:
			#	robot.enable_all_reaction_triggers(False)
			#robot.wait_for_all_actions_completed()
			#time.sleep(2)
			#try:
			#	if robot.is_on_charger == 0:
			#		#os.system('cls' if os.name == 'nt' else 'clear')
			#		print("State: bumped into something, backup a bit more, battery %s" % str(round(robot.battery_voltage, 2)))
			#		robot.backup_onto_charger(max_drive_time=3)
			#except robot.is_on_charger == 1:
			#	robot.stop_all_motors()
			#finally:
			#	robot.enable_all_reaction_triggers(False)
			#	time.sleep(2)
			robot.wait_for_all_actions_completed()
			# check if we're now docked
			if robot.is_on_charger:
				# Yes! we're docked!
				robot.play_anim("anim_sparking_success_02").wait_for_completed()
				robot.set_head_angle(degrees(0)).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(1.5)
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: I am now docked, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.set_all_backpack_lights(cozmo.lights.off_light)
				robot.play_anim("anim_gotosleep_getin_01").wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(1)
				robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(1)
			else:
				# No, we missed. Drive forward, turn around, and try again
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: failed to dock with charger, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.play_anim_trigger(cozmo.anim.Triggers.AskToBeRightedRight, ignore_body_track=True).wait_for_completed()
				#robot.move_lift(-3)
				robot.set_head_angle(degrees(0)).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(0.5)
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: drive forward, turn around, and try again, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees - 180), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=2).wait_for_completed()
				robot.drive_straight(distance_mm(90), speed_mmps(90)).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(0.5)
				robot.drive_straight(distance_mm(90), speed_mmps(90)).wait_for_completed()
				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()
				robot.set_head_angle(degrees(0)).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(1)
				if (charger.is_visible == False):
					# we know where the charger is
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: can't see charger after turnaround, try again, battery %s" % str(round(robot.battery_voltage, 2)))
					robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
					robot.set_head_angle(degrees(0)).wait_for_completed()
					robot.wait_for_all_actions_completed()
					#os.system('cls' if os.name == 'nt' else 'clear')
					print(str(charger))
					try:
						action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.LiftPlate, num_retries=2)
						action.wait_for_completed()
					finally:
						pass
					try:
						action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.LiftPlate, num_retries=2)
						action.wait_for_completed()
					finally:
						pass
					robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True, num_retries=2).wait_for_completed()
					robot.wait_for_all_actions_completed()
					if (charger.is_visible == False):
						# we lost the charger, clearing all map variables
						charger = None
						robot.world.charger = None
						temp_object = None
						robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
						robot.drive_straight(distance_mm(-120), speed_mmps(50)).wait_for_completed()
						robot.wait_for_all_actions_completed()
						#os.system('cls' if os.name == 'nt' else 'clear')
						print("State: charger lost while docking, clearing map, battery %s" % str(round(robot.battery_voltage, 2)))
						time.sleep(1)
						break
				if robot.world.charger != None:
					charger = robot.world.charger
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: update charger location, battery %s" % str(round(robot.battery_voltage, 2)))
					#os.system('cls' if os.name == 'nt' else 'clear')
					print(str(charger))
					robot.world.charger = None
	else:
	# we have not managed to find the charger. Falling back to freeplay.
		charger = None
		robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
		robot.wait_for_all_actions_completed()
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("State: charger unknown, fallback to freeplay for 90sec, battery %s" % str(round(robot.battery_voltage, 2)))
		robot.enable_all_reaction_triggers(True)
		robot.start_freeplay_behaviors()
		#try freeplay for 90 seconds, if found charger finally stop freeplay
		current_time = time.time()
		end_time = time.time() + 95.0
		while current_time < end_time:
			if found_object != None and str(found_object.object_type) == "CustomObjectTypes.CustomType01":
				robot.stop_freeplay_behaviors()
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: chargermarker still known, battery %s" % str(round(robot.battery_voltage, 2)))
				goto_chargermarker_pose(robot)
				custom_objects = None
				break
			try:
				charger = robot.world.wait_for_observed_charger(timeout=0.1, include_existing=True)
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: found the charger in 90sec playtime!, battery %s" % str(round(robot.battery_voltage, 2)))
				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()
					finally:
						pass
					try:
						robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed()
					finally:
						break
			except asyncio.TimeoutError:
				pass
			time.sleep(0.01)
			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.1)
				if custom_objects != None:
					if len(custom_objects) > 0 and str(custom_objects[0].object_type) == "CustomObjectTypes.CustomType01":
						#os.system('cls' if os.name == 'nt' else 'clear')
						print("State: found the chargermarker in 90sec playtime!, battery %s" % str(round(robot.battery_voltage, 2)))
						robot.stop_freeplay_behaviors()
						found_object = custom_objects[0]
						custom_objects=None
						break
				if custom_objects != None:
					custom_objects=None
			if time.time() > end_time:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: freeplay time ended, didn't find charger or marker, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.stop_freeplay_behaviors()
				break
			current_time = time.time()
			time.sleep(0.1)
		#after 90 seconds end freeplay
		robot.set_needs_levels(1, 1, 1)
		robot.world.disconnect_from_cubes()
		time.sleep(3)
		robot.wait_for_all_actions_completed()
		time.sleep(1)
		if found_object != None and charger==None:
			time.sleep(1)
			goto_chargermarker_pose(robot)
			custom_objects=None
	time.sleep(1)
#
# Cozmo's Scheduler resting time
#
def cozmo_resting(robot: robot):
	if (robot.is_on_charger == 1) and (robot.is_charging == 1):
		# In here we make Cozmo do on or off Charger things while Charging for fun
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("State: charging, waiting for scheduled playtime, battery %s" % str(round(robot.battery_voltage, 2)))
		i = random.randint(1, 100)
		if i >= 97:
			robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
		elif i >= 85:
			robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
		time.sleep(3)
		robot.set_all_backpack_lights(cozmo.lights.green_light)
		time.sleep(3)
		robot.set_all_backpack_lights(cozmo.lights.off_light)
	elif (robot.is_on_charger == 1) and (robot.is_charging == 0):
		# In here we make Cozmo do on or off Charger things while Charged for fun
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("State: Charged! waiting for scheduled playtime, battery %s" % str(round(robot.battery_voltage, 2)))
		i = random.randint(1, 100)
		if i >= 97:
			robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
		elif i >= 85:
			robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
		time.sleep(3)
		robot.set_all_backpack_lights(cozmo.lights.green_light)
		time.sleep(3)
		robot.set_all_backpack_lights(cozmo.lights.off_light)
	elif (robot.is_on_charger == 0) and (robot.is_charging == 0) and scheduled_days == "Resting":
		robot.stop_freeplay_behaviors()
		find_charger_and_goto(robot)
#
# Scheduler
#
def scheduler(robot: robot):
	global use_scheduler, nowtime, wday, scheduled_days, scheduled_time
	global wkDS, wkDSm, wkDE, wkDEm, wkNS, wkNSm, wkNE, wkNEm, wkEDS, wkEDSm, wkEDE, wkEDEm, wkENS, wkENSm, wkENE, wkENEm
	wday = nowtime.tm_wday # 0 is Monday, 6 is Sunday
	#TempTime = datetime.datetime.now().timetuple()
	
	# Display Time Tuple for scripting usage purposes
	#os.system('cls' if os.name == 'nt' else 'clear')
	#print("State: Full timetuple() is: "+str(nowtime))

	while True:
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("State: Scheduled days: "+str(scheduled_days)+", battery %s" % str(round(robot.battery_voltage, 2)))
		#weekday mornings
		if (wday < 5 and nowtime.tm_hour > wkDS or wday < 5 and (wkDE < wkDS and nowtime.tm_hour > wkDS or nowtime.tm_hour == wkDS and nowtime.tm_min >= wkDSm)) and (nowtime.tm_hour < wkDE or wkDE < wkDS and nowtime.tm_hour > wkDE or nowtime.tm_hour == wkDE and nowtime.tm_min <= wkDEm):
			scheduled_days = "WeekDayMorn"
			scheduled_time = 1
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: Weekday Morning Time is: "+str(nowtime.tm_hour)+":"+str(nowtime.tm_min)+", battery %s" % str(round(robot.battery_voltage, 2)))
			cozmo_unleashed(robot)
		#weekday evenings
		elif (wday < 5 and nowtime.tm_hour > wkNS or wday < 5 and (wkNE < wkNS and nowtime.tm_hour > wkNS or nowtime.tm_hour == wkNS and nowtime.tm_min >= wkNSm)) and (nowtime.tm_hour < wkNE or wkNE < wkNS and nowtime.tm_hour > wkNE or nowtime.tm_hour == wkNE and nowtime.tm_min <= wkNEm):
			scheduled_days = "WeekDayEve"
			scheduled_time = 1
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: Weekday Night Time is: "+str(nowtime.tm_hour)+":"+str(nowtime.tm_min)+", battery %s" % str(round(robot.battery_voltage, 2)))
			cozmo_unleashed(robot)
		#weekend mornings
		elif (wday >= 5 and nowtime.tm_hour > wkEDS or wday >= 5 and (wkEDS < wkEDE and nowtime.tm_hour > wkEDS or nowtime.tm_hour == wkEDS and nowtime.tm_min >= wkEDSm)) and (nowtime.tm_hour < wkEDE or wkEDE < wkEDS and nowtime.tm_hour > wkEDE or nowtime.tm_hour == wkEDE and nowtime.tm_min <= wkEDEm):
			scheduled_days = "WeekEndMorn"
			scheduled_time = 1
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: Weekend Morning Time is: "+str(nowtime.tm_hour)+":"+str(nowtime.tm_min)+", battery %s" % str(round(robot.battery_voltage, 2)))
			cozmo_unleashed(robot)
		#weekend evenings
		elif (wday >= 5 and nowtime.tm_hour > wkENS or wday >= 5 and (wkENS < wkENE and nowtime.tm_hour > wkENS or nowtime.tm_hour == wkENS and nowtime.tm_min >= wkENSm)) and (nowtime.tm_hour < wkENE or wkENE < wkENS and nowtime.tm_hour > wkENE or nowtime.tm_hour == wkENE and nowtime.tm_min <= wkENEm):
			scheduled_days = "WeekEndEve"
			scheduled_time = 1
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: Weekend Evening Time is: "+str(nowtime.tm_hour)+":"+str(nowtime.tm_min)+", battery %s" % str(round(robot.battery_voltage, 2)))
			cozmo_unleashed(robot)
		else:
			nowtime = datetime.datetime.now().timetuple()
			scheduled_days = "Resting"
			scheduled_time = 0
			cozmo_resting(robot)
#
#
# main program and loops
def cozmo_unleashed(robot: robot):
	global charger, freeplay, robotvolume, found_object, custom_objects, temp_object, marker_time, use_walls, use_cubes
	global use_scheduler, nowtime, wday, scheduled_days, scheduled_time
	if use_scheduler == 1:
		global wkDS, wkDSm, wkDE, wkDEm, wkNS, wkNSm, wkNE, wkNEm, wkEDS, wkEDSm, wkEDE, wkEDEm, wkENS, wkENSm, wkENE, wkENEm
	if scheduled_days=="None" or use_scheduler == 0:
		#
		# DEFINING MARKERS
		#
		# 1st 2 numbers are the width/height of the paper the printed marker is on
		# for the 1st dimension: measure the side of the width/height, from the printed marker's edge, then
		# multiply it by 2 then add it to the 2nd dimension
		# 2nd 2 numbers are the width/height of the printed marker on the paper
		# I suggest using a ruler to measure these dimensions, by the millimeter, after you print & cut them to get exact numbers

		# define the chargermarker for the dock
		robot.world.define_custom_wall(CustomObjectTypes.CustomType01, CustomObjectMarkers.Hexagons2, 62, 62, 40, 40, True)

		# these define the 4 walls of my play field, it helps Cozmo not bump into the walls
		# you can set the use_walls variable at the top of the script to 0 if you are not using walls
		if use_walls >= 1:
			robot.world.define_custom_wall(CustomObjectTypes.CustomType02, CustomObjectMarkers.Diamonds2, 800, 90, 40, 40, True)
			robot.world.define_custom_wall(CustomObjectTypes.CustomType03, CustomObjectMarkers.Diamonds3, 800, 90, 40, 40, True)
			robot.world.define_custom_wall(CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 550, 90, 40, 40, True)
			robot.world.define_custom_wall(CustomObjectTypes.CustomType05, CustomObjectMarkers.Diamonds5, 700, 90, 40, 40, True)

		robot.world.charger = None
		charger = None
		found_object=None
		temp_object=None
		custom_objects=[]
		if use_scheduler == 1:
			nowtime = datetime.datetime.now().timetuple()
		robot.wait_for_all_actions_completed()
		robot.clear_idle_animation()
		robot.stop_all_motors()
		robot.abort_all_actions(log_abort_messages=False)
		robot.enable_all_reaction_triggers(False)
		robot.enable_stop_on_cliff(True)
		robot.world.auto_disconnect_from_cubes_at_end(True)
		robot.set_robot_volume(robotvolume)
		time.sleep(1)
		# cube_lights(enable=True) if you are using freeplay lights, set False to save battery
		if use_cubes >= 2:
			robot.enable_freeplay_cube_lights(enable=True)
		time.sleep(2)
	while True:
#State 1: on charger, charging
		if (robot.is_on_charger == 1) and (robot.is_charging == 1):
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: charging, battery %s" % str(round(robot.battery_voltage, 2)))
			freeplay=0
			if scheduled_time == 1:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: charging, in scheduled playtime, battery %s" % str(round(robot.battery_voltage, 2)))
			else:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: charging, battery %s" % str(round(robot.battery_voltage, 2)))
			i = random.randint(1, 100)
			if i >= 97:
				robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
				robot.wait_for_all_actions_completed()
			elif i >= 85:
				robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
				robot.wait_for_all_actions_completed()
			time.sleep(1.5)
			robot.set_all_backpack_lights(cozmo.lights.green_light)
			time.sleep(1.5)
			robot.set_all_backpack_lights(cozmo.lights.off_light)
#State 2: on charger, fully charged
		if (robot.is_on_charger == 1) and (robot.is_charging == 0) and (scheduled_days != "Resting") and (scheduled_time == 1 or use_scheduler == 0):
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: charged, battery %s" % str(round(robot.battery_voltage, 2)))
			robot.set_needs_levels(1, 1, 1)
			robot.set_all_backpack_lights(cozmo.lights.off_light)
			robot.play_anim("anim_launch_altwakeup_01").wait_for_completed()
			robot.wait_for_all_actions_completed()
			time.sleep(2)
			robot.drive_off_charger_contacts(num_retries=3, in_parallel=False).wait_for_completed()
			robot.move_lift(-3)
			try:
				if (robot.is_on_charger == 1):
					robot.drive_straight(distance_mm(80), speed_mmps(25), num_retries=2).wait_for_completed()
			finally:
				pass
			robot.enable_all_reaction_triggers(True)
			time.sleep(2)
			robot.wait_for_all_actions_completed()
		elif use_scheduler == 1 and scheduled_days == "None":
			scheduler(robot)
#State 3: not on charger, good time play battery | default low battery is 3.65
		if (robot.battery_voltage > lowbatvolt) and (robot.is_on_charger == 0) and (scheduled_time == 1 or use_scheduler == 0):
			if use_scheduler == 1 and scheduled_days == "None":
				scheduler(robot)
			elif freeplay == 0:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: turn on good time play battery %s" % str(round(robot.battery_voltage, 2)))
				robot.clear_idle_animation()
				robot.abort_all_actions(log_abort_messages=False)
				if use_cubes == 1:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: turn on cubes %s" % str(round(robot.battery_voltage, 2)))
					robot.world.connect_to_cubes()
					time.sleep(5)
				elif use_cubes >= 2:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: turn on cubes and freeplay lights %s" % str(round(robot.battery_voltage, 2)))
					robot.enable_freeplay_cube_lights(True)
					robot.world.connect_to_cubes()
					time.sleep(5)
				robot.enable_all_reaction_triggers(True)
				robot.start_freeplay_behaviors()
				freeplay=1
			elif freeplay == 1 and robot.battery_voltage > lowbatvolt - 0.03:
				if use_cubes >= 2:
					robot.enable_freeplay_cube_lights(True)
				pass
			if robot.is_picked_up == True:
				charger = None
				robot.world.charger = None
				temp_object = None
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: robot delocalized during freeplay, resetting everything, battery %s" % str(round(robot.battery_voltage, 2)))
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: good battery playtime, battery %s" % str(round(robot.battery_voltage, 2)))
			time.sleep(3)
#State 4: not on charger, low battery | default low battery is 3.65
		if (robot.battery_voltage <= lowbatvolt) and (robot.is_on_charger == 0):
			if freeplay == 1:
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: turn off good battery freeplay, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.stop_all_motors()
				robot.clear_idle_animation()
				robot.enable_all_reaction_triggers(False)
				robot.abort_all_actions(log_abort_messages=False)
				robot.wait_for_all_actions_completed()
				time.sleep(1)
				robot.stop_freeplay_behaviors()
				if use_cubes == 1 or use_cubes == 3:
					robot.world.disconnect_from_cubes()
					time.sleep(5)
				elif use_cubes == 2:
					robot.enable_freeplay_cube_lights(False)
					robot.world.disconnect_from_cubes()
					time.sleep(5)
				freeplay=0
				robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(2)
				robot.set_all_backpack_lights(cozmo.lights.blue_light)
				robot.set_head_angle(degrees(0)).wait_for_completed()
				robot.move_lift(-3)
				robot.wait_for_all_actions_completed()
				time.sleep(0.1)
				#robot.drive_straight(distance_mm(-30), speed_mmps(50)).wait_for_completed()
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: low battery, finding charger, battery %s" % str(round(robot.battery_voltage, 2)))
			robot.enable_all_reaction_triggers(True)
			# charger location search
			# see if we already know where the charger is
			if charger != None:
				#we know where the charger is
				robot.move_lift(-3)
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: start docking loop, charger position known, battery %s" % str(round(robot.battery_voltage, 2)))
				robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
				robot.set_head_angle(degrees(0)).wait_for_completed()
				robot.wait_for_all_actions_completed()
				time.sleep(0.01)
				#os.system('cls' if os.name == 'nt' else 'clear')
				print(str(charger))
				if charger != None:
					try:
						action = robot.go_to_object(charger, distance_mm(110), RobotAlignmentTypes.LiftPlate, num_retries=3)
						action.wait_for_completed()
					finally:
						pass
					try:
						action = robot.go_to_object(charger, distance_mm(80), RobotAlignmentTypes.LiftPlate, num_retries=3)
						action.wait_for_completed()
					finally:
						pass
					robot.turn_in_place(degrees(charger.pose.rotation.angle_z.degrees), accel=degrees(80), angle_tolerance=None, is_absolute=True).wait_for_completed()
					robot.wait_for_all_actions_completed()
					time.sleep(1)
			elif found_object != None:
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: start of loop, chargermarker known, battery %s" % str(round(robot.battery_voltage, 2)))
					robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
					robot.set_head_angle(degrees(0)).wait_for_completed()
					robot.wait_for_all_actions_completed()
					time.sleep(0.01)
					goto_chargermarker_pose(robot)
			else:
				# we know where the charger is but we have been delocalized
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: did not find charger or marker after clearing map, battery %s" % str(round(robot.battery_voltage, 2)))
				if use_cubes == 1 or use_cubes == 3:
					robot.world.disconnect_from_cubes()
					time.sleep(4)
				elif use_cubes == 2:
					robot.enable_freeplay_cube_lights(enable=False)
					robot.world.disconnect_from_cubes()
					time.sleep(4)
			## decide if we need to run find charger routines or not
			find_charger_and_goto(robot)

		## end of loop routines
		if robot.is_picked_up == True:
			charger = None
			robot.world.charger = None
			temp_object = None
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: robot delocalized during freeplay, resetting charger, battery %s" % str(round(robot.battery_voltage, 2)))
		while temp_object == None:
			try:
				custom_objects = robot.world.wait_until_observe_num_objects(num=1, object_type=CustomObject, timeout=2, include_existing=True)
				if len(custom_objects) > 0:
					temp_object = custom_objects[0]
					custom_objects=[]
				if str(temp_object.object_type) != "CustomObjectTypes.CustomType01":
					temp_object=None
			finally:
				if temp_object != None and str(temp_object.object_type) == "CustomObjectTypes.CustomType01":
					if found_object == None:
						#os.system('cls' if os.name == 'nt' else 'clear')
						print("State: found chargermarker in freeplay, battery %s" % str(round(robot.battery_voltage, 2)))
						found_object = temp_object
						temp_object = None
						break
					if temp_object != None and found_object != None:
						if temp_object.pose.is_comparable(found_object.pose) == True:
							#os.system('cls' if os.name == 'nt' else 'clear')
							print("State: updating chargermarker location, battery %s" % str(round(robot.battery_voltage, 2)))
							found_object = temp_object
						else:
							#os.system('cls' if os.name == 'nt' else 'clear')
							print("State: chargermarker pose comparison failed, battery %s" % str(round(robot.battery_voltage, 2)))
				temp_object = None
				break
		#time.sleep(0.5)
		if robot.world.charger != None:
			charger = robot.world.charger
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: updating charger location, battery %s" % str(round(robot.battery_voltage, 2)))
			#os.system('cls' if os.name == 'nt' else 'clear')
			print("State: "+str(charger)+", battery %s" % str(round(robot.battery_voltage, 2)))
			robot.world.charger = None
			robot.clear_idle_animation()
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("State: program loop complete, battery %s" % str(round(robot.battery_voltage, 2)))
		time.sleep(3)
		if use_scheduler == 1:
			nowtime = datetime.datetime.now().timetuple()
			if (scheduled_days == "WeekDayMorn" and (nowtime.tm_hour <= wkDS and nowtime.tm_min <= wkDSm) or (wkDE < wkDS and nowtime.tm_hour <= wkDS and nowtime.tm_min <= wkDSm)) or (scheduled_days == "WeekDayMorn" and (nowtime.tm_hour >= wkDE and nowtime.tm_min >= wkDEm) or (wkDE < wkDS and nowtime.tm_hour >= wkDE and nowtime.tm_min >= wkDEm)):
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: Weekday Morning Schedule, battery %s" % str(round(robot.battery_voltage, 2)))
				scheduler(robot)
			elif (scheduled_days == "WeekDayEve" and (nowtime.tm_hour <= wkNS and nowtime.tm_min <= wkNSm) or (wkNE < wkNS and nowtime.tm_hour <= wkNS and nowtime.tm_min <= wkNSm)) or (scheduled_days == "WeekDayEve" and (nowtime.tm_hour >= wkNE and nowtime.tm_min >= wkNEm) or (wkNE < wkNS and nowtime.tm_hour >= wkNE and nowtime.tm_min >= wkNEm)):
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: Weekday Evening Schedule, battery %s" % str(round(robot.battery_voltage, 2)))
				scheduler(robot)
			elif (scheduled_days == "WeekEndMorn" and (nowtime.tm_hour <= wkEDS and nowtime.tm_min <= wkEDSm) or (wkEDE < wkEDS and nowtime.tm_hour <= wkEDS and nowtime.tm_min <= wkEDSm)) or (scheduled_days == "WeekEndMorn" and (nowtime.tm_hour >= wkEDE and nowtime.tm_min >= wkEDEm) or (wkEDE < wkEDS and nowtime.tm_hour >= wkEDE and nowtime.tm_min >= wkEDEm)):
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: Weekend Schedule, battery %s" % str(round(robot.battery_voltage, 2)))
				scheduler(robot)
			elif (scheduled_days == "WeekEndEve" and (nowtime.tm_hour <= wkENS and nowtime.tm_min <= wkENSm) or (wkENE < wkENS and nowtime.tm_hour <= wkENS and nowtime.tm_min <= wkENSm)) or (scheduled_days == "WeekEndEve" and (nowtime.tm_hour >= wkENE and nowtime.tm_min >= wkENEm) or (wkENE < wkENS and nowtime.tm_hour >= wkENE and nowtime.tm_min >= wkENEm)):
				#os.system('cls' if os.name == 'nt' else 'clear')
				print("State: Weekend Schedule, battery %s" % str(round(robot.battery_voltage, 2)))
				scheduler(robot)
			elif scheduled_days == "Resting" and scheduled_time == 0:
				if use_cubes == 3:
					robot.enable_freeplay_cube_lights(enable=True)
				if robot.is_on_charger == 0:
					robot.stop_freeplay_behaviors()
					time.sleep(3)
					#os.system('cls' if os.name == 'nt' else 'clear')
					print("State: Resting Time Schedule, Find Charger, battery %s" % str(round(robot.battery_voltage, 2)))
					find_charger_and_goto(robot)
				else:	
					scheduler(robot)
		#if use_scheduler == 1:
		#	#os.system('cls' if os.name == 'nt' else 'clear')
		#	print("State: End of Loop, How did we get here?, battery %s" % str(round(robot.battery_voltage, 2)))
		#	break
#
#
# START THE SHOW!
#
#cozmo.robot.Robot.drive_off_charger_on_connect = False
#cozmo.run_program(cozmo_unleashed, use_viewer=True, force_viewer_on_top=True, show_viewer_controls=True, exit_on_connection_error=True)

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

# --- Above ^^ is the common command for running the first def.
# --- Below -- is a script from acidzebra for loading the main program, that I will
# like to eventually use to turn into a SDK Loop that can stay running in an executed
# program so that if the WiFi disconnects & reconnects again, the SDK Loop will kick
# back in when it sees a phone with an open SDK connection again, without having to
# stop and restart the script manually.

#  Run at your own RISK, using the AbstractEventLoop is dangerous.

def run(sdk_conn_loop):
	global robot
	'''The run method runs once the Cozmo SDK is connected.'''
	robot = sdk_conn_loop.wait_for_robot(timeout=1)

	try:
		#cozmo_unleashed(robot)
		# ^^ this above line ^^ is the original safest call to use

		# this connect_on_loop seems to work, though i don't think it actually reconnects
		# the phone to cozmo as i intended it to. i'm still working on that part
		# this AbstractEventLoop line might cause issues, use at your own risk
		#connect_on_loop(AbstractEventLoop.run_forever(cozmo_unleashed(robot)), sdk_conn_loop)
		# this non-Abstract connect_on_loop is much safer, use this line below
		connect_on_loop(cozmo_unleashed(robot), sdk_conn_loop)

	except KeyboardInterrupt as k:
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("")
		#os.system('cls' if os.name == 'nt' else 'clear')
		print("Exit requested by user")
		SystemExit("Keyboard interrupt: %s" % k)

if __name__ == '__main__':
	cozmo.setup_basic_logging()
	cozmo.robot.Robot.drive_off_charger_on_connect = False  # Cozmo can stay on charger for now
	try:
		cozmo.connect_with_tkviewer(run, force_on_top=True)
	except cozmo.ConnectionError as e:
		SystemExit("A connection error with viewer occurred: %s" % e)