-
Notifications
You must be signed in to change notification settings - Fork 3
/
cozmo_unleashed.py
1771 lines (1684 loc) · 70.9 KB
/
cozmo_unleashed.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#!/usr/bin/env python3
#
# NOTE I HAVE MOVED ON TO USING A CUSTOM MARKER PRINTED OUT AND PASTED ON TOP OF MY CHARGER
# SIZED AT 4CM/SIDE COZMO HAS A MUCH BETTER CHANCE OF FINDING THE CHARGER - SIMILAR TO THE NEW VECTOR ROBOT
# the last "general purpose" version is https://gist.github.com/acidzebra/c02ff8c8ccb0e3a057ae0b48a5082a68/ad4ff9d686f7e2a1b197c4a26c6290d51a7c4380
# if you want to print out your own you can find a version of marker CustomObjectMarkers.Hexagons2 used here
# http://cozmosdk.anki.com/docs/generated/cozmo.objects.html?highlight=hexagons2#cozmo.objects.CustomObjectMarkers.Hexagons2
#
# WHAT THIS PROGRAM DOES
#
# SHORT VERSION: this program makes Cozmo play until he runs low on battery, then makes him find and dock with his charger.
# Once his battery is fully recharged, he will leave the dock and start playing again. Rinse, repeat.
#
# You can run this program as-is but it is likely some things will need to be tweaked to suit your Cozmo and environment.
#
# AUTOMATIC DOCK FINDING AND DOCKING
# this is highly reliant on a number of factors, including having enough ambient light, how big the area Cozmo has to explore is,
# and just sheer blind luck sometimes. The built-in camera of Cozmo is not very high rez, and the dock icon is a little small.
# You can improve this in several ways - better lights, custom printed large battery icon (and some fiddling with the docking routing)
#
# The surface Cozmo moves on can be more slippery or less than my environment, this will impact things like docking.
# On my table's surface, I do two 95 degree turns, this makes it so Cozmo has a high rate of successful dockings.
# Look for these two lines in the code and adjust as necessary
# robot.turn_in_place(degrees(95)).wait_for_completed()
# robot.turn_in_place(degrees(95)).wait_for_completed()
#
# Your Cozmo's battery will have slightly different upper and lower limits.
# there is a lower threshold at which Cozmo will switch states and start looking for his charger.
# The variable is called lowbatvoltage, 3.7 is the setting that works for me.
# a higher value will cause Cozmo to start looking for his dock sooner.
#
# have a look at the section called
# CONFIGURABLE VARIABLES
# for more stuff
#
# DETAILED VERSION:
# This program defines a number of 'states' for Cozmo:
# State 1: on charger, charging
# State 2: on charger, fully charged
# State 3: not on charger, battery starting to get low
# State 4: not on charger, good battery - freeplay active
# State 5: battery low, looking for charger
# State 6: battery low and we know where the charger is, moving to dock and docking
# State 9: Cozmo is on its side or is currently picked up
# State 0: recovery state - used to smooth state switching and to reassess which state Cozmo should be in
# state 99: "fake" state used when running a random animation
# state 98: "fake" state used when cozmo is looking around for his charger
#
# States dictate what Cozmo does. In state 4 Cozmo will enter freeplay mode, exploring and interacting with his environment:
# stack cubes, build pyramids, offer people he sees fistbumps and games of peekaboo, and more. When his battery drops below
# a certain threshold (set in the variable lowbatvoltage), he will start looking for his charger (state 5).
# If he finds it he will attempt to dock (state 6) and start charging (state 1). Once fully charged (state 2), he drives off
# the charger and the cycle repeats (state 4). State 3 is used as a 'delay' so a single drop in battery level won't
# immediately cause him to seek out his charger. States 9 and 0 handle being picked up, falling, being on his side, and eventual
# recovery.
#
# The battery level of Cozmo is also used to manipulate his three "needs" settings: repair, energy, play (the three metrics for
# 'happiness' that you see in the main screen of the Anki Cozmo app). As his battery level gets lower so will his overall mood
# and innate reactions to events. (TLDR: he gets grumpier the lower his battery level is)
#
# An event monitor running in a seperate thread watches for events like being picked up, seeing a face, detecting a cliff, etc.
# Some events are just logged, others trigger different Cozmo states or actions.
#
#import required functions
import sys, os, datetime, random, time, math, re, threading
##import logging
import asyncio, cozmo, cozmo.objects, cozmo.util
from cozmo.util import degrees, distance_mm, speed_mmps, Pose
from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes
#external libraries used for camera annotation
#you will need to add these using pip/pip3
from PIL import ImageDraw, ImageFont
import numpy as np
#logging.basicConfig(filename='cozmo.log',level=logging.WARN)
# set up global variables
global robot
global freeplay
global tempfreeplay
global needslevel
global cozmostate
global scheduler_playokay
global msg
global start_time
global use_cubes
global charger
global maxbatvoltage
global highbatvoltage
global lowbatvoltage
global batlightcounter
global use_scheduler
global camera
global foundcharger
global lightstate
global robotvolume
global debugging
global batcounter
global chargermarker1
# initialize needed variables
freeplay = 0
lightstate = 0
batcounter = 0
robot = cozmo.robot.Robot
msg = 'No status'
q = None # dependency on queue variable for messaging instead of printing to event-content directly
thread_running = False # starting thread for custom events
#
#============================
# CONFIGURABLE VARIABLES HERE
#============================
#
# BATTERY THRESHOLDS
#
# low battery voltage - when voltage drops below this Cozmo will start looking for his charger
# high battery voltage - when cozmo comes off your charger fully charge, this value will self-calibrate
# maxbatvoltage - the maximum battery level as recorded when cozmo is on charger but no longer charging
# tweak this to suit your cozmo
lowbatvoltage = 3.7
highbatvoltage= 4.14
maxbatvoltage = 4.8
#
# CUBE USAGE
#
# whether or not to activate the cubes (saves battery if you don't)
# I almost always leave this off, he will still stack them and mess around with them
# some games like "block guard dog" will not come up unless the blocks are active
use_cubes = 1
#
# COZMO VOLUME
# what volume Cozmo should play sounds at, value between 0 and 1
robotvolume = 0.2
#
# SCHEDULER USAGE
#
# whether or not to use the schedule to define allowed "play times"
# this code is a bit rough, use at your own risk
use_scheduler = 0
#
# DEBUGGING
# when disabled, clears the screen status updates every cycle
debugging = 1
# END OF CONFIGURABLE VARIABLES
#
#
# CAMERA ANNOTATOR
#
#
@cozmo.annotate.annotator
def camera_info(image, scale, annotator=None, world=None, **kw):
global camera
d = ImageDraw.Draw(image)
bounds = [3, 0, image.width, image.height]
camera = world.robot.camera
text_to_display = 'Exposure: %s ms\n' % camera.exposure_ms
text_to_display += 'Gain: %.3f\n' % camera.gain
text = cozmo.annotate.ImageText(text_to_display,position=cozmo.annotate.TOP_LEFT,line_spacing=2,color="white",outline_color="black", full_outline=True)
text.render(d, bounds)
#
# MAIN PROGRAM LOOP START
#
def cozmo_unleashed(robot: cozmo.robot.Robot):
cozmo.setup_basic_logging(general_log_level='WARN', protocol_log_level='WARN', protocol_log_messages='all', deprecated_filter='default')
#os.system('cls' if os.name == 'nt' else 'clear')
global cozmostate,chargermarker1,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, batlightcounter, lowbatvoltage, highbatvoltage, maxbatvoltage, use_scheduler,msg,objmsg,facemsg,camera, foundcharger, tempfreeplay
#robot.world.charger = None
#charger = None
foundcharger = 0
robot.set_robot_volume(robotvolume)
if use_cubes == 1:
robot.enable_freeplay_cube_lights(enable=True)
#robot.enable_device_imu(enable_raw=False, enable_user=True, enable_gyro=True)
# set up some camera stuff
robot.world.image_annotator.add_annotator('camera_info', camera_info)
camera = robot.camera
camera.enable_auto_exposure()
robot.enable_facial_expression_estimation(enable=True)
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
if use_cubes == 0:
robot.world.disconnect_from_cubes()
else:
robot.world.connect_to_cubes()
robot.enable_all_reaction_triggers(True)
robot.enable_stop_on_cliff(True)
q = None # dependency on queue variable for messaging instead of printing to event-content directly
thread_running = False # starting thread for custom events
robot.set_needs_levels(repair_value=1, energy_value=1, play_value=1)
needslevel = 1
tempfreeplay = 0
lowbatcount=0
batlightcounter=0
cozmostate = 0
#some custom objects that I printed out and use as virtual walls, if you don't have them don't worry about it, it won't affect the program
# wall_obj1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType01, CustomObjectMarkers.Circles2, 340, 120, 44, 44, True)
# wall_obj2 = robot.world.define_custom_wall(CustomObjectTypes.CustomType02, CustomObjectMarkers.Circles4, 340, 120, 44, 44, True)
# wall_obj3 = robot.world.define_custom_wall(CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles5, 340, 120, 44, 44, True)
# wall_obj4 = robot.world.define_custom_wall(CustomObjectTypes.CustomType04, CustomObjectMarkers.Triangles2,340, 120, 44, 44, True)
# wall_obj5 = robot.world.define_custom_wall(CustomObjectTypes.CustomType05, CustomObjectMarkers.Triangles3,340, 120, 44, 44, True)
# #wall_obj6 = robot.world.define_custom_wall(CustomObjectTypes.CustomType06, CustomObjectMarkers.Hexagons2, 120, 340, 44, 44, True)
# wall_obj6 = robot.world.define_custom_wall(CustomObjectTypes.CustomType06, CustomObjectMarkers.Hexagons2, 44, 44, 44, 44, True)
# wall_obj7 = robot.world.define_custom_wall(CustomObjectTypes.CustomType07, CustomObjectMarkers.Circles3, 120, 340, 44, 44, True)
# wall_obj8 = robot.world.define_custom_wall(CustomObjectTypes.CustomType08, CustomObjectMarkers.Hexagons3, 120, 340, 44, 44, True)
chargermarker1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType01, CustomObjectMarkers.Hexagons2, 40, 40, 40, 40, True)
# initialize event monitoring thread
q = None
monitor(robot, q)
start_time = time.time()
msg = 'initialization complete'
robot_print_current_state('entering main loop')
# ENTERING STATE LOOP
while True:
#robot_backbackbatteryindicator()
#robot_print_current_state('main loop checkpoint')
#
#State 1: on charger, charging
if (robot.is_on_charger == 1) and (robot.is_charging == 1):
if cozmostate != 1: # 1 is charging
robot_print_current_state('switching to state 1')
cozmostate = 1
start_time = time.time()
foundcharger = 0
if robot.is_freeplay_mode_active:
######robot.enable_all_reaction_triggers(False)
robot.stop_freeplay_behaviors()
freeplay = 0
if use_cubes == 1:
robot.world.disconnect_from_cubes()
lowbatcount=0
cozmostate = 1
msg = 'state 1 checkpoint'
robot_print_current_state('state 1 - charging')
# once in a while make random snoring noises
robot_check_sleep_snoring()
#
#State 2: on charger, fully charged
#
if (robot.is_on_charger == 1) and (robot.is_charging == 0):
cozmostate = 2
robot_print_current_state('switching to state 2 - pausing 30 secs')
time.sleep(30)
if cozmostate != 2: # 2 is fully charged
maxbatvoltage = robot.battery_voltage
robot_print_current_state('switching to state 2')
cozmostate = 2
lowbatcount=0
foundcharger = 0
if use_cubes == 1:
robot.world.connect_to_cubes()
msg = 'state 2 checkpoint'
robot_print_current_state('state 2 - charged')
robot.set_needs_levels(repair_value=1, energy_value=1, play_value=1)
# robot.drive_off_charger_contacts().wait_for_completed()
# robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
robot_check_scheduler()
#
#State 3: not on charger, battery starting to get low
#
# basic 'trigger guard' so Cozmo doesn't go to charger immediately if the voltage happens to dip below 3.7
if (robot.battery_voltage <= lowbatvoltage) and (robot.is_on_charger == 0) and cozmostate != 5 and cozmostate != 6 and cozmostate !=98 and cozmostate != 1 and cozmostate != 2:
lowbatcount += 1
robot_set_needslevel()
msg = 'state 3 checkpoint'
robot_print_current_state('state 3 - low battery threshold breach %s' % str(lowbatcount))
robot_reaction_chance(cozmo.anim.Triggers.CodeLabTakaTaka,1,False,False,False)
# print("Event log : %s" % str(msg))
time.sleep(0.5)
# if we dip below the threshold three times we switch to state 5
if lowbatcount > 2 and (robot.is_on_charger == 0) and cozmostate !=5 and cozmostate !=6 and cozmostate !=99 and cozmostate !=98 and cozmostate != 1 and cozmostate != 2:
if use_cubes == 1:
robot.world.disconnect_from_cubes()
robot_set_needslevel()
msg = 'state 3 exit'
robot_print_current_state('state 3 - low battery - switching to state 5')
if cozmostate != 1 and cozmostate != 6:
cozmostate = 5
#
#State 4: not on charger, good battery - freeplay active
#
if (robot.battery_voltage > lowbatvoltage) and (robot.is_on_charger == 0) and cozmostate != 9 and cozmostate != 5 and cozmostate != 6 and cozmostate != 3 and lowbatcount < 3 and cozmostate != 99 and cozmostate !=98:
if cozmostate != 4: # 4 is freeplay
msg = 'state 4 checkpoint'
robot_print_current_state('freeplay - switching to state 4')
cozmostate = 4
if freeplay == 0:
freeplay = 1
start_time = time.time()
try:
robot.drive_wheels(40, 40, l_wheel_acc=50, r_wheel_acc=50, duration=1)
except:
robot_print_current_state('state 4 - failed to drive wheels')
robot_reaction_chance(cozmo.anim.Triggers.OnSpeedtapGameCozmoWinHighIntensity,1,True,False,False)
if use_cubes == 1:
robot.world.connect_to_cubes()
if not robot.is_freeplay_mode_active:
#robot.enable_all_reaction_triggers(True)
robot.start_freeplay_behaviors()
if not robot.is_freeplay_mode_active and cozmostate == 4:
robot_print_current_state('state 4 - re-enabling freeplay')
freeplay = 1
#robot.enable_all_reaction_triggers(True)
robot.start_freeplay_behaviors()
robot_set_needslevel()
if cozmostate == 4:
robot_check_randomreaction()
time.sleep(0.5)
#
# state 5: battery low, looking for charger
#
if cozmostate == 5 and tempfreeplay != 1:
robot_print_current_state('switching to state 5')
if robot.is_freeplay_mode_active:
freeplay = 0
robot.stop_freeplay_behaviors()
#robot.enable_all_reaction_triggers(True)
robot_locate_dock()
# if cozmostate == 6:
# pass
# else:
# cozmostate = 0
#
# state 6: battery low and we know where the charger is, moving to dock and docking
#
if cozmostate == 6:
robot_print_current_state('switching to state 6')
if robot.is_freeplay_mode_active:
#####robot.enable_all_reaction_triggers(False)
robot.stop_freeplay_behaviors()
freeplay = 0
robot_print_current_state('state 6 - aborting freeplay')
robot.abort_all_actions(log_abort_messages=True)
robot.wait_for_all_actions_completed()
freeplay = 0
##robot_set_needslevel()
robot_start_docking()
#
# state 9: we're on our side or are currently picked up
#
if cozmostate == 9:
robot_print_current_state('switching to state 9')
robot_flash_backpacklights(4278190335) # 4278190335 is red
robot_reaction_chance(cozmo.anim.Triggers.CodeLabUnhappy,100,False,False,False)
while cozmostate == 9:
robot_print_current_state('state 9 - anger loop')
robot_set_needslevel()
if not robot.is_falling and not robot.is_picked_up:
robot_print_current_state('state 9 reset - switching to 0')
cozmostate = 0
lightstate = 0
break
if robot.is_freeplay_mode_active:
#robot.enable_all_reaction_triggers(True)
robot.stop_freeplay_behaviors()
freeplay = 0
robot.abort_all_actions(log_abort_messages=True)
robot.clear_idle_animation()
robot.wait_for_all_actions_completed()
robot_reaction_chance(cozmo.anim.Triggers.AskToBeRightedLeft,100,False,False,False)
robot_print_current_state('picked annoyed response 1')
time.sleep(0.5)
if not robot.is_falling and not robot.is_picked_up:
robot_print_current_state('state reset - switching to 0')
cozmostate = 0
lightstate = 0
break
robot_reaction_chance(cozmo.anim.Triggers.TurtleRoll,100,False,False,False)
robot_print_current_state('picked annoyed response 2')
time.sleep(0.5)
if not robot.is_falling and not robot.is_picked_up:
robot_print_current_state('state reset - switching to 0')
cozmostate = 0
lightstate = 0
break
robot_reaction_chance(cozmo.anim.Triggers.CodeLabUnhappy,100,False,False,False)
robot_print_current_state('picked annoyed response 3')
time.sleep(0.5)
robot_print_current_state('state 9 - loop complete')
#
# state 0: recovery state
#
if cozmostate == 0:
robot_print_current_state('state 0')
#robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,True,True)
#robot.set_all_backpack_lights(cozmo.lights.white_light)
lightstate = 0
time.sleep(0.5)
#
# state 98: lookaround loop trying to find charger
#
#
# state 99: animation is playing
#
#
# we have looped through the states
#
#robot_set_needslevel()
#robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,1,True,True,True)
#msg = 'state loop complete'
#robot_check_randomreaction()
#robot_print_current_state('cozmo_unleashed state program loop complete')
time.sleep(0.5)
#
#
# END OF STATE LOOP
#
##robot_set_needslevel()
robot_reaction_chance(cozmo.anim.Triggers.CodeLabTakaTaka,1,True,True,False)
robot_print_current_state('exiting main loop - how did we get here?')
#
# ROBOT FUNCTIONS
#
def robot_set_backpacklights(color):
global robot
robot.set_backpack_lights_off()
color1=cozmo.lights.Color(int_color=color, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=2000, off_period_ms=1000, transition_on_period_ms=1500, transition_off_period_ms=500)
light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=2000)
light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=2000, transition_on_period_ms=500, transition_off_period_ms=1500)
robot.set_backpack_lights(None, light1, light2, light3, None)
def robot_flash_backpacklights(color):
global robot
robot.set_backpack_lights_off()
color1=cozmo.lights.Color(int_color=color, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=250, transition_on_period_ms=375, transition_off_period_ms=125)
light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=250, off_period_ms=250, transition_on_period_ms=250, transition_off_period_ms=500)
light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=250, off_period_ms=500, transition_on_period_ms=125, transition_off_period_ms=375)
robot.set_backpack_lights(None, light1, light2, light3, None)
def robot_backbackbatteryindicator():
global robot,highbatvoltage,lowbatvoltage,maxbatvoltage,lightstate,batlightcounter
batmultiplier = ((highbatvoltage - lowbatvoltage)/3)+0.1
chargebatmultiplier = ((maxbatvoltage - lowbatvoltage)/3)+0.1
critbatmultiplier = ((lowbatvoltage - 3.5)/3)
robotvoltage=(robot.battery_voltage)
if not lightstate:
lightstate = 0
oldlightstate = lightstate
if robotvoltage > (highbatvoltage-batmultiplier) and cozmostate==4:
# bottom two lights on, third light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=1 and lightstate !=2:
lightstate = 1
color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1)
light2=cozmo.lights.Light(on_color=color1)
light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
elif robotvoltage > (highbatvoltage-(batmultiplier*1.5)) and robotvoltage <= (highbatvoltage-batmultiplier) and cozmostate==4:
#bottom one light on, second light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=2 and lightstate !=3:
lightstate = 2
color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1)
light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
light3=cozmo.lights.Light(on_color=color2)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
elif robotvoltage > (highbatvoltage-(batmultiplier*2.5)) and robotvoltage <= (highbatvoltage-(batmultiplier*1.5)) and cozmostate==4:
# # bottom one light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=3:
lightstate = 3
color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
light2=cozmo.lights.Light(on_color=color2)
light3=cozmo.lights.Light(on_color=color2)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
elif robotvoltage >= lowbatvoltage and cozmostate==4:
batlightcounter +=1
if batlightcounter > 5 and lightstate !=4:
lightstate = 4
color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500)
light2=cozmo.lights.Light(on_color=color2)
light3=cozmo.lights.Light(on_color=color2)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
#robot_set_backpacklights(65535) # 65535 is blue
elif robotvoltage >= (maxbatvoltage-(chargebatmultiplier/2.5)) and robotvoltage <= (maxbatvoltage-(chargebatmultiplier/3.5)) and cozmostate==1:
# # bottom one light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=7 and lightstate !=6 and lightstate !=5:
lightstate = 7
color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
light2=cozmo.lights.Light(on_color=color2)
light3=cozmo.lights.Light(on_color=color2)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
elif robotvoltage >= (maxbatvoltage-(chargebatmultiplier/1.0)) and robotvoltage <= (maxbatvoltage-chargebatmultiplier/2.5) and cozmostate==1:
#bottom one light on, second light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=6 and lightstate !=5:
lightstate = 6
color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1)
light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
light3=cozmo.lights.Light(on_color=color2)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
elif robotvoltage >= maxbatvoltage and cozmostate==1:
# bottom two lights on, third light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=5:
lightstate = 5
color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1)
light2=cozmo.lights.Light(on_color=color1)
light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
# elif robotvoltage >= maxbatvoltage and cozmostate==1:
# batlightcounter +=1
# if batlightcounter > 5 and lightstate !=8:
# lightstate = 8
# color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
# color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
# light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500)
# light2=cozmo.lights.Light(on_color=color2)
# light3=cozmo.lights.Light(on_color=color2)
# robot.set_backpack_lights(None, light3, light2, light1, None)
# batlightcounter = 0
# pass
#robot_set_backpacklights(4278190335) # 4278190335 is red
elif robotvoltage >= (lowbatvoltage+(critbatmultiplier*1.5)) and robotvoltage <= (lowbatvoltage+critbatmultiplier) and cozmostate==5:
#bottom one light on, second light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=10 and lightstate !=9:
lightstate = 10
color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1)
light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
light3=cozmo.lights.Light(on_color=color2)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
elif robotvoltage >= (lowbatvoltage+(critbatmultiplier*2.5)) and robotvoltage <= (lowbatvoltage+(critbatmultiplier*1.5)) and cozmostate==5:
# # bottom one light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=11 and lightstate !=10:
lightstate = 11
color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
light2=cozmo.lights.Light(on_color=color2)
light3=cozmo.lights.Light(on_color=color2)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
elif robotvoltage >= (lowbatvoltage+critbatmultiplier) and cozmostate==5:
# bottom two lights on, third light blinking
batlightcounter +=1
if batlightcounter > 5 and lightstate !=9:
lightstate = 9
color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1)
light2=cozmo.lights.Light(on_color=color1)
light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
elif robotvoltage >= lowbatvoltage and cozmostate==5:
batlightcounter +=1
if batlightcounter > 5 and lightstate !=12:
lightstate = 12
color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None)
color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500)
light2=cozmo.lights.Light(on_color=color2)
light3=cozmo.lights.Light(on_color=color2)
robot.set_backpack_lights(None, light3, light2, light1, None)
batlightcounter = 0
pass
if lightstate==0 or lightstate==99:
lightstate=99
if robot.is_on_charger:
robot_set_backpacklights(65535) # 16711935 is green
else:
if robot.battery_voltage > lowbatvoltage:
robot_set_backpacklights(16711935) # 65535 is blue
else :
robot_set_backpacklights(4278190335) # 4278190335 is red
def robot_set_needslevel():
global robot, needslevel, msg
needslevel = 1 - (4.05 - robot.battery_voltage)
if needslevel < 0.1:
needslevel = 0.1
if needslevel > 1:
needslevel = 1
# i = random.randint(1, 1000)
# if i >= 990:
#robot_print_current_state('updating needs levels')
robot.set_needs_levels(repair_value=needslevel, energy_value=needslevel, play_value=needslevel)
def robot_check_sleep_snoring():
global robot
i = random.randint(1, 1000)
if i >= 997 and cozmostate == 1 and not robot.is_animating:
robot_print_current_state('check complete - snore')
#robot.play_anim_trigger(Sleeping).wait_for_completed()
robot_reaction_chance(cozmo.anim.Triggers.Sleeping,100,True,False,True)
else:
#robot_print_current_state('check complete - no snore')
time.sleep(0.5)
def robot_check_scheduler():
global robot,scheduler_playokay,use_cubes,use_scheduler, highbatvoltage
# from 7pm on weekdays
weekdaystartplay = 19
# to 11pm on weekdays
weekdaystopplay = 23
# from 7am on weekends
weekendstartplay = 7
# to 11pm on weekends
weekendstopplay = 23
# scheduler - when battery is charged this represents the chance cozmo will get off his charger to play
# chance is defined as a number between 1-99 with a higher number representing a lesser chance
playchance = 1
robot_print_current_state('starting schedule check')
# day and time check - are we okay to play at this time and day?
day_of_week = datetime.date.today().weekday() # 0 is Monday, 6 is Sunday
ctime = datetime.datetime.now().time()
scheduler_playokay=0
#it's weekend! Check for allowed times.
if day_of_week > 4:
if (ctime > datetime.time(weekendstartplay) and ctime < datetime.time(weekendstopplay)):
scheduler_playokay=1
#it's a weekday! Check for allowed times.
else:
if (ctime > datetime.time(weekdaystartplay) and ctime < datetime.time(weekdaystopplay)):
scheduler_playokay=1
# are we using the scheduler?
if use_scheduler==0:
scheduler_playokay=1
# if the schedule says OK roll dice to see if we wake up
if scheduler_playokay==1:
robot_print_current_state('schedule OK - random chance to wake up')
i = random.randint(1, 100)
# wake up chance
if use_scheduler==0:
i = 100
if i >= playchance:
robot_print_current_state('waking up - leaving charger')
#robot.world.connect_to_cubes()
#robot_set_backpacklights(16711935) # 16711935 is green
try:
robot.play_anim("anim_gotosleep_getout_02").wait_for_completed()
except:
robot_print_current_state('wake up anim failed')
#pass
for _ in range(3):
try:
robot.drive_off_charger_contacts().wait_for_completed()
robot_print_current_state('drive off charger OK')
except:
robot_print_current_state('drive off charger error')
#pass
time.sleep(0.5)
highbatvoltage = robot.battery_voltage
try:
robot.move_lift(-3)
except:
robot_print_current_state('lift reset fail')
#pass
try:
robot.drive_straight(distance_mm(60), speed_mmps(50)).wait_for_completed()
except:
robot_print_current_state('drive straight fail')
#pass
try:
robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
except:
robot_print_current_state('drive straight fail')
#pass
# we're out of schedule or didn't make the dice roll, back off and check again later.
x = 1
while x < 20 and (robot.is_on_charger == 1):
if scheduler_playokay == 1:
robot_print_current_state('battery charged - schedule ok - not active')
time.sleep(1)
else:
#print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x))
robot_print_current_state('battery charged - out of schedule')
time.sleep(1)
robot_check_sleep_snoring()
if (robot.is_on_charger == 0):
robot_print_current_state('cozmo was removed from charger')
break
time.sleep(2)
def robot_check_randomreaction():
global robot,cozmostate,freeplay
i = random.randint(1, 1000)
#if i >= 980 and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and not robot.is_behavior_running and cozmostate==4:
if i >= 970 and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and cozmostate==4:
oldcozmostate=cozmostate
cozmostate=99
#random action!
robot_print_current_state('random animation starting')
if robot.is_freeplay_mode_active:
#robot.enable_all_reaction_triggers(True)
robot.stop_freeplay_behaviors()
robot.abort_all_actions(log_abort_messages=True)
#robot.clear_idle_animation()
#robot.wait_for_all_actions_completed()
#robot.wait_for_all_actions_completed()
# grab a list of animation triggers
all_animation_triggers = robot.anim_triggers
# randomly shuffle the animations
random.shuffle(all_animation_triggers)
# select the first animation from the shuffled list
triggers = 1
chosen_triggers = all_animation_triggers[:triggers]
print('Playing {} random animations:'.format(triggers))
for trigger in chosen_triggers:
if 'Onboarding' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'MeetCozmo' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'list' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'List' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'Severe' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'TakaTaka' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'Test' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'Loop' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'Sleep' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'Request' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'Singing' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'Drone' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
if 'SoundOnly' in trigger:
trigger = 'cozmo.anim.Triggers.SparkSuccess'
print("trigger %s" %str(trigger)," executed")
robot_print_current_state('playing random animation')
try:
robot.play_anim_trigger(trigger).wait_for_completed()
except:
robot_print_current_state('random animation play failed')
#pass
robot_print_current_state('played random animation')
#robot.wait_for_all_actions_completed()
if freeplay == 1 and not robot.is_freeplay_mode_active:
#robot.enable_all_reaction_triggers(True)
robot.start_freeplay_behaviors()
if cozmostate==6:
cozmostate=6
else:
cozmostate=oldcozmostate
time.sleep(0.5)
def robot_reaction_chance(animation,chance,ignorebody,ignorehead,ignorelift):
global robot, msg, freeplay,cozmostate
i = random.randint(1, 100)
if i >= chance and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and cozmostate !=99:
robot_print_current_state('starting animation')
oldcozmostate=cozmostate
cozmostate=99
oldfreeplay = 0
if freeplay == 1:
if robot.is_freeplay_mode_active:
robot_print_current_state('disabling freeplay')
#robot.enable_all_reaction_triggers(True)
oldfreeplay = 1
freeplay = 0
robot.stop_freeplay_behaviors()
robot.abort_all_actions(log_abort_messages=True)
robot.stop_all_motors()
robot_print_current_state('action queue aborted')
#robot.clear_idle_animation()
robot.wait_for_all_actions_completed()
try:
robot_print_current_state('playing animation')
robot.play_anim_trigger(animation, ignore_body_track=ignorebody, ignore_head_track=ignorehead, ignore_lift_track=ignorelift).wait_for_completed()
#print("reaction %s" %str(animation)," executed")
msg = ("reaction %s" %str(animation)," executed")
robot_print_current_state('animation completed')
except:
robot_print_current_state('animation failed')
#print("reaction %s" %str(animation)," aborted")
robot.wait_for_all_actions_completed()
try:
robot_print_current_state('resetting head angle')
robot.set_head_angle(degrees(0)).wait_for_completed()
except:
robot_print_current_state('head angle reset failed')
#print("reaction %s" %str(animation)," aborted")
#robot.wait_for_all_actions_completed()
robot.wait_for_all_actions_completed()
try:
robot_print_current_state('resetting lift')
robot.move_lift(-3)
except:
robot_print_current_state('lift move down failed')
#print("reaction %s" %str(animation)," aborted")
robot.wait_for_all_actions_completed()
if oldfreeplay == 1:
oldfreeplay = 0
freeplay = 1
robot_print_current_state('re-enabling freeplay')
if not robot.is_freeplay_mode_active:
#robot.enable_all_reaction_triggers(True)
robot.start_freeplay_behaviors()
if cozmostate == 6:
cozmostate =6
elif cozmostate == 1:
cozmostate = 1
else:
cozmostate=oldcozmostate
else:
time.sleep(0.5)
robot_print_current_state('animation check - no winner')
def robot_locate_dock():
global robot,cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger, tempfreeplay
#back off from whatever we were doing
#robot_set_backpacklights(4278190335) # 4278190335 is red
if robot.is_freeplay_mode_active:
robot_print_current_state('disabling freeplay')
robot.stop_freeplay_behaviors()
#robot.enable_all_reaction_triggers(True)
robot.abort_all_actions(log_abort_messages=True)
robot_print_current_state('starting locate dock sequence')
robot.clear_idle_animation()
#robot.wait_for_all_actions_completed()
if use_cubes==1:
robot.world.disconnect_from_cubes()
freeplay = 0
robot_reaction_chance(cozmo.anim.Triggers.NeedsMildLowEnergyRequest,1,False,False,False)
try:
robot.drive_straight(distance_mm(-30), speed_mmps(50)).wait_for_completed()
except:
robot_print_current_state('drive straight failed')
robot_set_needslevel()
robot_print_current_state('finding charger')
# charger location search
if robot.world.charger and cozmostate != 1 and cozmostate != 2 and cozmostate !=6:
if robot.world.charger.pose.is_comparable(robot.pose):
charger = robot.world.charger
#we know where the charger is
robot_print_current_state('finding charger, charger position known')
robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,False)
time.sleep(0.5)
cozmostate = 6
foundcharger = 1
else:
robot_print_current_state('finding charger, charger not in expected location')
charger = None
robot.world.charger = None
if cozmostate != 1 and cozmostate != 2:
cozmostate = 5
charger = robot.world.charger
if not charger and cozmostate != 1 and cozmostate != 2 and cozmostate !=6:
robot_print_current_state('looking for charger')
cozmostate = 5
robot_reaction_chance(cozmo.anim.Triggers.SparkIdle,30,True,False,True)
try:
robot.move_lift(-3)
except:
robot_print_current_state('failed to move lift')
try:
robot.set_head_angle(degrees(0)).wait_for_completed()
except:
robot_print_current_state('failed to set head angle')
try:
robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
except:
robot_print_current_state('failed to drive')
# randomly drive around for a bit and see if we can spot the charger
robot_drive_random_pattern()
robot_print_current_state('looking for charger, random drive loop complete')
time.sleep(0.5)
def robot_drive_random_pattern():
global cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger, chargermarker1
loops=5
while loops>0 and cozmostate == 5:
if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose):
loops=0
charger = robot.world.charger
robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,True)
robot_print_current_state('found charger, breaking loop')
cozmostate = 6
foundcharger = 1
break
if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
robot_print_current_state('breaking out of drive loop')
loops=0
break
# drive to a random point and orientation
counter=0
while counter < 1 and cozmostate ==5 and cozmostate !=6:
if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
robot_print_current_state('breaking out of drive loop')
loops=0
break
if random.choice((True, False)):
x=150
else:
x=-150
if random.choice((True, False)):
y=150
else:
y=-150
z= random.randrange(-40, 41, 80)
robot_print_current_state('looking for charger, going to random pose')
try:
robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(z)), relative_to_robot=True).wait_for_completed()
robot.set_head_light(False)
time.sleep(0.25)
robot.set_head_light(True)
time.sleep(0.25)
robot.set_head_light(False)
except:
robot_print_current_state('failed to go to pose')
if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
loops=0
break
if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose):
loops=0
charger = robot.world.charger
cozmostate = 6
foundcharger = 1
robot_print_current_state('found charger, breaking')
robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,True)
break
# else:
robot_check_randomreaction()
counter+=1
if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
loops=0
break
robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,1,True,False,True)
# turn around for a bit
time.sleep(0.5)
counter=0
oldcozmostate = cozmostate
cozmostate = 98
robot_print_current_state('start lookaround behavior')
robot.set_head_angle(degrees(20)).wait_for_completed()
# if chargermarker1.pose.is_comparable(robot.pose):
# action = robot.go_to_pose(pose=chargermarker1.pose)
# action.wait_for_completed()
# else:
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try: