-
Notifications
You must be signed in to change notification settings - Fork 20
/
create.py
1591 lines (1374 loc) · 58.8 KB
/
create.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
#
# create.py
#
# Python interface for the iRobot Create
# Licensed as Artistic License
#
# Zach Dodds [email protected]
# updated for SIGCSE 3/9/07
#
# modified by Sean Luke Oct 20 2007
#
# modified by James O'Beirne Dec 9 2009
# 1. Two new functions (senseFunc and sleepTill).
# 2. getPose fixed (call to r.sensors([POSE]) added to stop()).
# 3. move() changed to accept parameters in centimeters instead of milimeters
# for consistency with printSensors/getPose.
#
# modified by Martin Schaef Feb 2016
# 1. Added support for dirt sensor, encoders, and light bump.
# 2. Adjusted the size of the Roomba in WHEEL_SPAN
# 3. getPose seems to be broken.
import serial
import math
import time
import datetime
import thread
import threading
# some module-level definitions for the robot commands
START = chr(128) # already converted to bytes...
BAUD = chr(129) # + 1 byte
CONTROL = chr(130) # deprecated for Create
SAFE = chr(131)
FULL = chr(132)
POWER = chr(133)
SPOT = chr(134) # Same for the Roomba and Create
CLEAN = chr(135) # Clean button - Roomba
COVER = chr(135) # Cover demo - Create
MAX = chr(136) # Roomba
DEMO = chr(136) # Create
DRIVE = chr(137) # + 4 bytes
MOTORS = chr(138) # + 1 byte
LEDS = chr(139) # + 3 bytes
SONG = chr(140) # + 2N+2 bytes, where N is the number of notes
PLAY = chr(141) # + 1 byte
SENSORS = chr(142) # + 1 byte
CHANGE_TIME = chr(168)
FORCESEEKINGDOCK = chr(143) # same on Roomba and Create
# the above command is called "Cover and Dock" on the Create
DRIVEDIRECT = chr(145) # Create only
STREAM = chr(148) # Create only
QUERYLIST = chr(149) # Create only
PAUSERESUME = chr(150) # Create only
#### Sean
SCRIPT = chr(152)
ENDSCRIPT = chr(153)
WAITDIST = chr(156)
WAITANGLE = chr(157)
# the four SCI modes
# the code will try to keep track of which mode the system is in,
# but this might not be 100% trivial...
OFF_MODE = 0
PASSIVE_MODE = 1
SAFE_MODE = 2
FULL_MODE = 3
# the sensors
BUMPS_AND_WHEEL_DROPS = 7
WALL_IR_SENSOR = 8
CLIFF_LEFT = 9
CLIFF_FRONT_LEFT = 10
CLIFF_FRONT_RIGHT = 11
CLIFF_RIGHT = 12
VIRTUAL_WALL = 13
LSD_AND_OVERCURRENTS = 14
DIRT_DETECTED = 15
INFRARED_BYTE = 17
BUTTONS = 18
DISTANCE = 19
ANGLE = 20
CHARGING_STATE = 21
VOLTAGE = 22
CURRENT = 23
BATTERY_TEMP = 24
BATTERY_CHARGE = 25
BATTERY_CAPACITY = 26
WALL_SIGNAL = 27
CLIFF_LEFT_SIGNAL = 28
CLIFF_FRONT_LEFT_SIGNAL = 29
CLIFF_FRONT_RIGHT_SIGNAL = 30
CLIFF_RIGHT_SIGNAL = 31
CARGO_BAY_DIGITAL_INPUTS = 32
CARGO_BAY_ANALOG_SIGNAL = 33
CHARGING_SOURCES_AVAILABLE = 34
OI_MODE = 35
SONG_NUMBER = 36
SONG_PLAYING = 37
NUM_STREAM_PACKETS = 38
REQUESTED_VELOCITY = 39
REQUESTED_RADIUS = 40
REQUESTED_RIGHT_VELOCITY = 41
REQUESTED_LEFT_VELOCITY = 42
ENCODER_LEFT = 43
ENCODER_RIGHT = 44
LIGHTBUMP = 45
LIGHTBUMP_LEFT = 46
LIGHTBUMP_FRONT_LEFT = 47
LIGHTBUMP_CENTER_LEFT = 48
LIGHTBUMP_CENTER_RIGHT = 49
LIGHTBUMP_FRONT_RIGHT = 50
LIGHTBUMP_RIGHT = 51
# others just for easy access to particular parts of the data
POSE = 100
LEFT_BUMP = 101
RIGHT_BUMP = 102
LEFT_WHEEL_DROP = 103
RIGHT_WHEEL_DROP = 104
CENTER_WHEEL_DROP = 105
LEFT_WHEEL_OVERCURRENT = 106
RIGHT_WHEEL_OVERCURRENT = 107
ADVANCE_BUTTON = 108
PLAY_BUTTON = 109
# 0 1 2 3 4 5 6 7 8 9101112131415161718192021222324252627282930313233343536373839404142434445464748495051
SENSOR_DATA_WIDTH = [0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,2,2,1,2,2,1,2,2,2,2,2,2,2,1,2,1,1,1,1,1,2,2,2,2,2,2,1,2,2,2,2,2,2]
#The original value was 258.0 but my roomba has 235.0
WHEEL_SPAN = 235.0
WHEEL_DIAMETER = 72.0
TICK_PER_REVOLUTION = 508.8 # original 508.8
TICK_PER_MM = TICK_PER_REVOLUTION/(math.pi*WHEEL_DIAMETER)
# on my floor, a full turn is measured as sth like 450 deg
# add an error to the computation to account for that.
ANGULAR_ERROR = 360.0/450.0
# for printing the SCI modes
def modeStr( mode ):
""" prints a string representing the input SCI mode """
if mode == OFF_MODE: return 'OFF_MODE'
if mode == PASSIVE_MODE: return 'PASSIVE_MODE'
if mode == SAFE_MODE: return 'SAFE_MODE'
if mode == FULL_MODE: return 'FULL_MODE'
print 'Warning: unknown mode', mode, 'seen in modeStr'
return 'UNKNOWN_MODE'
#
# some module-level functions for dealing with bits and bytes
#
def _bytesOfR( r ):
""" for looking at the raw bytes of a sensor reply, r """
print 'raw r is', r
for i in range(len(r)):
print 'byte', i, 'is', ord(r[i])
print 'finished with formatR'
def _bitOfByte( bit, byte ):
""" returns a 0 or 1: the value of the 'bit' of 'byte' """
if bit < 0 or bit > 7:
print 'Your bit of', bit, 'is out of range (0-7)'
print 'returning 0'
return 0
return ((byte >> bit) & 0x01)
def _toBinary( val, numbits ):
""" prints numbits digits of val in binary """
if numbits == 0: return
_toBinary( val>>1 , numbits-1 )
print (val & 0x01), # print least significant bit
def _fromBinary( s ):
""" s is a string of 0's and 1's """
if s == '': return 0
lowbit = ord(s[-1]) - ord('0')
return lowbit + 2*_fromBinary( s[:-1] )
def _twosComplementInt1byte( byte ):
""" returns an int of the same value of the input
int (a byte), but interpreted in two's
complement
the output range should be -128 to 127
"""
# take everything except the top bit
topbit = _bitOfByte( 7, byte )
lowerbits = byte & 127
if topbit == 1:
return lowerbits - (1 << 7)
else:
return lowerbits
def _twosComplementInt2bytes( highByte, lowByte ):
""" returns an int which has the same value
as the twosComplement value stored in
the two bytes passed in
the output range should be -32768 to 32767
chars or ints can be input, both will be
truncated to 8 bits
"""
# take everything except the top bit
topbit = _bitOfByte( 7, highByte )
lowerbits = highByte & 127
unsignedInt = lowerbits << 8 | (lowByte & 0xFF)
if topbit == 1:
# with sufficient thought, I've convinced
# myself of this... we'll see, I suppose.
return unsignedInt - (1 << 15)
else:
return unsignedInt
def _toTwosComplement2Bytes( value ):
""" returns two bytes (ints) in high, low order
whose bits form the input value when interpreted in
two's complement
"""
# if positive or zero, it's OK
if value >= 0:
eqBitVal = value
# if it's negative, I think it is this
else:
eqBitVal = (1<<16) + value
return ( (eqBitVal >> 8) & 0xFF, eqBitVal & 0xFF )
#
# this class represents a snapshot of the robot's data
#
class SensorFrame:
""" the sensorFrame class is really a struct whose
fields are filled in by sensorStatus
"""
def __init__(self):
""" constructor -- set all fields to 0
"""
self.casterDrop = 0
self.leftWheelDrop = 0
self.rightWheelDrop = 0
self.leftBump = 0
self.rightBump = 0
self.wallSensor = 0
self.leftCliff = 0
self.frontLeftCliff = 0
self.frontRightCliff = 0
self.rightCliff = 0
self.virtualWall = 0
self.driveLeft = 0
self.driveRight = 0
self.mainBrush = 0
self.vacuum = 0
self.sideBrush = 0
self.leftDirt = 0
self.rightDirt = 0
self.remoteControlCommand = 0
self.powerButton = 0
self.spotButton = 0
self.cleanButton = 0
self.maxButton = 0
self.distance = 0
self.rawAngle = 0
self.angleInRadians = 0
self.chargingState = 0
self.voltage = 0
self.current = 0
self.temperature = 0
self.charge = 0
self.capacity = 0
self.lightBumpLeft = 0
self.lightBumpFrontLeft = 0
self.lightCenterLeft = 0
self.lightCenterRight = 0
self.lightBumpFrontRight = 0
self.lightBumpRight = 0
self.dirt = 0
def __str__(self):
""" returns a string with the information
from this SensorFrame
"""
# there's probably a more efficient way to do this...
# perhaps just making it all + instead of the separate
# += would be more efficient
#
# actually, we should make a list and call ''.join(list)
# not that we will...
#
s = ''
s += 'casterDrop: ' + str(self.casterDrop) + '\n'
s += 'leftWheelDrop: ' + str(self.leftWheelDrop) + '\n'
s += 'rightWheelDrop: ' + str(self.rightWheelDrop) + '\n'
s += 'leftBump: ' + str(self.leftBump) + '\n'
s += 'rightBump: ' + str(self.rightBump) + '\n'
s += 'wallSensor: ' + str(self.wallSensor) + '\n'
s += 'leftCliff: ' + str(self.leftCliff) + '\n'
s += 'frontLeftCliff: ' + str(self.frontLeftCliff) + '\n'
s += 'frontRightCliff: ' + str(self.frontRightCliff) + '\n'
s += 'rightCliff: ' + str(self.rightCliff) + '\n'
s += 'virtualWall: ' + str(self.virtualWall) + '\n'
s += 'driveLeft: ' + str(self.driveLeft) + '\n'
s += 'driveRight: ' + str(self.driveRight) + '\n'
s += 'mainBrush: ' + str(self.mainBrush) + '\n'
s += 'vacuum: ' + str(self.vacuum) + '\n'
s += 'sideBrush: ' + str(self.sideBrush) + '\n'
s += 'leftDirt: ' + str(self.leftDirt) + '\n'
s += 'rightDirt: ' + str(self.rightDirt) + '\n'
s += 'remoteControlCommand: ' + str(self.remoteControlCommand) + '\n'
s += 'powerButton: ' + str(self.powerButton) + '\n'
s += 'spotButton: ' + str(self.spotButton) + '\n'
s += 'cleanButton: ' + str(self.cleanButton) + '\n'
s += 'maxButton: ' + str(self.maxButton) + '\n'
s += 'distance: ' + str(self.distance) + '\n'
s += 'rawAngle: ' + str(self.rawAngle) + '\n'
s += 'angleInRadians: ' + str(self.angleInRadians) + '\n'
# no data member needed for this next line
s += 'angleInDegrees: ' + str(math.degrees(self.angleInRadians)) + '\n'
s += 'chargingState: ' + str(self.chargingState) + '\n'
s += 'voltage: ' + str(self.voltage) + '\n'
s += 'current: ' + str(self.current) + '\n'
s += 'temperature: ' + str(self.temperature) + '\n'
s += 'charge: ' + str(self.charge) + '\n'
s += 'capacity: ' + str(self.capacity) + '\n'
return s
def _toBinaryString(self):
""" this converts the calling SensorFrame into a 26-byte
string of the format the roomba sends back
"""
# todo: handle the different subsets (frames) of sensor data
# here are the 26 bytes in list form
slist = [0]*26
# First Frame
# byte 0: bumps and wheeldrops
slist[0] = self.casterDrop << 4 | \
self.leftWheelDrop << 3 | \
self.rightWheelDrop << 2 | \
self.leftBump << 1 | \
self.rightBump
# byte 1: wall data
slist[1] = self.wallSensor
# byte 2: cliff left
slist[2] = self.leftCliff
# byte 3: cliff front left
slist[3] = self.frontLeftCliff
# byte 4: cliff front right
slist[4] = self.frontRightCliff
# byte 5: cliff right
slist[5] = self.rightCliff
# byte 6: virtual wall
slist[6] = self.virtualWall
# byte 7: motor overcurrents
slist[7] = self.driveLeft << 4 | \
self.driveRight << 3 | \
self.mainBrush << 2 | \
self.vacuum << 1 | \
self.sideBrush
# byte 8: dirt detector left
slist[8] = self.leftDirt
# byte 9: dirt detector left
slist[9] = self.rightDirt
# Second Frame
# byte 10: remote control command
slist[10] = self.remoteControlCommand
# byte 11: buttons
slist[11] = self.powerButton << 3 | \
self.spotButton << 2 | \
self.cleanButton << 1 | \
self.maxButton
# bytes 12, 13: distance
highVal, lowVal = _toTwosComplement2Bytes( self.distance )
slist[12] = highVal
slist[13] = lowVal
# bytes 14, 15: angle
highVal, lowVal = _toTwosComplement2Bytes( self.rawAngle )
slist[14] = highVal
slist[15] = lowVal
# Third Frame
# byte 16: charging state
slist[16] = self.chargingState
# bytes 17, 18: voltage
slist[17] = (self.voltage >> 8) & 0xFF
slist[18] = self.voltage & 0xFF
# bytes 19, 20: current
highVal, lowVal = _toTwosComplement2Bytes( self.current )
slist[19] = highVal
slist[20] = lowVal
# byte 21: temperature
slist[21] = self.temperature
# bytes 22, 23: charge
slist[22] = (self.charge >> 8) & 0xFF
slist[23] = self.charge & 0xFF
# bytes 24, 25: capacity
slist[24] = (self.capacity >> 8) & 0xFF
slist[25] = self.capacity & 0xFF
# convert to a string
s = ''.join([ chr(x) for x in slist ])
return s
#
# the robot class
#
class Create:
""" the Create class is an abstraction of the iRobot Create's
SCI interface, including communication and a bit
of processing of the strings passed back and forth
when you create an object of type Create, the code
will try to open a connection to it - so, it will fail
if it's not attached!
"""
# to do: check if we can start in other modes...
def __init__(self, PORT, BAUD_RATE=115200, startingMode=SAFE_MODE):
""" the constructor which tries to open the
connection to the robot at port PORT
"""
_debug = False
# to do: find the shortest safe serial timeout value...
# to do: use the timeout to do more error checking than
# is currently done...
#
# the -1 here is because windows starts counting from 1
# in the hardware control panel, but not in pyserial, it seems
# if PORT is the string 'simulated' (or any string for the moment)
# we use our SRSerial class
print 'PORT is', PORT
if type(PORT) == type('string'):
if PORT == 'sim':
print 'In simulated mode...'
self.ser = 'sim'; # SRSerial('mapSquare.txt')
else:
# for Mac/Linux - use whole port name
# print 'In Mac/Linux mode...'
self.ser = serial.Serial(PORT, baudrate=BAUD_RATE, timeout=0.5)
# otherwise, we try to open the numeric serial port...
else:
# print 'In Windows mode...'
self.ser = serial.Serial(PORT-1, baudrate=BAUD_RATE, timeout=0.5)
# did the serial port actually open?
if self.ser != 'sim' and self.ser.isOpen():
print 'Serial port did open, presumably to a roomba...'
else:
print 'Serial port did NOT open, check the'
print ' - port number'
print ' - physical connection'
print ' - baud rate of the roomba (it\'s _possible_, if unlikely,'
print ' that it might be set to 19200 instead'
print ' of the default 57600 - removing and'
print ' reinstalling the battery should reset it.'
# our OI mode
self.sciMode = OFF_MODE
# our sensor dictionary, currently empty
self.sensord = {}
# here are the variables that constitute the robot's
# estimated odometry, thr is theta in radians...
# these are updated by integrateNextOdometricStep
self.xPose = 0.0
self.yPose = 0.0
self.thrPose = 0.0
self.leftEncoder = -1
self.rightEncoder = -1
self.leftEncoder_old = -1
self.rightEncoder_old = -1
time.sleep(0.3)
self._start() # go to passive mode - want to do this
# regardless of the final mode we'd like to be in...
time.sleep(0.3)
if (startingMode == SAFE_MODE):
print 'Putting the robot into safe mode...'
self.toSafeMode()
if (startingMode == FULL_MODE):
print 'Putting the robot into full mode...'
self.toSafeMode()
time.sleep(0.3)
self.toFullMode()
# We need to read the angle and distance sensors so that
# their values clear out!
time.sleep(0.25)
#self.sensors(6) # read all sensors to establish the sensord dictionary
self.setPose(0,0,0)
_debug = False
def _write(self, byte):
if self._debug==True:
print ord(byte)
self.ser.write(byte)
def getPose(self, dist='cm', angle='deg'):
""" getPose returns the current estimate of the
robot's global pose
dist may be 'cm' or 'mm'
angle may be 'deg' or 'rad'
"""
x = 0; y = 0; th = 0
if dist == 'cm':
x = self.xPose/10.0; y = self.yPose/10.0
else:
x = self.xPose; y = self.yPose
if angle == 'deg':
th = math.degrees(self.thrPose)
else:
th = self.thrPose
return (x,y,th)
def setPose(self, x, y, th, dist='cm', angle='deg'):
""" setPose sets the internal odometry to the input values
x: global x in mm
y: global y in mm
th: global th in radians
dist: 'cm' or 'mm' for x and y
angle: 'deg' or 'rad' for th
"""
if dist == 'cm':
self.xPose = x*10.0; self.yPose = y*10.0
else:
self.xPose = x; self.yPose = y
if angle == 'deg':
self.thrPose = math.radians(th)
else:
self.thrPose = th
def resetPose(self):
""" resetPose simply sets the internal odometry to 0,0,0
"""
self.setPose(0.0,0.0,0.0)
def _getEncoderDelta(self, oldEnc, newEnc):
#encoder wrap around at 2^16
#check if the step is bigger than half the
#possible range and treat this as wraparound
delta = newEnc-oldEnc
if delta< -65536/2:
delta = (newEnc+65536)-oldEnc
if delta> 65536/2:
delta = newEnc-(oldEnc+65536)
return delta
def _integrateNextEncoderStep(self):
if self.leftEncoder_old == -1:
self.leftEncoder_old = self.leftEncoder
self.rightEncoder_old = self.rightEncoder
return
left_diff = self._getEncoderDelta(self.leftEncoder_old,self.leftEncoder)
right_diff = self._getEncoderDelta(self.rightEncoder_old,self.rightEncoder)
left_mm = left_diff / TICK_PER_MM;
right_mm = right_diff / TICK_PER_MM;
distance = (left_mm + right_mm) / 2.0;
dAngle = (right_mm - left_mm) / WHEEL_SPAN
dAngle *= ANGULAR_ERROR
self.thrPose += dAngle
if self.thrPose > 100*math.pi:
self.thrPose -= 101*math.pi
if self.thrPose < -100*math.pi:
self.thrPose += 101*math.pi
self.xPose += distance * math.cos(self.thrPose)
self.yPose += distance * math.sin(self.thrPose)
self.leftEncoder_old = self.leftEncoder
self.rightEncoder_old = self.rightEncoder
def _integrateNextOdometricStepCreate(self, distance, rawAngle):
""" integrateNextOdometricStep adds the reported inputs
distance in mm
rawAngle in degrees
to the estimate of the robot's global pose
"""
# OK, so this _should_ be easy
# distance is, supposedly, the arc length that the center
# of the robot has traveled (the average of
# the two wheel's linear distances)
#
# rawAngle is, supposedly, the (RightWheel-LeftWheel)/2.0
#
# the distance (diameter) between the two wheels is 258mm
# keep in mind that the robot's physical diameter is larger ~
#
# 0.5*258 == 129mm radius
#
# perhaps there's nothing to do...
if distance == 0 and rawAngle == 0:
return
print rawAngle
# then again, maybe there is something to do...
dthr = math.radians(rawAngle) # angle traveled
d = distance # distance traveled
# compute offsets in the local coordinate system,
# with the x-axis pointing in the direction the robot was
# initially facing (for this move) and the y-axis pointing
# perpendicularly to the left (when facing forward)
#
# first, the special case when the angle is zero...
if rawAngle == 0:
dx = float(d)
dy = 0.0
# or if the distance is zero...
elif distance == 0:
dx = 0.0
dy = 0.0
# or when neither is zero...
else:
# finite radius of curvature
ROC = float(d)/dthr # remember, this is signed!
dx = ROC*math.sin(dthr) # because ROC is signed,
dy = ROC-ROC*math.cos(dthr) # we don't need two cases
#
# we need to add dx, dy, and dthr to the global pose
# and so we need to do so in the global direction in
# which the robot was facing at the start of this movement
#
# here is the unit vector describing that direction
unitForwardX = math.cos( self.thrPose )
unitForwardY = math.sin( self.thrPose )
# here is the unit vector perpendicular to the left
unitPerpX = math.cos( self.thrPose + math.pi/2.0 )
unitPerpY = math.sin( self.thrPose + math.pi/2.0 )
# now we compute our global offsets
dx_global = dx*unitForwardX + dy*unitPerpX
dy_global = dx*unitForwardY + dy*unitPerpY
##print 'distance and rawAngle', distance, rawAngle
##print 'local offsets, x, y, thd', dx, dy, math.degrees(dthr)
##print 'global offsets, x, y, thd', dx_global, dy_global, math.degrees(dthr)
# and we add them all in...
self.xPose += dx_global
self.yPose += dy_global
self.thrPose += dthr
#print 'final pose', self.xPose, self.yPose, self.thrPose
return
def setWheelVelocities( self, left_cm_sec, right_cm_sec ):
""" sends velocities of each wheel independently
left_cm_sec: left wheel velocity in cm/sec (capped at +- 50)
right_cm_sec: right wheel velocity in cm/sec (capped at +- 50)
"""
if left_cm_sec < -50: left_cm_sec = -50;
if left_cm_sec > 50: left_cm_sec = 50;
if right_cm_sec < -50: right_cm_sec = -50;
if right_cm_sec > 50: right_cm_sec = 50;
# convert to mm/sec, ensure we have integers
leftHighVal, leftLowVal = _toTwosComplement2Bytes( int(left_cm_sec*10) )
rightHighVal, rightLowVal = _toTwosComplement2Bytes( int(right_cm_sec*10) )
# send these bytes and set the stored velocities
self._write( DRIVEDIRECT )
self._write( chr(rightHighVal) )
self._write( chr(rightLowVal) )
self._write( chr(leftHighVal) )
self._write( chr(leftLowVal) )
def stop(self):
""" stop calls go(0,0) """
self.go(0,0)
# we've gotta update pose information
foo = self.sensors([POSE])
def motors ( self, side_brush = 0, main_brush = 0, vacuum = 0):
if (side_brush > 1):
side_brush = 1
elif (side_brush < -1):
side_brush = -1
if (main_brush > 1):
main_brush = 1
elif (main_brush < -1):
main_brush = -1
if (vacuum > 1):
vacuum = 1
elif (vacuum < 0):
vacuum = 0
byteToWrite = chr((16 if main_brush < 0 else 0) |
(8 if side_brush < 0 else 0) |
(4 if main_brush != 0 else 0) |
(2 if vacuum != 0 else 0) |
(1 if side_brush > 0 else 0))
self._write( MOTORS )
self._write( byteToWrite)
def go( self, cm_per_sec=0, deg_per_sec=0 ):
""" go(cmpsec, degpsec) sets the robot's velocity to
cmpsec centimeters per second
degpsec degrees per second
go() is equivalent to go(0,0)
"""
# need to convert to the roomba's drive parameters
#
# for now, just one or the other...
if cm_per_sec == 0:
# just handle rotation
# convert to radians
rad_per_sec = math.radians(deg_per_sec)
# make sure the direction is correct
if rad_per_sec >= 0: dirstr = 'CCW'
else: dirstr = 'CW'
# compute the velocity, given that the robot's
# radius is 258mm/2.0
vel_mm_sec = math.fabs(rad_per_sec) * (WHEEL_SPAN/2.0)
# send it off to the robot
self._drive( vel_mm_sec, 0, dirstr )
elif deg_per_sec == 0:
# just handle forward/backward translation
vel_mm_sec = 10.0*cm_per_sec
big_radius = 32767
# send it off to the robot
self._drive( vel_mm_sec, big_radius )
else:
# move in the appropriate arc
rad_per_sec = math.radians(deg_per_sec)
vel_mm_sec = 10.0*cm_per_sec
radius_mm = vel_mm_sec / rad_per_sec
# check for extremes
if radius_mm > 32767: radius_mm = 32767
if radius_mm < -32767: radius_mm = -32767
self._drive( vel_mm_sec, radius_mm )
return
def _start(self):
""" changes from OFF_MODE to PASSIVE_MODE """
self._write( START )
# they recommend 20 ms between mode-changing commands
time.sleep(0.25)
# change the mode we think we're in...
return
def close(self):
""" tries to shutdown the robot as kindly as possible, by
clearing any remaining odometric data
going to passive mode
closing the serial port
"""
# is there other clean up to be done?
# let's get rid of any lingering odometric data
# we don't call getSensorList, because we don't want to integrate the odometry...
self._getRawSensorDataAsList( [19,20] )
time.sleep(0.1)
self._start() # send Create back to passive mode
time.sleep(0.1)
self.ser.close()
return
def _closeSer(self):
""" just disconnects the serial port """
self.ser.close()
return
def _openSer(self):
""" opens the port again """
self.ser.open()
return
def _drive(self, roomba_mm_sec, roomba_radius_mm, turn_dir='CCW'):
""" implements the drive command as specified
the turn_dir should be either 'CW' or 'CCW' for
clockwise or counterclockwise - this is only
used if roomba_radius_mm == 0 (or rounds down to 0)
other drive-related calls are available
"""
# first, they should be ints
# in case they're being generated mathematically
if type(roomba_mm_sec) != type(42):
roomba_mm_sec = int(roomba_mm_sec)
if type(roomba_radius_mm) != type(42):
roomba_radius_mm = int(roomba_radius_mm)
# we check that the inputs are within limits
# if not, we cap them there
if roomba_mm_sec < -500:
roomba_mm_sec = -500
if roomba_mm_sec > 500:
roomba_mm_sec = 500
# if the radius is beyond the limits, we go straight
# it doesn't really seem to go straight, however...
if roomba_radius_mm < -2000:
roomba_radius_mm = 32768
if roomba_radius_mm > 2000:
roomba_radius_mm = 32768
# get the two bytes from the velocity
# these come back as numbers, so we will chr them
velHighVal, velLowVal = _toTwosComplement2Bytes( roomba_mm_sec )
# get the two bytes from the radius in the same way
# note the special cases
if roomba_radius_mm == 0:
if turn_dir == 'CW':
roomba_radius_mm = -1
else: # default is 'CCW' (turning left)
roomba_radius_mm = 1
radiusHighVal, radiusLowVal = _toTwosComplement2Bytes( roomba_radius_mm )
#print 'bytes are', velHighVal, velLowVal, radiusHighVal, radiusLowVal
# send these bytes and set the stored velocities
self._write( DRIVE )
self._write( chr(velHighVal) )
self._write( chr(velLowVal) )
self._write( chr(radiusHighVal) )
self._write( chr(radiusLowVal) )
def setLEDs(self, power_color, power_intensity, play, advance ):
""" The setLEDs method sets each of the three LEDs, from left to right:
the power LED, the play LED, and the status LED.
The power LED at the left can display colors from green (0) to red (255)
and its intensity can be specified, as well. Hence, power_color and
power_intensity are values from 0 to 255. The other two LED inputs
should either be 0 (off) or 1 (on).
"""
# make sure we're within range...
if advance != 0: advance = 1
if play != 0: play = 1
try:
power = int(power_intensity)
powercolor = int(power_color)
except TypeError:
power = 128
powercolor = 128
print 'Type excpetion caught in setAbsoluteLEDs in roomba.py'
print 'Your power_color or power_intensity was not of type int.'
if power < 0: power = 0
if power > 255: power = 255
if powercolor < 0: powercolor = 0
if powercolor > 255: powercolor = 255
# create the first byte
#firstByteVal = (status << 4) | (spot << 3) | (clean << 2) | (max << 1) | dirtdetect
firstByteVal = (advance << 3) | (play << 1)
# send these as bytes
# print 'bytes are', firstByteVal, powercolor, power
self._write( LEDS )
self._write( chr(firstByteVal) )
self._write( chr(powercolor) )
self._write( chr(power) )
return
#
# DO NOT CALL THIS FUNCTION!
# call readSensors instead - it will integrate odometry
# for what that's worth, admittedly...
# if you call this without integrating odometry, the
# distance and rawAngle reported will be lost...
#
def _getRawSensorFrameAsList(self, packetnumber):
""" gets back a raw string of sensor data
which then can be used to create a SensorFrame
"""
if type(packetnumber) != type(1):
packetnumber = 6
if packetnumber < 0 or packetnumber > 6:
packetnumber = 6
self._write( SENSORS )
self._write( chr(packetnumber) )
if packetnumber == 0:
r = self.ser.read(size=26)
if packetnumber == 1:
r = self.ser.read(size=10)
if packetnumber == 2:
r = self.ser.read(size=6)
if packetnumber == 3:
r = self.ser.read(size=10)
if packetnumber == 4:
r = self.ser.read(size=14)
if packetnumber == 5:
r = self.ser.read(size=12)
if packetnumber == 6:
r = self.ser.read(size=52)
r = [ ord(c) for c in r ] # convert to ints
return r
def _getRawSensorDataAsList(self, listofsensors):
""" gets the chosen sensors
and returns the raw bytes, as a string
needs to be converted to integers...
"""
numberOfSensors = len(listofsensors)
self._write( QUERYLIST )
self._write( chr(numberOfSensors) )
resultLength = 0
for sensornum in listofsensors:
self._write( chr(sensornum) )
resultLength += SENSOR_DATA_WIDTH[sensornum]
r = self.ser.read(size=resultLength)
r = [ ord(c) for c in r ] # convert to ints
#print 'r is ', r
return r
def seekDock(self):
""" sends the force-seeking-dock signal
"""
self.demo(1)
def demo(self, demoNumber=-1):
""" runs one of the built-in demos for Create
if demoNumber is
<omitted> or
-1 stop current demo
0 wander the surrounding area
1 wander and dock, when the docking station is seen
2 wander a more local area
3 wander to a wall and then follow along it
4 figure 8
5 "wimp" demo: when pushed, move forward
when bumped, move back and away
6 home: will home in on a virtual wall, as
long as the back and sides of the IR receiver
are covered with tape
7 tag: homes in on sequential virtual walls
8 pachelbel: plays the first few notes of the canon in D
9 banjo: plays chord notes according to its cliff sensors
chord key is selected via the bumper
"""
if (demoNumber < -1 or demoNumber > 9):
demoNumber = -1 # stop current demo
self._write( DEMO )
if demoNumber < 0 or demoNumber > 9:
# invalid values are equivalent to stopping
self._write( chr(255) ) # -1
else:
self._write( chr(demoNumber) )
def setSong(self, songNumber, songDataList):
""" this stores a song to roomba's memory to play later
with the playSong command
songNumber must be between 0 and 15 (inclusive)
songDataList is a list of (note, duration) pairs (up to 16)
note is the midi note number, from 31 to 127
(outside this range, the note is a rest)
duration is from 0 to 255 in 1/64ths of a second
"""
# any notes to play?
if type(songDataList) != type([]) and type(songDataList) != type(()):
print 'songDataList was', songDataList