Issue with multiple bytes from Irobot Create 2

Question

I was having problems reading sensor information from my Irobot Create 2 and sent an email asking for help from the Irobot staff. They were super helpful and gave me an answer(the next day!!!) that helped push along my project. I was requesting data from the create2 to print to the screen so I could figure out how to write a code that would read the data. I started with this section of code that was not working for me (I trimmed some of the code off that controlled other functions):

from Tkinter import *
from subprocess import call
import datetime
import serial
import ttk
import struct
import thread

port = '/dev/ttyUSB0' #sets the com port for Atlas
baud = 115200 #sets the baud rate
connection = serial.Serial(port, baud) #starts the serial communication

#program to read communication from create2
def program2(threadName):
    while True:
        x = connection.read()
        print x

#program to write to create2    
def program1(threadName):
    atlas = Tk() #starts a new GUI for atlas control
    atlas.geometry('1000x500') #sets the size of the control window
    atlas.title('Atlas Control Panal') #sets the name of the control window

    def sendCommandASCII(command):#used to send a command to the create2
    cmd = ""
    for v in command.split():
        cmd += chr(int(v))

    sendCommandRaw(cmd)

    def sendCommandRaw(command):#used to send a command to the create2
    global connection

    try:
        if connection is not None:
            connection.write(command)
        else:
            tkMessageBox.showerror('Not connected!', 'Not connected to a robot!')
            print "Not connected."
    except serial.SerialException:
        print "Lost connection"
        tkMessageBox.showinfo('Uh-oh', "Lost connection to the robot!")
        connection = None

    def test():#sets a test command up to check connection
        global buttonpress
        buttonpress='test'
        sendCommandASCII('142 7')

    #makes a button on the GUI that starts the test command
    button1 = Button(atlas, text = 'test mode', command=test)
    button1.place(x=600, y=400)
    button1.config(width=10, height=5)

    atlas.mainloop() #runs the GUI as a loop to keep the window open.

#runs the read and the write program at the same time
try:
    thread.start_new_thread(program1, ("program1",))
    thread.start_new_thread(program2, ("program2",))
except Exception, e:
    print str(e)

They told me that the code was actually working fine but I was trying to print out the value of the sensor packet without parsing it in any way. They then recommended I change the code in program2 to this:

while True:

    def toHexFromByte(val):
        return hex(ord(val))[2:}.rjust(2, '0').upper()

    x = connection.read()
    for b in x:
        print toHexFromByte(b)

this works beautifully and prints to the screen if the bumper is pressed or a wheel drops. My question is how to deal with responses that are longer than one byte (ie Packet ID: 22 for voltage)?

When I try Packet ID: 22 it prints to screen and it sends the high byte of 3F and a low byte of D7. I can manually combine them to get 3FD7 and convert it to a decimal of 16.343 Volts but I would like to figure a way to print to screen the the voltage by having my program do this for me. Many of the sensors send data in 2 bytes and I need a way to make sure that it is combined automatically.

Robb

Answer 1

The program shown below illustrates two ways of determining result size, and a method that reads the right number of bytes to combine.

The first method suggested in the program below is to define singleSet, a set containing the packet numbers of sensors that return single-byte results. [The function resultSize() as shown uses return ord(sizesList[sensor]) - ord('0'); a method depending on singleSet would instead say return 1 if sensor in singleSet else 2.

The second method shown is slightly more adaptable than the first, and if extended would allow treatment of large packets #0-#6. With a bit more work it could accomodate packets #100-#107.

Here is the output from the program:

 7  8  9 10 11 12 13 14 15 16 17 18 21 24 32 34 35 36 37 38 45 52 53 58
 1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1  1

 0  1  2  3  4  5  6 19 20 22 23 25 26 27 28 29 30 31 33 39 40 41 42 43 44 46 47 48 49 50 51 54 55 56 57
26 10  6 10 14 12 52  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2  2

 1  16      54  0x36         54.000
 1  17      71  0x47         71.000
 1  18      88  0x58         88.000
 2  19   27002  0x697a    27002.000
 2  20   35740  0x8b9c    35740.000
 1  21     173  0xad        173.000
 2  22   48847  0xbecf       48.847
 2  23   57585  0xe0f1       57.585
 1  24       3  0x3           3.000
 2  25    5157  0x1425        5.157
 2  26   13895  0x3647       13.895
 2  27   22633  0x5869    22633.000

Here is the program itself. Replace the call to dummy routine connectionread() with your connection.read() call.

#!/usr/bin/env python
# re: Combining bytes from Create2 sensor inputs 
# http://robotics.stackexchange.com/questions/7219/issue-with-multiple-bytes-from-irobot-create-2

singleSet = set((7,8,9,10,11,12,13,14,15,16,17,18,21,24,
                 32,34,35,36,37,38,45,52,53,58))

sizesList = 'J:6:><d1111111111112212212222222121111122222212222221122221'

def resultSize(sensor):
    if sensor < 59:       # Sensor groups > 58 not handled
        return ord(sizesList[sensor]) - ord('0')
    else: return -1

# The next 11 lines print debugging / demo  output
for i in singleSet:  print '{:>2}'.format(i),
print
for i in singleSet:  print '{:>2}'.format(sizesList[i]),
print; print

for i in range(len(sizesList)):
    if i not in singleSet: print '{:>2}'.format(i),
print
for i in range(len(sizesList)):
    if i not in singleSet: print '{:>2}'.format(resultSize(i)),
print; print

# Routine to scale a sensor result and return real result
def scaleResult(sensor, r):
    if sensor in (22,23,25,26,54,55,56,57):
        return r/1000.0
    else:
        return r

# Dummy routine in place of connection.read()
crv = 37
def connectionread():
    global crv;
    crv = (crv+17)%255
    return crv

# Routine to query a sensor, read one or two bytes, and return result
def readResult(sensor):
    if 6 < sensor < 59:         # Work on basic sensors, not groups
        valu = 0
        for i in range(resultSize(sensor)):
            valu = (valu<<8) | connectionread()
        return valu
    else:
        return -1

# Some test cases
for i in range(16,28):
    r = readResult(i)
    print '{:>2} {:>3} {:>7}  {:8}  {:9.3f}'.format(resultSize(i), i, r, hex(r), scaleResult(i, r))

There are of course dozens of ways to address the issue, and the two shown above are two of the simpler ones. If you are going to create a large software suite, you might instead use a more sophisticated approach, in which you would set up a record for each kind of packet, with each record containing packet number and name, some brief id, size, scale, and units. For example, packet 25 would have a record like (25, 'Battery Charge', 'B_Ch', 2, 1000m 'Ah'). Records would be stored in a dict indexed by packet number. Records could also include group-packets membership information.

Answer 2

Many of the sensors send data in 2 bytes and I need a way to make sure that it is combined automatically.

Robb,

I have been working on a project and have faced a similar problem. The best way that I have found to combine individual bytes has been to use bit shifting to concatenate the bytes into a single variable.

I am using Arduino to interface with the Create 2, and I am trying to work with the encoder counts of each wheel, which are two bytes signed. I load an integer array with the bytes individually, and then, I join them as follows:

right_encoder=(array[0] << 8)|(array[1]);

array[0] is the high byte, and array[1] is the low byte. The bit shifting shifts the high byte by 8 bits, which makes room for the low byte. The low byte is then 'or'ed with the shifted high byte, which finalizes the concatenation.

Obviously, the syntax will look different for you, but the idea is the same.