acidzebra · December 30, 2017 18:09
diff --git a/cozmo_unleashed-devel.py b/cozmo_unleashed-devel.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.
 # 
 #
 # cozmo_unleashed
 #
 # Script based on the Cozmo SDK drive_to_charger example script 
 # and raproenca's modifications of same.
 # rewritten to support battery/charge states and all kinds of bells and whistles by acid zebra
 # 
 import sys
 import os
 import datetime
 import random
 import time
 import asyncio
 import cozmo
 from cozmo.util import degrees, distance_mm, speed_mmps, Pose
 from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes
 global freeplay
 global chargerequired
 freeplay=0
 chargerequired=0
 # main program and loops
 def cozmo_unleashed(robot: cozmo.robot.Robot):
 	global freeplay
 	global chargerequired
 	robot.enable_facial_expression_estimation(enable=True)
 	robot.world.disconnect_from_cubes()
 	robot.enable_all_reaction_triggers(False)
 	robot.enable_stop_on_cliff(True)
 	robot.set_robot_volume(0.04)
 	os.system('cls' if os.name == 'nt' else 'clear')
 	while True:
 #State 1: on charger, charging - wait for full charge
 		if (robot.is_on_charger == 1) and (robot.is_charging == 1):
 			#reset charge required (I know we're still charging, trust me)
 			chargerequired=0
 			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 == 100:
 				robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
 				if (robot.is_on_charger == 0):
 					print("we were taken off charger")
 					break
 				robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
 				if (robot.is_on_charger == 0):
 					print("we were taken off charger")
 					break
 			elif i >= 85:
 				robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
 			else:
 				if (robot.is_on_charger == 0):
 					print("we were taken off charger")
 					break
 				time.sleep(5)
 			time.sleep(5)
 			#TODO: figure out how to smoothly cycle lights
 			robot.set_all_backpack_lights(cozmo.lights.green_light)
 			time.sleep(5)
 			robot.set_all_backpack_lights(cozmo.lights.off_light)
 #State 2: on charger, fully charged - check schedule for go/no go
 		if (robot.is_on_charger == 1) and (robot.is_charging == 0):
 			#reset charge required
 			chargerequired=0
 			# day and time check - are we okay to play at this time and day?
 			day_of_week = datetime.date.today().weekday() # 0 is Monday, 6 is Sunday
 			ctime = datetime.datetime.now().time()
 			playokay=0
 			#it's weekend! Check for allowed times.
 			if day_of_week > 4:
 				if (ctime > datetime.time(9) and ctime < datetime.time(23)):
 					playokay=1
 			#it's a weekday! Check for allowed times.
 			else:
 				if (ctime > datetime.time(7,30) and ctime < datetime.time(22)):
 					playokay=1
 			# if the schedule says roll dice to see if we wake up (20 in 100 chance)
 			if playokay==1:
 				i = random.randint(1, 100)
 				# 20 in 100 chance of waking up every 5 minutes
 				if i >= 80:
 					os.system('cls' if os.name == 'nt' else 'clear')
 					print("State: leaving charger, battery %s" % str(round(robot.battery_voltage, 2)))
 					robot.set_all_backpack_lights(cozmo.lights.off_light)
 					robot.play_anim("anim_gotosleep_getout_02").wait_for_completed()
 					robot.drive_off_charger_contacts().wait_for_completed()
 					robot.drive_off_charger_contacts().wait_for_completed()
 					robot.drive_off_charger_contacts().wait_for_completed()
 					time.sleep(2)
 					robot.move_lift(-3)
 					robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
 					robot.drive_straight(distance_mm(100), speed_mmps(90)).wait_for_completed()
 			# we're out of schedule or didn't make the dice roll, back off for ~5 minutes and check again.
 			x = 1
 			while x < 20 and (robot.is_on_charger == 1):
 				os.system('cls' if os.name == 'nt' else 'clear')
 				if playokay == 1:
 					print("State: charged, schedule OK but not active, sleep loop %d of 30 before next check." % (x))
 				else:
 					print("State: charged,  not active by schedule, sleep loop %d of 30 before next check." % (x))
 				i = random.randint(1, 100)
 				if i == 100:
 					robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
 					robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
 				elif i >= 90:
 					robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
 				else:
 					time.sleep(5)
 					#robot.play_anim("anim_gotosleep_off_01").wait_for_completed()
 				if (robot.is_on_charger == 0):
 					print("we were taken off charger")
 					break
 				time.sleep(5)
 				if (robot.is_on_charger == 0):
 					print("we were taken off charger")
 					break
 				robot.set_all_backpack_lights(cozmo.lights.blue_light)
 				time.sleep(5)
 				if (robot.is_on_charger == 0):
 					print("we were taken off charger")
 					break
 				robot.set_all_backpack_lights(cozmo.lights.off_light)
 				x += 1
 #State 3: not on charger, good battery - freeplay
 		if (robot.battery_voltage > 3.7) and (robot.is_on_charger == 0):
 			os.system('cls' if os.name == 'nt' else 'clear')
 			print("State: freeplay, battery %s" % str(round(robot.battery_voltage, 2)))
 			#initiate freeplay
 			if freeplay==0 and chargerequired==0:
 				print("freeplay active")
 				robot.world.connect_to_cubes()
 				robot.enable_all_reaction_triggers(True)
 				robot.start_freeplay_behaviors()
 				freeplay=1
 			if chargerequired==0:
 				time.sleep(5)
 #State 4: not on charger, low battery, or chargerequired flag is set - seek charger
 		if (robot.battery_voltage <= 3.7)  and (robot.is_on_charger == 0) or (chargerequired==1) and (robot.is_on_charger == 0):
 			#back off from whatever we were doing
 			chargerequired=1
 			if freeplay==1:
 				robot.abort_all_actions(log_abort_messages=False)
 				robot.wait_for_all_actions_completed()
 				robot.stop_freeplay_behaviors()
 				robot.enable_all_reaction_triggers(False)	
 				robot.world.disconnect_from_cubes()
 				freeplay=0
 				robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed()
 				robot.set_head_angle(degrees(0)).wait_for_completed()
 				robot.move_lift(-3)
 				robot.drive_straight(distance_mm(-30), speed_mmps(90)).wait_for_completed()
 			robot.set_all_backpack_lights(cozmo.lights.blue_light)
 			os.system('cls' if os.name == 'nt' else 'clear')
 			print("State: finding charger, battery %s" % str(round(robot.battery_voltage, 2)))
 			# charger location search
 			charger = None
 			# see if we already know where the charger is
 			if robot.world.charger:
 				if robot.world.charger.pose.origin_id == robot.pose.origin_id:
 					#we know where the charger is
 					os.system('cls' if os.name == 'nt' else 'clear')
 					print("State: charger position known, battery %s" % str(round(robot.battery_voltage, 2)))
 					robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
 					robot.move_lift(-3)
 					robot.set_head_angle(degrees(0)).wait_for_completed()
 					charger = robot.world.charger
 				else:
 					# we know where the charger is but we have been delocalized
 					os.system('cls' if os.name == 'nt' else 'clear')
 					print("State: robot delocalized recently, battery %s" % str(round(robot.battery_voltage, 2)))
 					pass
 			# we don't know where the charger is
 			if not charger:
 				os.system('cls' if os.name == 'nt' else 'clear')
 				print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
 				robot.play_anim_trigger(cozmo.anim.Triggers.SparkIdle, ignore_body_track=True).wait_for_completed()
 				robot.move_lift(-3)
 				robot.set_head_angle(degrees(0)).wait_for_completed()
 				robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
 				# randomly drive around for a bit and see if we can spot the lightcubes or charger
 				#TODO: better search AI and beter code
 				loops=2
 				while loops>0:
 					if robot.world.charger:
 						loops=0
 						charger = robot.world.charger
 						print("breaking charger loop as charger is known")
 						break
 					for _ in range(11):
 						robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
 						if robot.world.charger:
 							loops=0
 							charger = robot.world.charger
 							print("breaking charger loop as charger is known")
 							break
 						time.sleep(0.3)
 					robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
 					if robot.world.charger:
 						loops=0
 						charger = robot.world.charger
 						print("breaking charger loop as charger is known")
 						break
 					cube1 = robot.world.get_light_cube(cozmo.objects.LightCube1Id)
 					if cube1:
 						if robot.world.charger:
 							loops=0
 							charger = robot.world.charger
 							print("breaking charger loop as charger is known")
 							break
 						print("going to cube1!")
 						robot.go_to_object(cube1, distance_mm(80)).wait_for_completed()
 						robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
 					print("going to random point +/-100 in X+Y")
 					x= random.randrange(-100, 101, 100)
 					y= random.randrange(-100, 101, 100)
 					robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
 					for _ in range(11):
 						robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
 						if robot.world.charger:
 							loops=0
 							charger = robot.world.charger
 							print("breaking charger loop as charger is known")
 							break
 						time.sleep(0.3)
 					robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
 					if robot.world.charger:
 						loops=0
 						charger = robot.world.charger
 						print("breaking charger loop as charger is known")
 						break
 					cube2 = robot.world.get_light_cube(cozmo.objects.LightCube2Id)
 					if cube2:
 						if robot.world.charger:
 							loops=0
 							charger = robot.world.charger
 							print("breaking charger loop as charger is known")
 							break
 						print("going to cube2!")
 						robot.go_to_object(cube2, distance_mm(80)).wait_for_completed()
 						robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
 					print("going to random point +/-100 in X+Y")
 					x= random.randrange(-100, 101, 100)
 					y= random.randrange(-100, 101, 100)
 					robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
 					for _ in range(11):
 						robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
 						if robot.world.charger:
 							loops=0
 							charger = robot.world.charger
 							print("breaking charger loop as charger is known")
 							break
 						time.sleep(0.3)
 					robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
 					if robot.world.charger:
 						loops=0
 						charger = robot.world.charger
 						print("breaking charger loop as charger is known")
 						break
 					cube3 = robot.world.get_light_cube(cozmo.objects.LightCube3Id)
 					if cube3:
 						if robot.world.charger:
 							loops=0
 							charger = robot.world.charger
 							print("breaking charger loop as charger is known")
 							break
 						print("going to cube3!")
 						robot.go_to_object(cube3, distance_mm(80)).wait_for_completed()
 						robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
 					print("going to random point +/-100 in X+Y")
 					x= random.randrange(-100, 101, 100)
 					y= random.randrange(-100, 101, 100)
 					robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
 					for _ in range(11):
 						robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
 						if robot.world.charger:
 							loops=0
 							charger = robot.world.charger
 							print("breaking charger loop as charger is known")
 							break
 						time.sleep(0.3)
 					robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
 					if robot.world.charger:
 						loops=0
 						charger = robot.world.charger
 						print("breaking charger loop as charger is known")
 						break
 					robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
 					loops=loops-1
 				print("locator loop complete.")
 				time.sleep(1)
 			# Charger location and docking handling here
 			#TODO: improve this spaghetti code
 			if charger:
 				while (robot.is_on_charger == 0):
 					robot.set_lift_height(0.8,0.8,0.8,0.1).wait_for_completed()
 					# drive near to the charger, and then stop.
 					os.system('cls' if os.name == 'nt' else 'clear')
 					print("State: moving to charger, battery %s" % str(round(robot.battery_voltage, 2)))
 					robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabChatty, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
 					robot.move_lift(-3)
 					robot.set_head_angle(degrees(0)).wait_for_completed()
 					action = robot.go_to_pose(charger.pose)
 					action.wait_for_completed()
 					robot.drive_straight(distance_mm(-85), speed_mmps(50)).wait_for_completed()
 					action = robot.go_to_pose(charger.pose)
 					action.wait_for_completed()
 					robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
 					# we should be right in front of the charger, can we see it?
 					if (charger.is_visible==False):
 						#No we can't see it. Remove charger from navigation map and quit this loop.
 						robot.world.charger = None
 						robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
 						print("State: charger not found, clearing map. battery %s" % str(round(robot.battery_voltage, 2)))
 						break
 					i = random.randint(1, 100)
 					if i >= 85:
 						robot.play_anim_trigger(cozmo.anim.Triggers.FeedingReactToShake_Normal, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
 					os.system('cls' if os.name == 'nt' else 'clear')
 					# Dock. Turn around and drive backwards
 					print("State: docking, battery %s" % str(round(robot.battery_voltage, 2)))
 					robot.turn_in_place(degrees(95)).wait_for_completed()
 					robot.turn_in_place(degrees(95)).wait_for_completed()
 					time.sleep( 1 )
 					robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake, ignore_body_track=True).wait_for_completed()
 					robot.set_head_angle(degrees(0)).wait_for_completed()
 					robot.drive_straight(distance_mm(-145), speed_mmps(150)).wait_for_completed()
 					time.sleep( 1 )
 					# check if we're now docked
 					if robot.is_on_charger:
 						# Yes! we're docked!
 						robot.play_anim("anim_sparking_success_02").wait_for_completed()
 						robot.set_head_angle(degrees(0)).wait_for_completed()
 						os.system('cls' if os.name == 'nt' else 'clear')
 						print("State: docked, battery %s" % str(round(robot.battery_voltage, 2)))
 						robot.set_all_backpack_lights(cozmo.lights.off_light)
 						robot.play_anim("anim_gotosleep_getin_01").wait_for_completed()
 						robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed()
 					# No, we missed. Back off and try again
 					else:
 						os.system('cls' if os.name == 'nt' else 'clear')
 						print("State: failed to dock with charger, battery %s" % str(round(robot.battery_voltage, 2)))
 						robot.world.charger = None
 						robot.play_anim_trigger(cozmo.anim.Triggers.AskToBeRightedRight, ignore_body_track=True).wait_for_completed()
 						robot.move_lift(-3)
 						robot.set_head_angle(degrees(0)).wait_for_completed()
 						os.system('cls' if os.name == 'nt' else 'clear')
 						print("State: backing off to attempt docking, battery %s" % str(round(robot.battery_voltage, 2)))
 						robot.drive_straight(distance_mm(50), speed_mmps(90)).wait_for_completed()
 						robot.turn_in_place(degrees(-3)).wait_for_completed()
 						robot.drive_straight(distance_mm(150), speed_mmps(90)).wait_for_completed()
 						robot.turn_in_place(degrees(95)).wait_for_completed()
 						robot.turn_in_place(degrees(96)).wait_for_completed()
 						robot.set_head_angle(degrees(0)).wait_for_completed()
 						time.sleep( 1 )
 			else:
 			# we have not managed to find the charger. Falling back to freeplay with occasional checks
 				#robot.world.charger = None
 				robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
 				#robot.world.connect_to_cubes()
 				robot.enable_all_reaction_triggers(True)
 				robot.start_freeplay_behaviors()
 				freeplay=1
 				x=0
 				while x<20:
 					if not robot.world.charger:
 						time.sleep( 5 )
 						os.system('cls' if os.name == 'nt' else 'clear')
 						print("State: charger not found, falling back to freeplay for a bit, loop %d of 20." % x)
 					x+=1
 				#after ~100 seconds or spotting the charger end freeplay
 				robot.enable_all_reaction_triggers(False)
 				robot.stop_freeplay_behaviors()
 				freeplay=0
 			print("charger loop completed.")
 			time.sleep(1)
 		print("state cycle complete, looping.")
 		time.sleep(1)
 	os.system('cls' if os.name == 'nt' else 'clear')
 	print("State: program loop complete (we should not ever reach this state), battery %s" % str(round(robot.battery_voltage, 2)))
 	time.sleep( 3 )

 cozmo.robot.Robot.drive_off_charger_on_connect = False
 #cozmo.run_program(cozmo_unleashed, use_viewer=True)
 # if you have freeglut in the same folder as this script you can change the above line to
 cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
 # which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus
	#!/usr/bin/env python3

	# based on Copyright (c) 2016 Anki, Inc.
	#
	# Licensed under the Apache License, Version 2.0 (the "License");
	# you may not use this file except in compliance with the License.
	# You may obtain a copy of the License in the file LICENSE.txt or at
	#
	# http://www.apache.org/licenses/LICENSE-2.0
	#
	# Unless required by applicable law or agreed to in writing, software
	# distributed under the License is distributed on an "AS IS" BASIS,
	# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	# See the License for the specific language governing permissions and
	# limitations under the License.
	#
	#
	# cozmo_unleashed
	#
	# Script based on the Cozmo SDK drive_to_charger example script
	# and raproenca's modifications of same.
	# rewritten to support battery/charge states and all kinds of bells and whistles by acid zebra
	#
	import sys
	import os
	import datetime
	import random
	import time
	import asyncio
	import cozmo
	from cozmo.util import degrees, distance_mm, speed_mmps, Pose
	from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes
	global freeplay
	global chargerequired
	freeplay=0
	chargerequired=0
	# main program and loops
	def cozmo_unleashed(robot: cozmo.robot.Robot):
	global freeplay
	global chargerequired
	robot.enable_facial_expression_estimation(enable=True)
	robot.world.disconnect_from_cubes()
	robot.enable_all_reaction_triggers(False)
	robot.enable_stop_on_cliff(True)
	robot.set_robot_volume(0.04)
	os.system('cls' if os.name == 'nt' else 'clear')
	while True:
	#State 1: on charger, charging - wait for full charge
	if (robot.is_on_charger == 1) and (robot.is_charging == 1):
	#reset charge required (I know we're still charging, trust me)
	chargerequired=0
	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 == 100:
	robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
	if (robot.is_on_charger == 0):
	print("we were taken off charger")
	break
	robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
	if (robot.is_on_charger == 0):
	print("we were taken off charger")
	break
	elif i >= 85:
	robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
	else:
	if (robot.is_on_charger == 0):
	print("we were taken off charger")
	break
	time.sleep(5)
	time.sleep(5)
	#TODO: figure out how to smoothly cycle lights
	robot.set_all_backpack_lights(cozmo.lights.green_light)
	time.sleep(5)
	robot.set_all_backpack_lights(cozmo.lights.off_light)
	#State 2: on charger, fully charged - check schedule for go/no go
	if (robot.is_on_charger == 1) and (robot.is_charging == 0):
	#reset charge required
	chargerequired=0
	# day and time check - are we okay to play at this time and day?
	day_of_week = datetime.date.today().weekday() # 0 is Monday, 6 is Sunday
	ctime = datetime.datetime.now().time()
	playokay=0
	#it's weekend! Check for allowed times.
	if day_of_week > 4:
	if (ctime > datetime.time(9) and ctime < datetime.time(23)):
	playokay=1
	#it's a weekday! Check for allowed times.
	else:
	if (ctime > datetime.time(7,30) and ctime < datetime.time(22)):
	playokay=1
	# if the schedule says roll dice to see if we wake up (20 in 100 chance)
	if playokay==1:
	i = random.randint(1, 100)
	# 20 in 100 chance of waking up every 5 minutes
	if i >= 80:
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: leaving charger, battery %s" % str(round(robot.battery_voltage, 2)))
	robot.set_all_backpack_lights(cozmo.lights.off_light)
	robot.play_anim("anim_gotosleep_getout_02").wait_for_completed()
	robot.drive_off_charger_contacts().wait_for_completed()
	robot.drive_off_charger_contacts().wait_for_completed()
	robot.drive_off_charger_contacts().wait_for_completed()
	time.sleep(2)
	robot.move_lift(-3)
	robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
	robot.drive_straight(distance_mm(100), speed_mmps(90)).wait_for_completed()
	# we're out of schedule or didn't make the dice roll, back off for ~5 minutes and check again.
	x = 1
	while x < 20 and (robot.is_on_charger == 1):
	os.system('cls' if os.name == 'nt' else 'clear')
	if playokay == 1:
	print("State: charged, schedule OK but not active, sleep loop %d of 30 before next check." % (x))
	else:
	print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
	i = random.randint(1, 100)
	if i == 100:
	robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed()
	robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
	elif i >= 90:
	robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
	else:
	time.sleep(5)
	#robot.play_anim("anim_gotosleep_off_01").wait_for_completed()
	if (robot.is_on_charger == 0):
	print("we were taken off charger")
	break
	time.sleep(5)
	if (robot.is_on_charger == 0):
	print("we were taken off charger")
	break
	robot.set_all_backpack_lights(cozmo.lights.blue_light)
	time.sleep(5)
	if (robot.is_on_charger == 0):
	print("we were taken off charger")
	break
	robot.set_all_backpack_lights(cozmo.lights.off_light)
	x += 1
	#State 3: not on charger, good battery - freeplay
	if (robot.battery_voltage > 3.7) and (robot.is_on_charger == 0):
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: freeplay, battery %s" % str(round(robot.battery_voltage, 2)))
	#initiate freeplay
	if freeplay==0 and chargerequired==0:
	print("freeplay active")
	robot.world.connect_to_cubes()
	robot.enable_all_reaction_triggers(True)
	robot.start_freeplay_behaviors()
	freeplay=1
	if chargerequired==0:
	time.sleep(5)
	#State 4: not on charger, low battery, or chargerequired flag is set - seek charger
	if (robot.battery_voltage <= 3.7) and (robot.is_on_charger == 0) or (chargerequired==1) and (robot.is_on_charger == 0):
	#back off from whatever we were doing
	chargerequired=1
	if freeplay==1:
	robot.abort_all_actions(log_abort_messages=False)
	robot.wait_for_all_actions_completed()
	robot.stop_freeplay_behaviors()
	robot.enable_all_reaction_triggers(False)
	robot.world.disconnect_from_cubes()
	freeplay=0
	robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed()
	robot.set_head_angle(degrees(0)).wait_for_completed()
	robot.move_lift(-3)
	robot.drive_straight(distance_mm(-30), speed_mmps(90)).wait_for_completed()
	robot.set_all_backpack_lights(cozmo.lights.blue_light)
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: finding charger, battery %s" % str(round(robot.battery_voltage, 2)))
	# charger location search
	charger = None
	# see if we already know where the charger is
	if robot.world.charger:
	if robot.world.charger.pose.origin_id == robot.pose.origin_id:
	#we know where the charger is
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: charger position known, battery %s" % str(round(robot.battery_voltage, 2)))
	robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
	robot.move_lift(-3)
	robot.set_head_angle(degrees(0)).wait_for_completed()
	charger = robot.world.charger
	else:
	# we know where the charger is but we have been delocalized
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: robot delocalized recently, battery %s" % str(round(robot.battery_voltage, 2)))
	pass
	# we don't know where the charger is
	if not charger:
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2)))
	robot.play_anim_trigger(cozmo.anim.Triggers.SparkIdle, ignore_body_track=True).wait_for_completed()
	robot.move_lift(-3)
	robot.set_head_angle(degrees(0)).wait_for_completed()
	robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
	# randomly drive around for a bit and see if we can spot the lightcubes or charger
	#TODO: better search AI and beter code
	loops=2
	while loops>0:
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	for _ in range(11):
	robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	time.sleep(0.3)
	robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	cube1 = robot.world.get_light_cube(cozmo.objects.LightCube1Id)
	if cube1:
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	print("going to cube1!")
	robot.go_to_object(cube1, distance_mm(80)).wait_for_completed()
	robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
	print("going to random point +/-100 in X+Y")
	x= random.randrange(-100, 101, 100)
	y= random.randrange(-100, 101, 100)
	robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
	for _ in range(11):
	robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	time.sleep(0.3)
	robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	cube2 = robot.world.get_light_cube(cozmo.objects.LightCube2Id)
	if cube2:
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	print("going to cube2!")
	robot.go_to_object(cube2, distance_mm(80)).wait_for_completed()
	robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
	print("going to random point +/-100 in X+Y")
	x= random.randrange(-100, 101, 100)
	y= random.randrange(-100, 101, 100)
	robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
	for _ in range(11):
	robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	time.sleep(0.3)
	robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	cube3 = robot.world.get_light_cube(cozmo.objects.LightCube3Id)
	if cube3:
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	print("going to cube3!")
	robot.go_to_object(cube3, distance_mm(80)).wait_for_completed()
	robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed()
	print("going to random point +/-100 in X+Y")
	x= random.randrange(-100, 101, 100)
	y= random.randrange(-100, 101, 100)
	robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
	for _ in range(11):
	robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1)
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	time.sleep(0.3)
	robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
	if robot.world.charger:
	loops=0
	charger = robot.world.charger
	print("breaking charger loop as charger is known")
	break
	robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
	loops=loops-1
	print("locator loop complete.")
	time.sleep(1)
	# Charger location and docking handling here
	#TODO: improve this spaghetti code
	if charger:
	while (robot.is_on_charger == 0):
	robot.set_lift_height(0.8,0.8,0.8,0.1).wait_for_completed()
	# drive near to the charger, and then stop.
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: moving to charger, battery %s" % str(round(robot.battery_voltage, 2)))
	robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabChatty, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
	robot.move_lift(-3)
	robot.set_head_angle(degrees(0)).wait_for_completed()
	action = robot.go_to_pose(charger.pose)
	action.wait_for_completed()
	robot.drive_straight(distance_mm(-85), speed_mmps(50)).wait_for_completed()
	action = robot.go_to_pose(charger.pose)
	action.wait_for_completed()
	robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
	# we should be right in front of the charger, can we see it?
	if (charger.is_visible==False):
	#No we can't see it. Remove charger from navigation map and quit this loop.
	robot.world.charger = None
	robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
	print("State: charger not found, clearing map. battery %s" % str(round(robot.battery_voltage, 2)))
	break
	i = random.randint(1, 100)
	if i >= 85:
	robot.play_anim_trigger(cozmo.anim.Triggers.FeedingReactToShake_Normal, ignore_body_track=True, ignore_head_track=True).wait_for_completed()
	os.system('cls' if os.name == 'nt' else 'clear')
	# Dock. Turn around and drive backwards
	print("State: docking, battery %s" % str(round(robot.battery_voltage, 2)))
	robot.turn_in_place(degrees(95)).wait_for_completed()
	robot.turn_in_place(degrees(95)).wait_for_completed()
	time.sleep( 1 )
	robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake, ignore_body_track=True).wait_for_completed()
	robot.set_head_angle(degrees(0)).wait_for_completed()
	robot.drive_straight(distance_mm(-145), speed_mmps(150)).wait_for_completed()
	time.sleep( 1 )
	# check if we're now docked
	if robot.is_on_charger:
	# Yes! we're docked!
	robot.play_anim("anim_sparking_success_02").wait_for_completed()
	robot.set_head_angle(degrees(0)).wait_for_completed()
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: docked, battery %s" % str(round(robot.battery_voltage, 2)))
	robot.set_all_backpack_lights(cozmo.lights.off_light)
	robot.play_anim("anim_gotosleep_getin_01").wait_for_completed()
	robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed()
	# No, we missed. Back off and try again
	else:
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: failed to dock with charger, battery %s" % str(round(robot.battery_voltage, 2)))
	robot.world.charger = None
	robot.play_anim_trigger(cozmo.anim.Triggers.AskToBeRightedRight, ignore_body_track=True).wait_for_completed()
	robot.move_lift(-3)
	robot.set_head_angle(degrees(0)).wait_for_completed()
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: backing off to attempt docking, battery %s" % str(round(robot.battery_voltage, 2)))
	robot.drive_straight(distance_mm(50), speed_mmps(90)).wait_for_completed()
	robot.turn_in_place(degrees(-3)).wait_for_completed()
	robot.drive_straight(distance_mm(150), speed_mmps(90)).wait_for_completed()
	robot.turn_in_place(degrees(95)).wait_for_completed()
	robot.turn_in_place(degrees(96)).wait_for_completed()
	robot.set_head_angle(degrees(0)).wait_for_completed()
	time.sleep( 1 )
	else:
	# we have not managed to find the charger. Falling back to freeplay with occasional checks
	#robot.world.charger = None
	robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
	#robot.world.connect_to_cubes()
	robot.enable_all_reaction_triggers(True)
	robot.start_freeplay_behaviors()
	freeplay=1
	x=0
	while x<20:
	if not robot.world.charger:
	time.sleep( 5 )
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: charger not found, falling back to freeplay for a bit, loop %d of 20." % x)
	x+=1
	#after ~100 seconds or spotting the charger end freeplay
	robot.enable_all_reaction_triggers(False)
	robot.stop_freeplay_behaviors()
	freeplay=0
	print("charger loop completed.")
	time.sleep(1)
	print("state cycle complete, looping.")
	time.sleep(1)
	os.system('cls' if os.name == 'nt' else 'clear')
	print("State: program loop complete (we should not ever reach this state), battery %s" % str(round(robot.battery_voltage, 2)))
	time.sleep( 3 )

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