def alarm_clock(robot: cozmo.robot.Robot):
'''The core of the alarm_clock program'''
alarm_time = extract_time_from_args()
if alarm_time:
print("Alarm set for %s" % alarm_time)
else:
print("No Alarm time provided. Usage example: 'alarm_clock.py 17:23' to set alarm for 5:23 PM. (Input uses the 24-hour clock.)")
print("Press CTRL-C to quit")
get_in_position(robot)
was_before_alarm_time = False
last_displayed_time = None
while True:
current_time = datetime.datetime.now().time()
do_alarm = False
if alarm_time:
is_before_alarm_time = current_time < alarm_time
do_alarm = was_before_alarm_time and not is_before_alarm_time
was_before_alarm_time = is_before_alarm_time
if do_alarm:
robot.abort_all_actions()
with robot.perform_off_charger():
short_time_string = str(current_time.hour) + ":" + str(current_time.minute)
robot.say_text("Wake up lazy human! it's " + short_time_string).wait_for_completed()
else:
if (last_displayed_time is None) or (current_time.second != last_displayed_time.second):
clock_image = make_clock_image(current_time)
oled_face_data = cozmo.oled_face.convert_image_to_screen_data(clock_image)
# display for 1 second
robot.display_oled_face_image(oled_face_data, 1000.0)
last_displayed_time = current_time
time.sleep(0.1)
def drone(robot: cozmo.robot.Robot):
# code for state drone
# look for nearby cubes
lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cubes = robot.world.wait_until_observe_num_objects(num=1, object_type=cozmo.objects.LightCube, timeout=60)
# stop looking around if it finds any of the cubes
lookaround.stop()
# pick up the cube in front of it, making 5 attempts
current_action = robot.pickup_object(cubes[0], num_retries=5)
current_action.wait_for_completed()
# move forward 100mm
robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
# drop the cube
current_action = robot.place_object_on_ground_here(cubes[0]).wait_for_completed()
# go back 100mm
robot.drive_straight(distance_mm(-100), speed_mmps(50)).wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
# return to idle state
idle(robot)
def alarm_clock(robot: cozmo.robot.Robot):
'''The core of the alarm_clock program'''
alarm_time = extract_time_from_args()
if alarm_time:
print("Alarm set for %s" % alarm_time)
else:
print(
"No Alarm time provided. Usage example: 'alarm_clock.py 17:23' to set alarm for 5:23 PM. (Input uses the 24-hour clock.)"
)
print("Press CTRL-C to quit")
get_in_position(robot)
was_before_alarm_time = False
last_displayed_time = None
while True:
# Check the current time, and see if it's time to play the alarm
current_time = datetime.datetime.now().time()
do_alarm = False
if alarm_time:
is_before_alarm_time = current_time < alarm_time
do_alarm = was_before_alarm_time and not is_before_alarm_time # did we just cross the alarm time
was_before_alarm_time = is_before_alarm_time
if do_alarm:
# Cancel the latest image display action so that the alarm actions can play
robot.abort_all_actions()
# Speak The Time (off the charger as it's an animation)
with robot.perform_off_charger():
short_time_string = str(current_time.hour) + ":" + str(
current_time.minute)
robot.say_text("Wake up lazy human! it's " +
short_time_string).wait_for_completed()
else:
# See if the displayed time needs updating
if (last_displayed_time is None) or (current_time.second !=
last_displayed_time.second):
# Create the updated image with this time
clock_image = make_clock_image(current_time)
oled_face_data = cozmo.oled_face.convert_image_to_screen_data(
clock_image)
# display for 1 second
robot.display_oled_face_image(oled_face_data, 1000.0)
last_displayed_time = current_time
# only sleep for a fraction of a second to ensure we update the seconds as soon as they change
time.sleep(0.1)
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)
async def test_color_pass(robot: cozmo.robot.Robot):
await robot.set_head_angle(degrees(80)).wait_for_completed()
color_finder_game = custom_color_finder.ColorFinder(robot, "purple")
await color_finder_game.run()
robot.abort_all_actions()
await robot.say_text("testing").wait_for_completed()
robot.abort_all_actions()
await robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabExcited).wait_for_completed()
async def gameOne_cozmo_program(robot: cozmo.robot.Robot):
# purpose of setting the head angle is so each game starts with the robot looking in the same direction. 80
# degrees is slightly up from the x plane
await robot.set_head_angle(degrees(80)).wait_for_completed()
# determine a random number between 0 and 5 to determine which color Cozmo will speak
index = random.randint(0, 5)
# this calculation determines the color that is passed to the color_finder object.
# order that the colors appear in the color_list array is the order on the addative color wheel
# if that list is changed, the following formula will no longer generate the correct complementary color
correct_color_to_find = (index + 3) % len(color_list)
# print to screen will let the facilitator know what color the robot is looking for
print("Color to find is " + color_list[correct_color_to_find])
# these lines used for testing. allows developers to monitor what colors are being selected
# print(correct_color_to_find)
# print(color_list[correct_color_to_find])
# prompt for the user that begins the game
# purpose of print line is to let the facilator know what color the robot is looking for.
await robot.say_text("What color is opposite of " + color_list[index], duration_scalar=0.5).wait_for_completed()
'''
color_finder_game object is dependent on the imported custom_color_finder.py file.
this file is part of the Anki SDK but is modified for this game.
changes made custom_color_finder.py will affect the execution of main.py
modifications made have been documented in custom_color_finder.py
ARGS - color_list[correct_color_to_find] is a string that is used to initialize the object with the
color that Cozmo will search for
'''
color_finder_game = custom_color_finder.ColorFinder(robot, color_list[correct_color_to_find])
# execute the color_finder object
await color_finder_game.run()
# purpose of this call is to clear out all actions pending on the robot
robot.abort_all_actions()
# speaks the string passed in the array reference.
await robot.say_text("Correct" + color_list[correct_color_to_find] +
"is opposite of " + color_list[index], duration_scalar=0.80).wait_for_completed()
robot.abort_all_actions()
# executes a dance. this is a standard function provided in the import cozmo
await robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabExcited).wait_for_completed()
def example4_abort_all_actions(robot: cozmo.robot.Robot):
cozmo.logger.info("----------------------------------------")
cozmo.logger.info("Example 4: Abort all parallel actions.")
cozmo.logger.info("Start multiple parallel actions:")
action1 = robot.set_lift_height(0.0, in_parallel=True, duration=6.0)
action2 = robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE, duration=6.0, in_parallel=True)
action3 = robot.drive_straight(distance_mm(75), speed_mmps(25), should_play_anim=False, in_parallel=True)
action4 = robot.display_oled_face_image(face_image, 30000.0) # Note: face image is in_parallel by default
# wait two seconds and abort everything
time.sleep(2)
robot.abort_all_actions()
# wait for results to come back for all actions
robot.wait_for_all_actions_completed()
# All actions should have aborted
cozmo.logger.info("action1 res = %s", action1)
cozmo.logger.info("action2 res = %s", action2)
cozmo.logger.info("action3 res = %s", action3)
cozmo.logger.info("action4 res = %s", action4)
def inspection(robot: cozmo.robot.Robot):
# 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
# drive in a square with side 20cm
robot.set_lift_height(0.0, in_parallel=False, duration=1.0).wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
for i in range(4):
# raise the arm for 3 seconds, at a speed of 1/3 = 0.333
robot.move_lift(0.33)
robot.drive_straight(distance_mm(200), speed_mmps(40), should_play_anim=False, in_parallel=True, num_retries=0)
time.sleep(3)
# lower the arm for 2 seconds, at a speed of 1/2 = 0.5
robot.move_lift(-0.50)
time.sleep(2)
# abort all ongoing actions
robot.abort_all_actions(log_abort_messages=True)
# perform the turn: 90 degree
robot.turn_in_place(degrees(90)).wait_for_completed()
# stop all actions, if there are any
robot.abort_all_actions(log_abort_messages=True)
# return to idle state
idle(robot)
async def react(robot: cozmo.robot.Robot):
"""
Cozmo will react to the facial expressions of the first face it sees.
Rules:
1. If Cozmo sees a sad face, she will try to cheer you up.
2. If Cozmo sees an angry face, she will run away.
3. If Cozmo sees a happy face, she will celebrate with you.
4. At any point, you can ask Cozmo why she did something. She will stop what
she's doing, explain, and then go back to her alert "watching" state.
"""
robot.enable_facial_expression_estimation(enable=True)
state = "finding face"
face_visible = False
explanation = "I'm just chilling."
any_face = None
def on_press(key):
#print('{0} release'.format(key))
if key == Key.shift_l or key == Key.shift_r:
#listen(robot)
state = "explaining"
while True:
########## INITIAL CHECKS ##########
# check for key input
# later, check for voice input (from computer microphone)
listener = Listener(on_press=on_press)
listener.join()
if not face_visible:
state = "finding face"
########## STATE MACHINE ##########
print(state)
if state is "init":
# explanation = "I'm just getting started."
robot.set_head_angle(
cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()
robot.move_lift(-3)
if state is "finding face":
# explanation = "I'm trying to find a face."
any_face = None
print("Looking for a face!")
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.FindFaces)
try:
any_face = await (robot.world.wait_for_observed_face(
timeout=30))
except asyncio.TimeoutError:
print("Didn't find anyone :-(")
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
if any_face is None:
print("no faces found :(")
face_visible = False
else:
print("FOUND A FACE")
face_visible = True
state = "watching face"
elif state is "watching face":
#
# explanation = "I'm just staying alert."
print("face is ", any_face)
robot.stop_all_motors()
expression = any_face.expression
print("expression is ", expression)
if expression is "sad":
state = "reacting to sad face"
elif expression is "angry":
state = "reacting to angry face"
elif expression is "happy":
state = "reacting to happy face"
else:
state = "finding face"
elif state is "reacting to sad face":
explanation = "You seemed sad, so I'm sad too."
reaction = robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabDejected)
await (reaction.wait_for_completed())
print("tried to cheer you up")
time.sleep(200)
state = "watching face"
elif state is "reacting to angry face":
explanation = "You seemed angry, so I'm angry too."
reaction = robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabFrustrated)
await (reaction.wait_for_completed())
print("ran away")
time.sleep(200)
state = "watching face"
elif state is "reacting to happy face":
explanation = "You seemed happy, so I'm celebrating!"
reaction = robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabPartyTime)
await (reaction.wait_for_completed())
print("did a happy dance")
time.sleep(200)
state = "watching face"
elif state is "explaining":
robot.abort_all_actions()
explain = cozmo.say_text(explanation)
await (explain.wait_for_completed())
state = "watching face"
def idle(robot: cozmo.robot.Robot):
# code for state idle
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
# reset head angle so it is facing forward
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
# reuse the model from lab 1
classifier = load('clf.joblib')
# initialize the detection window of size 5
img_window = ['none', 'none', 'none', 'none', 'none']
# intialize the array of labels
label_arr = ['drone', 'hands', 'inspection', 'none', 'order', 'place', 'plane', 'truck']
state_idle = True
# pause 0.5 sec
while (state_idle):
time.sleep(0.5)
# take the image
latest_image = robot.world.latest_image
new_image = latest_image.raw_image
new_image = np.array(new_image)
# rescale the image into 32 X 32 X 3 array
image = rescale(new_image,32,32)
image = img_to_array(image)
# normalize the image array
img = np.array(image, dtype="float") / 255.0
img = img.reshape(-1, 32, 32, 3)
# make the prediction, the result is an array with the maximum number corresponding
# the correct prediction
result = classifier.predict(img)
# convert the prediction result to array
result = result[0].tolist()
# extract the index for the maximum label
idx = result.index(max(result))
# extract the label
label = label_arr[idx]
for i in range(1, len(img_window)):
img_window[i] = img_window[i - 1]
img_window[0] = label
majority_label = statistics.mode(img_window)
print(majority_label)
if majority_label == 'drone':
# say the text "drone", and jump to drone
robot.say_text('drone').wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
drone(robot)
if majority_label == 'order':
# say the text "order" and jump to order
img_window = ['none', 'none', 'none', 'none', 'none']
robot.say_text('order').wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
order(robot)
if majority_label == 'inspection':
# say the text "inspection", and jump to inspection
img_window = ['none', 'none', 'none', 'none', 'none']
robot.say_text('inspection').wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
inspection(robot)
# exit the code execution if there are any keyboard interrupts
exit(0)
def cozmo_program(robot: cozmo.robot.Robot):
global angle
angle = 25.
robot.set_head_angle(degrees(angle)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = True
robot.camera.enable_auto_exposure = True
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3'
frame = os.path.join(directory, "current.jpeg")
print("Starting Tensorflow...")
with tf.Session() as sess:
print("Session successfully started")
model = load_model('modelv1.07-0.96.hdf5')
while True:
global X, Y, W, H
global result
X = 245.
Y = 165.
W = 150.
H = 150.
gt = [X, Y, W, H]
pos_x, pos_y, target_w, target_h = region_to_bbox(gt)
frame = os.path.join(directory, "current.jpeg")
result = 0
dog_counter = 0
cat_counter = 0
background_counter = 0
next_state = 0
current_state = 0 #Background: 0, Cat:1, Dog:2
while True:
latest_img = robot.world.latest_image
if latest_img is not None:
pilImage = latest_img.raw_image
pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG")
show_frame(np.asarray(Image.open(frame)), [900.,900.,900.,900.], 1)
img = load_image(frame)
[result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\
,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img})
next_state = np.argmax(result)
print('Arg max: ',next_state)
# Initial Current State is Background
if current_state == 0:
print('Background')
if next_state == 1: # Detected a Cat
current_state = 1 # Transition to Cat State
background_counter = 0
cat_counter = 1
dog_counter = 0
elif next_state == 2: # Detected a Dog
current_state = 2 # Transition to Dog state
background_counter = 0
cat_counter = 0
dog_counter = 1
# Current State is Cat
elif current_state == 1:
print('\t\t\t\t\t\tCat')
if next_state == 0: # Detected Background
background_counter += 1
if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times
background_counter = 0
current_state = 0
cat_counter = 0
elif next_state == 1: # Detected Cat itself
cat_counter +=1
if cat_counter >= 30:
print('Cozmo sees a cat')
dense = model.get_layer('dense').get_weights()
weights = dense[0].T
testing_counter = 0
detected_centroid = 0
xmin_avg = 0
xmax_avg = 0
ymin_avg = 0
ymax_avg = 0
frame_average = 2
frame_count = 0
while True:
latest_img = robot.world.latest_image
if latest_img is not None:
pilImage = latest_img.raw_image
pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG")
img = load_image(frame)
[result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\
,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img})
kernels = out_relu.reshape(7,7,1280)
final = np.dot(kernels,weights[result[0].argmax()])
final1 = array_to_img(final.reshape(7,7,1))
final1 = final1.resize((224,224), Image.ANTIALIAS)
box = img_to_array(final1).reshape(224,224)
#box = cv2.blur(box,(30,30))
temp = (box > box.max()*.8) *1
temp_adjusted = np.ndarray(shape=np.shape(temp), dtype=np.dtype(np.uint8))
temp_adjusted[:,:] = np.asarray(temp)*255
contours, hierarchy = cv2.findContours(temp_adjusted, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:]
contours = np.array(contours)
max_area = [0,0] # contours index and area
for index, contour in enumerate(contours):
if(max_area[1]< len(contour)):
max_area = [index,len(contour)]
contours_adjusted = contours[max_area[0]].squeeze(axis=1).T
xmin = contours_adjusted[0].min() * (640./224.)
ymin = contours_adjusted[1].min() * (480./224.)
xmax = contours_adjusted[0].max() * (640./224.)
ymax = contours_adjusted[1].max() * (480./224.)
if result[0].argmax() == 1:
# Frame smoothing
frame_count = frame_count + 1
xmin_avg = xmin_avg + xmin
xmax_avg = xmax_avg + xmax
ymin_avg = ymin_avg + ymin
ymax_avg = ymax_avg + ymax
if frame_count % frame_average == 0:
frame_count = 0
xmin_avg = xmin_avg/frame_average
xmax_avg = xmax_avg/frame_average
ymin_avg = ymin_avg/frame_average
ymax_avg = ymax_avg/frame_average
print(xmin_avg, end=",")
print(ymin_avg, end=",")
print(xmax_avg, end=",")
print(ymax_avg, end="\n")
ymin_avg = ymin_avg + (ymax_avg - ymin_avg)/2. - H/2.
xmin_avg = xmin_avg + (xmax_avg - xmin_avg)/2. - W/2.
print("150: ",xmin_avg, end=",")
print("150: ",ymin_avg, end="\n")
gt = [xmin_avg, ymin_avg, W, H]
xmin_avg = 0
xmax_avg = 0
ymin_avg = 0
ymax_avg = 0
pos_x, pos_y, target_w, target_h = region_to_bbox(gt)
bboxes = np.zeros((1, 4))
#bboxes[0,:] = pos_x-target_w/2, pos_y-target_h/2, target_w, target_h
bboxes[0,:] = pos_x-W/2, pos_y-H/2, W, H
print(len(contours))
testing_counter = testing_counter + 1
print("Testing_counter: ",testing_counter)
show_frame(np.asarray(Image.open(frame)), gt, 1)
print("Cat is detected")
print("Starting the tracker ...")
if (bboxes[0,1] + bboxes[0,3]/2) < (Y + H/2 - 40):
print("Command: Raise the head")
angle = angle + 0.5
if angle > 44.5:
angle = 44.5
elif (bboxes[0,1] + bboxes[0,3]/2) > (Y + H/2 + 40):
print("Command: Lower the head")
angle = angle - 0.5
if angle < 0:
angle = 0
else:
pass
set_head_angle_action = robot.set_head_angle(degrees(angle), max_speed=20, in_parallel=True)
if straight(bboxes[0,:])[0] != 0 and turn(bboxes[0,:])[0] != 0:
robot.drive_wheel_motors(straight(bboxes[0,:])[0] + turn(bboxes[0,:])[0], straight(bboxes[0,:])[1] + turn(bboxes[0,:])[1])
detected_centroid = 0
elif straight(bboxes[0,:])[0] == 0 and turn(bboxes[0,:])[0] == 0:
robot.stop_all_motors()
detected_centroid = detected_centroid + 1
elif straight(bboxes[0,:])[0] == 0:
robot.drive_wheel_motors(turn(bboxes[0,:])[0], turn(bboxes[0,:])[1])
detected_centroid = 0
elif turn(bboxes[0,:])[0] == 0:
robot.drive_wheel_motors(straight(bboxes[0,:])[0], straight(bboxes[0,:])[1])
detected_centroid = 0
else:
robot.stop_all_motors()
detected_centroid = detected_centroid + 1
if detected_centroid > 20//frame_average:
detected_centroid = 0
print("Reached a stable state.........\t\t\t\t\t\t\t\t STABLE")
# Go near the object
set_head_angle_action.wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
robot.wait_for_all_actions_completed()
robot.set_head_angle(degrees(0.5)).wait_for_completed()
print("Robot's head angle: ",robot.head_angle)
target_frame_count = 1
while True:
latest_img = None
while latest_img is None:
latest_img = robot.world.latest_image
target_frame1 = latest_img.raw_image
target_frame1 = target_frame1.resize((640,480), Image.ANTIALIAS)
#target_frame1 = target_frame1.convert('L')
target_frame1 = np.asarray(target_frame1)
#orb1 = cv2.ORB_create(500)
#kp1 = orb1.detect(target_frame1,None)
#kp1, des1 = orb1.compute(target_frame1, kp1)
#features_img1 = cv2.drawKeypoints(target_frame1, kp1, None, color=(255,0,0), flags=0)
#plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",features_img1)
plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",target_frame1)
drive_straight_action = robot.drive_straight(distance=cozmo.util.distance_mm(distance_mm=10),speed=cozmo.util.speed_mmps(10), in_parallel=True)
drive_straight_action.wait_for_completed()
robot.set_head_angle(degrees(0.5)).wait_for_completed()
print("Robot's head angle: ",robot.head_angle)
latest_img = None
while latest_img is None:
latest_img = robot.world.latest_image
target_frame2 = latest_img.raw_image
target_frame2 = target_frame2.resize((640,480), Image.ANTIALIAS)
#target_frame2 = target_frame2.convert('L')
target_frame2 = np.asarray(target_frame2)
#orb2 = cv2.ORB_create(500)
#kp2 = orb2.detect(target_frame2,None)
#kp2, des2 = orb2.compute(target_frame2, kp2)
#features_img2 = cv2.drawKeypoints(target_frame2, kp2, None, color=(255,0,0), flags=0)
#plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",features_img2)
plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",target_frame2)
target_frame_count = target_frame_count + 1
'''
matcher = cv2.DescriptorMatcher_create(cv2.DESCRIPTOR_MATCHER_BRUTEFORCE_HAMMING)
matches = matcher.match(des1, des2, None)
matches.sort(key=lambda x: x.distance, reverse=False)
matches = matches[:10]
imMatches = cv2.drawMatches(target_frame1, kp1, target_frame2, kp2, matches, None)
cv2.imwrite("matches_tf1_tf2.jpg", imMatches)
points1 = np.zeros((len(matches), 2), dtype=np.float32)
points2 = np.zeros((len(matches), 2), dtype=np.float32)
for i, match in enumerate(matches):
points1[i, :] = kp1[match.queryIdx].pt
points2[i, :] = kp2[match.trainIdx].pt
print("Points1 [{}]: {}".format(i,points1[i][0]), points1[i][1],"\tPoints2: ",points2[i][0], points2[i][1])
index = None
dist1_x = []
dist2_x = []
for index in range(len(points1)):
dist1_x.append((W/2.)-points1[index][0]) # Extract only the x-coordinate
dist2_x.append((W/2.)-points2[index][0]) # Extract only the x-coordinate
fw_x = 1./((1./np.array(dist2_x)) - (1./np.array(dist1_x))) # Calculate the image plane to obj plane mapping in x direction
pt1_x = []
pt2_x = []
for index in range(len(points1)):
pt1_x.append(fw_x[index]/(W/2. - points1[index][0]))
pt2_x.append(fw_x[index]/(W/2. - points2[index][0]))
print("Approx. distance[{}]: {}".format(index, pt1_x[index]))
if len(pt2_x) < 10:
break
'''
sys.exit(0)
else: # Detected Dog
dog_counter += 1
if dog_counter >= 6: # Transition to Dog only if Dog appeared for more than 6 times
cat_counter = 0
current_state = 2
# Current State is Dog
elif current_state == 2:
print('\t\t\t\t\t\t\t\t\t\t\t\tDog')
if next_state == 0: # Detected Background
background_counter += 1
if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times
background_counter = 0
current_state = 0
dog_counter = 0
elif next_state == 2: # Detected Dog itself
dog_counter +=1
if dog_counter >= 30:
print('Cozmo sees a Dog')
robot.drive_wheels(-50, -50)
time.sleep(3)
robot.drive_wheels(70, -70)
time.sleep(2.8)
robot.drive_wheels(0, 0)
break
else: # Detected Cat
cat_counter += 1
if cat_counter >= 6: # Transition to Cat only if Cat appeared for more than 6 times
dog_counter = 0
current_state = 1
async def gameTwo_cozmo_program(robot: cozmo.robot.Robot):
# purpose of setting the head angle is so each game starts with the robot looking in the same direction. 80
# degrees is slightly up from the x plane
await robot.set_head_angle(degrees(80)).wait_for_completed()
# colors_not_different purpose to ensure that Cozmo doesn't select te same primary color for both inputs
colors_not_different = True
# color_to_pass = ""
# select a random primary color
primary_color_1_selected_index = random.randint(0, 2)
primary_color_2_selected_index = 0
# select a second random primary color. the while loop ensures the same color is not selected twice
while colors_not_different:
primary_color_2_selected_index = random.randint(0, 2)
if primary_color_1_selected_index is not primary_color_2_selected_index:
colors_not_different = False
# Next two lines for tesing during development
# print(primary_color_list[primary_color_1_selected_index])
# print(primary_color_list[primary_color_2_selected_index])
# selects the color that cozmo will look for
# this formula determines what color is selected that cozmo is looking for as for the chart below
# if correct_color_to_find = 1 : Green
# if correct_color_to_find = 2 : Orange
# if correct_color_to_find = 3 : Purple
# calculation to determine which elment in the color_list Cozmo will select. the adjusment of -1 due to
# 0 indexing
correct_color_to_find_index = (primary_color_1_selected_index + primary_color_2_selected_index) - 1
# print to screen will let the facilitator know what color the robot is looking for
print("Color to find is " + secondary_color_list[correct_color_to_find_index])
# cozmo will speak the two colors that he is looking forward
await robot.say_text("What color is made by " + primary_color_list[primary_color_1_selected_index] +
" and" + primary_color_list[primary_color_2_selected_index], duration_scalar=0.5).wait_for_completed()
# print statement for testing
# print("index: " + str(correct_color_to_find_index))
# declare the object color)_finder_game
# ARGS - robot, and array color_list and index correct_color_to_find
'''
color_finder_game object is dependent on the imported custom_color_finder.py file.
this file is part of the Anki SDK but is modified for this game.
changes made custom_color_finder.py will affect the execution of main.py
modifications made have been documented in custom_color_finder.py
ARGS - color_list[correct_color_to_find] is a string that is used to initialize the object with the
color that Cozmo will search for
'''
color_finder_game = custom_color_finder.ColorFinder(robot, secondary_color_list[correct_color_to_find_index])
# run the color_finder_object
await color_finder_game.run()
# following code is for development and testing
# if correct_color_to_find_index == 1:
# color_to_pass = "******"
# elif correct_color_to_find_index == 2:
# color_to_pass = "******"
# else:
# color_to_pass = "******"
# purpose of this call is to clear out all actions pending on the robot
robot.abort_all_actions()
# await robot.say_text("testing").wait_for_completed()
# added the extra spaced in the first string has the affect of putting more of a delay before Cozmo
# speaks the string passed in the array reference.
await robot.say_text("Correct " + primary_color_list[primary_color_1_selected_index]
+ " and " + primary_color_list[primary_color_2_selected_index]
+ " will make " + secondary_color_list[correct_color_to_find_index], duration_scalar=0.55).wait_for_completed()
# purpose of this call is to clear out all actions pending on the robot
robot.abort_all_actions()
await robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabExcited).wait_for_completed()
