-
-
Save raproenca/e8aeba61c34cafc4b7ed3d3788834d5f to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python3 | |
| # 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 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 | |
| print("I am on the charger. Driving off the charger...") | |
| robot.drive_off_charger_contacts().wait_for_completed() | |
| robot.drive_straight(distance_mm(70), speed_mmps(50)).wait_for_completed() | |
| # Start moving the lift down | |
| robot.move_lift(-3) | |
| # turn around to look at the charger | |
| 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) | |
| # 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 | |
| # show the battery voltage | |
| print("battery voltage currently is: %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("Cozmo already knows where the charger is!") | |
| 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...") | |
| look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) | |
| try: | |
| charger = robot.world.wait_for_observed_charger(timeout=60) | |
| print("Found charger: %s" % charger) | |
| except asyncio.TimeoutError: | |
| print("Didn't see the charger") | |
| 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.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 finalle: 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("Get out of the way: here I go!") | |
| robot.drive_straight(distance_mm(-130), speed_mmps(150)).wait_for_completed() | |
| print("checking if I did it...") | |
| if robot.is_on_charger: | |
| print("I did it! Yay!") | |
| else: | |
| print("I did not manage to dock in the charger =(") | |
| print("Trying again...") | |
| robot.world.charger = None | |
| print("let me go a little bit further to be easier to see...") | |
| robot.drive_straight(distance_mm(90), speed_mmps(50)).wait_for_completed() | |
| trial += 1 | |
| if trial < 4: | |
| drive_to_charger(robot, trial) | |
| else: | |
| print("tired of trying. Giving up =(") | |
| 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) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment