76 lines
2.1 KiB
Python
76 lines
2.1 KiB
Python
import cozmo
|
|
import time
|
|
from cozmo.util import degrees, distance_mm, speed_mmps
|
|
|
|
|
|
def cozmo_program(robot: cozmo.robot.Robot):
|
|
# robot.say_text("Hi Martina").wait_for_completed()
|
|
# Drive forwards for 150 millimeters at 50 millimeters-per-second.
|
|
# robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
|
|
|
|
# Turn 90 degrees to the left.
|
|
# Note: To turn to the right, just use a negative number.
|
|
# robot.turn_in_place(degrees(90)).wait_for_completed()
|
|
|
|
print("fov_x ", robot.camera.config.fov_x)
|
|
print("fov_y ", robot.camera.config.fov_y)
|
|
print("focal_length ", robot.camera.config.focal_length)
|
|
|
|
cozmo = CozmoDriver(robot)
|
|
cozmo.updateLift()
|
|
cozmo.drive()
|
|
|
|
turn_speed = 30
|
|
forward_speed = 50
|
|
|
|
l_wheel_speed = (0 * forward_speed) + (turn_speed * 1)
|
|
r_wheel_speed = (0 * forward_speed) - (turn_speed * 1)
|
|
robot.drive_wheels(l_wheel_speed,r_wheel_speed,l_wheel_speed *4,r_wheel_speed *4)
|
|
time.sleep(9.5)
|
|
|
|
|
|
|
|
class CozmoDriver:
|
|
""" Contains all functions that will be used to move cozmo
|
|
"""
|
|
|
|
def __init__(self, robot: cozmo.robot.Robot):
|
|
self.speed = 50
|
|
self.turnSpeed = 30
|
|
self.robot = robot
|
|
self.initializeDriver()
|
|
|
|
def initializeDriver(self):
|
|
""" Set Cozmo in the starting position
|
|
"""
|
|
self.updateLift()
|
|
self.updateHead()
|
|
|
|
def drive(self):
|
|
""" Start driving
|
|
"""
|
|
self.robot.drive_wheel_motors(self.speed, self.speed)
|
|
|
|
def driveLeft(self):
|
|
""" Drive to left
|
|
"""
|
|
self.robot.drive_wheel_motors(self.speed - 15, self.speed)
|
|
|
|
def driveRight(self):
|
|
""" Drive to right
|
|
"""
|
|
self.robot.drive_wheel_motors(self.speed, self.speed - 15)
|
|
|
|
def updateLift(self):
|
|
""" Set Lift to maximum height
|
|
"""
|
|
self.robot.set_lift_height(1).wait_for_completed()
|
|
|
|
|
|
def updateHead(self):
|
|
""" Set Head to minimum angle
|
|
"""
|
|
self.robot.set_head_angle(cozmo.robot.MIN_HEAD_ANGLE).wait_for_completed()
|
|
|
|
cozmo.run_program(cozmo_program)
|