cozmo_go_to_charger.py

#!/usr/bin/env python3

# 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.

'''Make Cozmo drive to his charger.
'''
import time
import sys
import asyncio
import time
import cozmo
from cozmo.util import degrees, distance_mm, speed_mmps


def drive_to_charger(robot, trial):
	'''The core of the drive_to_charger program'''
	
	# If the robot was on the charger, drive them forward and clear of the charger
	if robot.is_on_charger:
		# drive off the charger
		robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed()
		robot.play_anim("anim_launch_altwakeup_01").wait_for_completed()
		print("getting off the charger...")
		robot.drive_off_charger_contacts().wait_for_completed()
		robot.move_lift(-3)
		robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
		# Start moving the lift down
		robot.move_lift(-3)
		robot.start_freeplay_behaviors()
		time.sleep( 60 )
		robot.stop_freeplay_behaviors()
		robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed()
		# turn around to look at the charger
		robot.turn_in_place(degrees(95)).wait_for_completed()
		robot.turn_in_place(degrees(95)).wait_for_completed()
		#robot.turn_in_place(degrees(180)).wait_for_completed()
		# Tilt the head to be level
		robot.set_head_angle(degrees(0)).wait_for_completed()
		# wait half a second to ensure Cozmo has seen the charger
		#time.sleep(0.5)
		robot.play_anim("anim_cozmosays_getin_medium_01").wait_for_completed()
		robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabChatty, ignore_body_track=True).wait_for_completed()
		# drive backwards away from the charger
		robot.drive_straight(distance_mm(-60), speed_mmps(50)).wait_for_completed()

	# try to find the charger
	charger = None
	robot.world.charger = None
	
	# show the battery voltage
	print("battery voltage: %s" % robot.battery_voltage)

	# see if Cozmo already knows where the charger is
	if robot.world.charger:
		if robot.world.charger.pose.origin_id == robot.pose.origin_id:
			print("I know where the charger is!")
			robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True).wait_for_completed()
			robot.set_head_angle(degrees(0)).wait_for_completed()
			charger = robot.world.charger
		else:
			# Cozmo knows about the charger, but the pose is not based on the
			# same origin as the robot (e.g. the robot was moved since seeing
			# the charger) so try to look for the charger first
			pass

	if not charger:
		# Tell Cozmo to look around for the charger
		print("looking for the charger now...")
		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.turn_in_place(degrees(95)).wait_for_completed()
		robot.turn_in_place(degrees(95)).wait_for_completed()
		look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
		try:
			charger = robot.world.wait_for_observed_charger(timeout=60)
			print("I found the charger: %s" % charger)
		except asyncio.TimeoutError:
			print("I didn't see the charger")
			robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabUnhappy, ignore_body_track=True).wait_for_completed()
			robot.set_head_angle(degrees(0)).wait_for_completed()
		finally:
			# whether we find it or not, we want to stop the behavior
			look_around.stop()


	if charger:
		# lift his arms to manouver
		robot.set_lift_height(0.8,0.8,0.8,0.1).wait_for_completed()
		# Attempt to drive near to the charger, and then stop.
		print("Trial number %s" % trial)
		print("Going for the charger!")
		robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabChatty, ignore_body_track=True).wait_for_completed()
		robot.set_head_angle(degrees(0)).wait_for_completed()
		#robot.go_to_pose(charger.pose, relative_to_robot=True).wait_for_completed()
		#action = robot.go_to_object(charger, distance_mm(60.0))
		action = robot.go_to_pose(charger.pose)
		action.wait_for_completed()
		print("Completed action: result = %s" % action)
		robot.drive_straight(distance_mm(-30), speed_mmps(50)).wait_for_completed()
		print("Done.")
        
		# Turn 180 (and 10) degrees, then goes backwards at full speed
		print("Now the grand finale: turn around and park!")
		print("Turning...")
		#robot.turn_in_place(degrees(190)).wait_for_completed()
		robot.turn_in_place(degrees(95)).wait_for_completed()
		robot.turn_in_place(degrees(95)).wait_for_completed()
		#robot.turn_in_place(degrees(10)).wait_for_completed()
		time.sleep( 1 )
		print("here I go!")
		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(-137), speed_mmps(150)).wait_for_completed()
		time.sleep( 1 )
		print("checking if I did it...")
		if robot.is_on_charger:
			#robot.play_anim_trigger(cozmo.anim.Triggers.FistBumpSuccess, ignore_body_track=True).wait_for_completed()
			robot.play_anim("anim_sparking_success_02").wait_for_completed()
			robot.set_head_angle(degrees(0)).wait_for_completed()
			print("I did it! Yay!")
			time.sleep( 5 )
			robot.play_anim("anim_gotosleep_getin_01").wait_for_completed()
			robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed()
			time.sleep( 60 )
			drive_to_charger(robot, trial=1)
		else:
			print("I did not manage to dock in the charger =(")
			robot.play_anim_trigger(cozmo.anim.Triggers.AskToBeRightedRight, ignore_body_track=True).wait_for_completed()
			print("Trying again...")
			robot.world.charger = None
			print("let me move away a little bit so I can see better...")
			robot.drive_straight(distance_mm(50), speed_mmps(90)).wait_for_completed()
			robot.drive_straight(distance_mm(50), speed_mmps(90)).wait_for_completed()
			robot.drive_straight(distance_mm(50), speed_mmps(90)).wait_for_completed()
			trial += 1
			if trial < 10:
				drive_to_charger(robot, trial)
			else:
				robot.play_anim_trigger(cozmo.anim.Triggers.NothingToDoBoredEvent, ignore_body_track=True).wait_for_completed()
				print("can't find the charger, playing some more...")
			robot.start_freeplay_behaviors()
			time.sleep( 60 )
			robot.stop_freeplay_behaviors()
			robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed()
			#robot.play_anim("anim_gotosleep_getin_01").wait_for_completed()
			#robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed()
			#time.sleep( 60 )
			robot.world.charger = None
			drive_to_charger(robot, trial=1)
			

			
def run(sdk_conn):
	'''The run method runs once the Cozmo SDK is connected.'''
	robot = sdk_conn.wait_for_robot()

	try:
		# register the number of trials
		drive_to_charger(robot, trial=1)

	except KeyboardInterrupt:
		print("")
		print("Exit requested by user")


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:
        sys.exit("A connection error occurred: %s" % e)

Nice, i love it, it's so well made and it helped me alot 👍