def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose, confident
global grid, gui
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
pf = ParticleFilter(grid)
###################
############YOUR CODE HERE#################
###################
robot.set_head_angle(radians(0)).wait_for_completed()
estimated = [0, 0, 0, False]
trueVal = 0
while True:
# print(robot.is_picked_up)
if risingEdge(flag_odom_init, robot.is_picked_up):
pf = ParticleFilter(grid)
elif robot.is_picked_up:
robot.drive_wheels(0, 0)
elif not robot.is_picked_up:
if trueVal < 35:
robot.drive_wheels(5, 20)
else:
robot.stop_all_motors()
gh = math.degrees(
math.atan2(goal[1] - estimated[1], goal[0] - estimated[0]))
ch = estimated[2]
dh = gh - ch
print("gh:", gh % 360, "ch:", ch % 360, "dh:", dh % 360)
tol = 0.01
if dh > tol or dh < -tol:
print("hi")
robot.turn_in_place(degrees(dh)).wait_for_completed()
robot.drive_straight(
distance_inches(
grid_distance(goal[0], goal[1],
estimated[0], estimated[1]) - 1.75),
speed_mmps(25)).wait_for_completed()
dh = goal[2] - estimated[2] - dh
print("dh2", dh % 360)
robot.turn_in_place(degrees(dh)).wait_for_completed()
return 0
curr_pose = robot.pose
img = image_processing(robot)
markers = cvt_2Dmarker_measurements(img)
# print(markers)
odom = compute_odometry(curr_pose)
estimated = pf.update(odom, markers)
gui.show_particles(pf.particles)
gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3])
gui.updated.set()
last_pose = curr_pose
if estimated[3]:
trueVal += 1
def move_in_circle(robot: cozmo.robot.Robot, speed, seconds):
robot.say_text("I will spin in three circles").wait_for_completed()
# the first value is the speed for one of the treads, and the second value
# is the speed for the other tread (left? right?). They can both be
# the same sign, or have opposite signs. the last value is the duration of the
# movement (measured in seconds?)
robot.drive_wheels(speed, -1 * speed, None, None, seconds)
def cozmo_program(robot: cozmo.robot.Robot):
# dire_bonjour+setup
robot.play_anim('anim_freeplay_reacttoface_like_01').wait_for_completed()
cute = CuteCozmo(robot)
cute.face_cube(1)
# demo gamme
for i in range(8):
cute.play_note(i)
# Animation content, je t'invite a continuer
robot.play_anim('anim_fistbump_success_01').wait_for_completed()
cute.face_cube(1)
#Partie jouer mélodie simple
melodie = [1,4,7]
print('playing simple melody')
mini_jeu_jouer_melodie(cute, melodie)
mini_jeu_enregistrer_melodie(cute)
robot.play_anim('anim_hiking_observe_01').wait_for_completed()
mini_jeu_jouer_melodie(cute)
robot.play_anim('anim_freeplay_reacttoface_like_01').wait_for_completed()
robot.turn_in_place(Angle(degrees=180)).wait_for_completed()
robot.drive_wheels(500, 500)
time.sleep(5)
def run(self, robot: cozmo.robot.Robot):
robot.set_head_angle(degrees(-15)).wait_for_completed()
robot.move_lift(-5)
goLeft = False
showImage(robot, "RIGHT.PNG")
while True:
event = robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage,
timeout=30)
if event.image is not None:
image = np.asarray(event.image)
cube = None
try:
cube = find_cube(image, YELLOW_LOWER, YELLOW_UPPER)
print(cube)
# time.sleep(1)
except asyncio.TimeoutError:
print("Cube not found")
if cube:
return "colorTrack", robot
else:
robot.drive_wheels(25, -25)
time.sleep(1)
robot.stop_all_motors()
def cozmo_program(robot: cozmo.robot.Robot):
lw = 0
rw = 0
count = 0
set_init_pose(robot)
demo_camera_exposure(robot)
while True:
start = time.time()
key = getKey(robot)
cozmo.logger.info("'{}' key pressed".format(key))
if key in moveBindings.keys():
lw = moveBindings[key][0]
rw = moveBindings[key][1]
count = 0
else:
count = count + 1
if count > 4:
lw = 0
rw = 0
if key == 'q' or key == '\x03':
data = [('img', img_list), ('key', key_list)]
df = pd.DataFrame.from_items(data)
df.to_csv('driving_log.csv')
cozmo.logger.info('Writing to csv file, Exiting')
break
robot.drive_wheels(lw, rw)
ls = robot.left_wheel_speed
rs = robot.right_wheel_speed
cozmo.logger.info('cmd left, right speed: {}, {}'.format(lw, rw))
cozmo.logger.info('fb left,right speed: {}, {}'.format(ls, rs))
def followComposedTrajectory(robot: cozmo.robot.Robot, pathPositions,
pPosition, vl, vr, angle):
'''
Path Following Algorithm
'''
stateVector = [vr, vl, pPosition[0], pPosition[1], angle]
subPath = 0
nbSubPath = len(pathPositions) - 1
i = 0
#While the robot is not close enough of the last position of the trajectory
while np.linalg.norm(
alg.euclideanVector([robot.pose.position.x, robot.pose.position.y],
pathPositions[nbSubPath])) > scale / 10:
#The robot drive with the instructions computed with newWheelsSpeed
robot.drive_wheels(stateVector[1], stateVector[0])
time.sleep(deltaT) #During a certain time
plt.scatter(
[robot.pose.position.x],
[robot.pose.position.y]) #Plot the real position of the robot
stateVector = newWheelsSpeed(
stateVector[0], stateVector[1],
[robot.pose.position.x, robot.pose.position.y],
pathPositions[subPath], pathPositions[subPath + 1],
robot.pose.rotation.angle_z.radians, pathPositions)
subPath = predic.closestPath(
subPath, pathPositions,
[robot.pose.position.x, robot.pose.position.y])
i += 1
def drone(robot: cozmo.robot.Robot):
# Make robot drive in an S formation. Show animation of choice on robot face
robot.drive_wheels(50.0, 120.0, duration=3.5)
robot.drive_wheels(120.0, 50.0, duration=4.0)
robot.set_head_angle(degrees(30.0)).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabWin).wait_for_completed()
idle(robot)
def cozmo_program(robot: cozmo.robot.Robot):
time.sleep(2)
robot.play_anim_trigger(cozmo.anim.Triggers.FacePlantRollArmUp).wait_for_completed() #tries to get up, fails
robot.play_anim_trigger(cozmo.anim.Triggers.FacePlantRollArmUp).wait_for_completed()
robot.drive_wheels(-1000, 1000, l_wheel_acc=250, r_wheel_acc=250, duration=2) #spin
robot.set_lift_height(1.0, accel=100.0, max_speed=100.0).wait_for_completed() #slam lift
robot.set_lift_height(0.0, accel=100.0, max_speed=100.0).wait_for_completed()
robot.drive_wheels(-200, -200, l_wheel_acc=9999, r_wheel_acc=9999, duration=0.5) #sucessfully gets up
robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabCelebrate).wait_for_completed() #celebrate
def cozmo_program(robot: cozmo.robot.Robot):
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
while True:
latest_image = robot.world.latest_image
if latest_image is not None:
new_image = latest_image.raw_image
new_image = np.array(new_image)
image = img_clf.extract_image_features([new_image])
s = str(img_clf.predict_labels(image))
if "order" in s:
for i in range(1):
robot.say_text(s).wait_for_completed()
robot.drive_wheels(2,5)
time.sleep(18)
if "drone" in s:
robot.say_text(s).wait_for_completed()
cube = robot.world.wait_for_observed_light_cube(timeout=30)
robot.pickup_object(cube).wait_for_completed()
robot.drive_straight(distance_mm(100), speed_mmps(30)).wait_for_completed()
robot.place_object_on_ground_here(cube).wait_for_completed()
robot.drive_straight(distance_mm(-100), speed_mmps(30)).wait_for_completed()
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
if "inspection" in s:
robot.say_text(s).wait_for_completed()
#time.sleep(5)
for i in range(4):
robot.move_lift(.2)
robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
robot.move_lift(-.4)
robot.turn_in_place(degrees(90)).wait_for_completed()
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
if "plane" in s:
robot.say_text(s).wait_for_completed()
if "truck" in s:
robot.say_text(s).wait_for_completed()
if "hands" in s:
robot.say_text(s).wait_for_completed()
if "place" in s:
robot.say_text(s).wait_for_completed()
if "none" in s:
time.sleep(5)
def do_drive(robot: cozmo.robot.Robot):
drive_duration = extract_float(duration)
if drive_duration is not None:
drive_speed = 50
drive_dir = "forwards"
if drive_duration < 0:
drive_speed = -drive_speed
drive_duration = -drive_duration
drive_dir = "backwards"
robot.drive_wheels(drive_speed, drive_speed, duration=drive_duration)
def cozmo_program(robot: cozmo.robot.Robot):
# robot.say_text("I'm a gangstah robot!").wait_for_completed()
# robot.move_head(-5)
# Tell Cozmo to drive the left wheel at 25 mmps (millimeters per second),
# and the right wheel at 50 mmps (so Cozmo will drive Forwards while also
# turning to the left
robot.drive_wheels(20, 80)
# wait for 3 seconds (the head, lift and wheels will move while we wait)
time.sleep(10)
def cozmo_program(robot: cozmo.robot.Robot):
robot.move_head(-1)
robot.move_lift(-1)
robot.drive_wheels(50, 25)
time.sleep(3)
robot.move_head(1)
robot.move_lift(1)
robot.drive_wheels(50, -50)
time.sleep(3)
def run(robot: cozmo.robot.Robot):
print("***** Front wheel radius: " + str(get_front_wheel_radius(robot)))
print("***** Distance between wheels: " +
str(get_distance_between_wheels()))
## Example tests of the functions
robot.drive_wheels(50, 25, duration=6.4)
"""cozmo_drive_straight(robot, 85, 10)
def custom_objects(robot: cozmo.robot.Robot):
# Add event handlers for whenever Cozmo sees a new object
robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared)
robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared)
custom_box = robot.world.define_custom_box(custom_object_type=cozmo.objects.CustomObjectTypes.CustomType00, \
marker_front=cozmo.objects.CustomObjectMarkers.Circles2, \
marker_back=cozmo.objects.CustomObjectMarkers.Circles3, \
marker_top=cozmo.objects.CustomObjectMarkers.Circles4, \
marker_bottom=cozmo.objects.CustomObjectMarkers.Circles5, \
marker_left=cozmo.objects.CustomObjectMarkers.Diamonds2, \
marker_right=cozmo.objects.CustomObjectMarkers.Diamonds3, \
depth_mm=60, \
width_mm=60, \
height_mm=45, \
marker_width_mm=24.892, \
marker_height_mm=24.892, \
is_unique=True)
if (custom_box is not None):
print("All objects defined successfully!")
else:
print("One or more object definitions failed!")
return
print("Show the above markers to Cozmo and you will see the related objects "
"annotated in Cozmo's view window, you will also see print messages "
"everytime a custom object enters or exits Cozmo's view.")
print("Press CTRL-C to quit")
cubes = lookForCubes(robot, 45, 0)
old_pose = robot.pose
print("looking for cubes!")
if len(cubes) == 1:
print("found object")
robot.set_lift_height(height = 0, accel = 6, max_speed = 500, duration = 1, in_parallel = False, num_retries = 3).wait_for_completed()
robot.go_to_pose(cubes[0].pose,relative_to_robot=False).wait_for_completed()
pickupObject(robot)
robot.drive_wheels(-50,-50)
time.sleep(3)
robot.drive_wheels(0,0)
robot.go_to_pose(old_pose,relative_to_robot=False).wait_for_completed()
robot.set_lift_height(height = 0, accel = 6, max_speed = 500, duration = 1, in_parallel = False, num_retries = 3).wait_for_completed()
else:
print("Cannot locate custom box")
while True:
time.sleep(0.1)
def handle_control_input_for_acceleration(robot: cozmo.robot.Robot,
pad_events):
for pad_event in pad_events:
# Check for direction
# Go forward or go backward
if pad_event.ev_type == 'Absolute' and pad_event.code == 'ABS_RZ':
speed = pad_event.state * acceleration_factor
robot.drive_wheels(speed, speed)
elif pad_event.ev_type == 'Absolute' and pad_event.code == 'ABS_Z':
speed = pad_event.state * acceleration_factor * -1
robot.drive_wheels(speed, speed)
def drone(robot: cozmo.robot.Robot):
# Have the robot drive in an “S” formation. Show an animation of your choice on the
# robot’s face. Then return to the Idle state.
robot.say_text(str('drone')).wait_for_completed()
trigger = cozmo.anim.Triggers.CodeLabAmazed
robot.play_anim_trigger(trigger, in_parallel=True)
robot.drive_wheels(l_wheel_speed=10, r_wheel_speed=30, duration=10)
robot.play_anim_trigger(trigger, in_parallel=True)
robot.drive_wheels(l_wheel_speed=30, r_wheel_speed=10, duration=10)
robot.play_anim_trigger(trigger, in_parallel=True).wait_for_completed()
def order(robot: cozmo.robot.Robot):
# code for state order
# drive in a circle with radius of approximately 10 cm
robot.drive_wheels(80, 40)
# drive for about 13 seconds, approximately one cycle
time.sleep(13)
# stop driving after 13 sec
robot.drive_wheels(0, 0)
# end all actions
robot.abort_all_actions(log_abort_messages=True)
idle(robot)
def get_distance_between_wheels_test(robot: cozmo.robot.Robot): """Returns the distance between the wheels of the Cozmo robot in millimeters.""" # #### # TODO: Empirically determine the distance between the wheels of the robot using # robot.drive_wheels() function. Write a comment that explains how you determined # it and any computation you do as part of this function. # #### initialAngleZ = robot.pose.rotation.angle_z robot.drive_wheels(100,-100) time.sleep(3.33623) robot.drive_wheels(0,0)
def cozmo_program(robot: cozmo.robot.Robot):
robot.world.request_nav_memory_map(1)
N = 4000
w = 1e-2 * np.pi / 4
# Loop following sinusoidal control and printing position and visible cubes.
for t in range(N):
robot.drive_wheels(20 * np.sin(w * t), 20 * np.cos(w * t))
position = robot.pose.position.x_y_z
landmarks = [(i, c.is_visible)
for i, c in robot.world.light_cubes.items()]
print(position, landmarks)
time.sleep(1e-1)
def order(robot: cozmo.robot.Robot
): # Call this function once the robot sees the order object
#Drive in a circle. Need to test out to change the configurations to 10cm radius
lSpeed = 20.0
rSpeed = 50.0
l_acc = 10.0
r_acc = 10.0
robot.drive_wheels(lSpeed,
rSpeed,
l_wheel_acc=None,
r_wheel_acc=None,
duration=19.3).wait_for_completed(
) #take out wait for completed if necessary
def cozmo_program(robot: cozmo.robot.Robot):
# falls der Lift oben ist wird er runter geholt
robot.set_lift_height(0.0).wait_for_completed()
# H geht hoch
robot.move_lift(10)
# faehrt einen Kreis rechtsrum
robot.drive_wheels(200, 10)
time.sleep(0.5)
# H geht runter
robot.move_lift(-10)
# faehrt einen Kreis linkssrum
robot.drive_wheels(10, 201)
time.sleep(2.3)
def cozmo_program(robot: cozmo.robot.Robot):
robot.pop_a_wheelie(robot.world.get_light_cube(3)).wait_for_completed()
robot.say_text('Oh oh, nop').wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabSurprise).wait_for_completed()
robot.drive_wheels(-100,
100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.3)
robot.drive_wheels(100,
-100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.6)
robot.drive_wheels(-100,
100,
l_wheel_acc=999,
r_wheel_acc=999,
duration=0.3)
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabWhee1).wait_for_completed()
robot.set_lift_height(1.0, accel=100.0,
max_speed=100.0).wait_for_completed()
robot.set_lift_height(0.0, accel=100.0,
max_speed=100.0).wait_for_completed()
robot.drive_wheels(-200,
-200,
l_wheel_acc=9999,
r_wheel_acc=9999,
duration=0.5)
time.sleep(1)
robot.say_text('Ooo YES').wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
try:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
except socket_error as msg:
robot.say_text("socket failed" + msg).wait_for_completed()
ip = "10.0.1.10"
port = 5000
try:
s.connect((ip, port))
except socket_error as msg:
robot.say_text("socket failed to bind").wait_for_completed()
cont = True
robot.say_text("ready").wait_for_completed()
# keys cozmo responds to
movement = {'w', 's'}
turning = {'a', 'd'}
while cont:
bytedata = s.recv(4048)
# data = str(bytedata)
data = bytedata.decode('utf-8')
if not data:
cont = False
s.close()
quit()
else:
print(data)
instructions = data.split(';')
print(instructions)
# check the name to Stop movement, move forward or back, or turn R/L:
if instructions[0] == "stop":
robot.stop_all_motors()
if instructions[0] in movement:
# Movement is based on F forwards or B backwards and the speed at which both wheels move
if instructions[1] == 'F':
robot.drive_wheels(100, 100, 0, 0)
elif instructions[1] == 'B':
robot.drive_wheels(-100, -100, 0, 0)
# We want to turn left or right
if instructions[0] in turning:
if instructions[0] == 'a':
robot.turn_in_place(degrees(90)).wait_for_completed()
elif instructions[0] == 'd':
robot.turn_in_place(degrees(-90)).wait_for_completed()
print(instructions)
def order(robot: cozmo.robot.Robot
): # Call this function once the robot sees the order object
#Drive in a circle. Need to test out to change the configurations to 10cm radius
robot.say_text("order",
play_excited_animation=False,
use_cozmo_voice=True,
duration_scalar=1.0,
voice_pitch=5.0,
in_parallel=False,
num_retries=0).wait_for_completed()
lSpeed = 20.0
rSpeed = 50.0
l_acc = 10.0
r_acc = 10.0
robot.drive_wheels(
lSpeed, rSpeed, l_wheel_acc=None, r_wheel_acc=None,
duration=19.18) #take out wait for completed if necessary
def cozmo_program(robot: cozmo.robot.Robot):
time.sleep(2)
robot.move_lift(50)
robot.drive_straight(distance_mm(-50),
speed_mmps(150)).wait_for_completed() #drive forward
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabVampire).wait_for_completed() #grossed out
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabSquint1).wait_for_completed() #squint
robot.move_lift(-50) #put arms down
robot.drive_wheels(-1000,
1000,
l_wheel_acc=-250,
r_wheel_acc=250,
duration=1) #drive away
time.sleep(.5)
robot.drive_straight(distance_mm(200),
speed_mmps(150)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
robot.set_head_angle(degrees(0))
robot.set_lift_height(0, in_parallel=True).wait_for_completed()
#turn in circles until you see the charger
robot.drive_wheels(SPIN_SPEED, -SPIN_SPEED)
charger = robot.world.wait_for_observed_charger(timeout=30)
robot.drive_wheels(0.0, 0.0)
if charger:
if robot.pose.is_comparable(charger.pose):
#they're both on the same origin (cozmo)
print('going to charger pose')
robot.go_to_pose(charger.pose,
in_parallel=True).wait_for_completed()
robot.set_lift_height(.2, in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(180)).wait_for_completed()
wiggle_for_charger(robot)
else:
print('charger not found.')
def handle_control_input(robot: cozmo.robot.Robot):
'''
Méthode permettant de faire les actions en fonction des inputs de gamepad
'''
stop_all_movement = False
while True and not stop_all_movement:
events = get_gamepad()
for event in events:
#print(event.ev_type, event.code, event.state)
handle_control_input_for_lift(robot, event)
handle_control_input_for_head(robot, event)
if robot.is_cliff_detected:
robot.move_lift(5)
robot.drive_wheels(-800, -800, duration=0.5)
robot.move_lift(-5)
stop_all_movement = True
break
handle_control_input_for_acceleration(robot, events)
def cozmo_program(robot: cozmo.robot.Robot):
robot.world.image_annotator.add_static_text('text', 'Coz-Cam', position=cozmo.annotate.TOP_RIGHT)
robot.world.image_annotator.add_annotator('clock', clock)
robot.world.image_annotator.add_annotator('battery', Battery)
lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cubes = robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube, timeout=None)
cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip
cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart
cube3 = robot.world.get_light_cube(LightCube3Id)
lookaround.stop()
robot.pickup_object(cube3).wait_for_completed()
d = cozmo.util.Distance(distance_mm = 40)
robot.place_on_object(cube1).wait_for_completed()
robot.set_lift_height(height = 0.0, max_speed = 5).wait_for_completed()
robot.go_to_object(cube1,d).wait_for_completed()
robot.set_lift_height(height = 1.0, max_speed = 5).wait_for_completed()
d2 = cozmo.util.Distance(distance_inches=-2.0)
d3= cozmo.util.Distance(distance_inches=2.0)
d4 = cozmo.util.Distance(distance_inches = 3.0)
s = cozmo.util.Speed(speed_mmps = 10.0)
robot.drive_straight(d2,s, False).wait_for_completed()
robot.drive_wheels(10.0, -10.0, duration = 5.0)
robot.drive_straight(d4,s,False).wait_for_completed()
robot.drive_wheels(-10.0, 10.0, duration = 5.0)
robot.drive_straight(d3,s,False).wait_for_completed()
robot.set_lift_height (height = 0.0).wait_for_completed()
# Shutdown the program after 200 seconds
time.sleep(200)
def cozmo_program(robot: cozmo.robot.Robot):
# command cozmo to talk
# 'wait_for_completed()' syntax complete the statement before moving to the next line
robot.say_text("Hello World").wait_for_completed()
# A "for loop" runs for each value i in the given range - in this example
# starting from 0, while i is less than 5 (so 0,1,2,3,4).
for i in range(5):
# Add 1 to the number (so that we count from 1 to 5, not 0 to 4),
# then convert the number to a string and make Cozmo say it.
robot.say_text(str(i+1)).wait_for_completed()
# Drive forwards for X millimeters at X millimeters-per-second.
# X = any integer
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()
# Use a "for loop" to repeat the indented code 4 times
# Note: the _ variable name can be used when you don't need the value
for _ in range(4):
robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
# Tell the head motor to start lowering the head (at 5 radians per second)
robot.move_head(-5)
# Tell the lift motor to start lowering the lift (at 5 radians per second)
robot.move_lift(-5)
# Tell Cozmo to drive the left wheel at 25 mmps (millimeters per second),
# and the right wheel at 50 mmps (so Cozmo will drive Forwards while also
# turning to the left
robot.drive_wheels(25, 50)
# wait for 3 seconds (the head, lift and wheels will move while we wait)
time.sleep(3)
def cozmo_program(robot: cozmo.robot.Robot):
# Tell the head motor to start lowering the head (at 5 radians per second)
robot.move_head(-5)
# Tell the lift motor to start lowering the lift (at 5 radians per second)
robot.move_lift(-5)
# Tell Cozmo to drive the left wheel at 25 mmps (millimeters per second),
# and the right wheel at 50 mmps (so Cozmo will drive Forwards while also
# turning to the left
robot.drive_wheels(25, 50)
# wait for 3 seconds (the head, lift and wheels will move while we wait)
time.sleep(3)
# Tell the head motor to start raising the head (at 5 radians per second)
robot.move_head(5)
# Tell the lift motor to start raising the lift (at 5 radians per second)
robot.move_lift(5)
# Tell Cozmo to drive the left wheel at 50 mmps (millimeters per second),
# and the right wheel at -50 mmps (so Cozmo will turn in-place to the right)
robot.drive_wheels(50, -50)
# wait for 3 seconds (the head, lift and wheels will move while we wait)
time.sleep(3)
