def toStr(self, byte_list, with_crc8=False, format=0):
"""
Do the opposite of toBytesList. Convert a list of bytes (array of 8-bit ints in decimal)
to a byte string in hex.
:param byte_list the list of bytes (as decimal ints) to convert to a byte string
:param with_crc8 1 if a crc8 checksum for the number given by byte_list should be appended to the string
Default is False, so no crc8 is computed or added
:param format can take the value 0 (bytes) or 1 (string))
If format = 0, numbers will undergo the following conversion: 15 -> 0x0f -> b'\x0f' which is a bytes
object useful for sending to the sensor. This works in Python 2.7 and 3
If format = 1, numbers will be converted directly to a hex string: 15 -> '0f', which is actually 2 bytes b'\x30\x66'.
These are the ASCII values of the characters. This is not useable for sending to a sensor.
"""
string = b''
if format == 0:
string = self.toBytes(byte_list)
else:
for i in range(len(byte_list)):
string += self.toHex(byte_list[i])
if with_crc8 == True:
if format == 0:
string += self.toBytes(
crc.crc8(byte_list, table=self.crc8_table))
else:
string += self.toHex(crc.crc8(byte_list,
table=self.crc8_table))
return string
def wait_for_packet(self, timeout=1000):
"""
Scan the incoming data for the initialization byte. If one is found, test the checksum in the next byte.
If the checksum matches, return True. The program should then read in the next 53 bytes as a packet.
:param timeout how many bytes to attempt before stopping and giving up, returning False
:param verbose Set to true to print out all the extra bytes that were read in before receiving a star byte or timing out
:return True if the program can read in the next 53 bytes as a valid data packet
False if no start byte + checksum combination is found within timeout tries
"""
data = ''
for i in range(timeout):
#Check for initialization byte
try:
dat = self.port.read(1)
if dat == b'\xAA':
#Read next 2 bytes (counter and crc8)
byte = self.readBytes(2)
if byte != None and byte != []:
counter = byte[0]
crcVal = byte[1]
else:
continue
#Combine initialization byte and counter
p = [INIT_BYTE, counter]
#Test CRC8 checksum
return crc.crc8(p, table=self.parent.crc8_table) == crcVal
except:
print('Communication failing. Replug USB')
continue
def readSTM():
global ser, inSeq, seqData, world, STATE, STOP, PLAY, onLine
LEN = 4
while ser.in_waiting:
x = ser.read()[0]
if not inSeq and x == 0xBB:
inSeq = True
seqData = [x]
elif inSeq:
if len(seqData) == LEN:
if x == crc8(seqData):
world.interface.angleUpdate(
(seqData[1] + seqData[2] / 256) * 360 / 256)
if seqData[3] % 2 == 1:
if STATE == STOP:
STATE = PLAY
else:
STATE = STOP
if (seqData[3] / 2) % 2 == 1:
onLine = time()
else:
print("incorrect STM data")
inSeq = False
elif len(seqData) < LEN:
seqData.append(x)
else:
inSeq = False
world.interface.flags = int(world.interface.flags / 2) * 2 + int(
STATE != STOP)
def stopBytes(self):
self.updateModel()
msg = [0xBB, 0, 0, 0, 0, 0, 0, 0]
msg.append(crc8(msg))
return bytes(msg)
def toBytes(self):
self.updateModel()
if abs(self.targetHead - self.head) > 180:
signum = -1
else:
signum = 1
delta = self.targetHead - self.head
if abs(delta) > 5:
delta = sign(delta) * 5
self.head += delta * signum
self.head %= 360
K = 256 / 360
d1 = int(((360 - self.dir) % 360) * K)
d2 = int((((360 - self.dir) % 360) * K - d1) * 256)
h1 = int(self.head * K)
h2 = int((self.head * K - h1) * 256)
msg = [
0xBB,
int(self.vel * 50), d1, d2, h1, h2,
int(self.acc), self.flags
]
print(msg)
msg.append(crc8(msg))
return bytes(msg)
def fixup(self, seq_num=0):
buf = self.get_buffer()
if buf[0] == START_OF_PACKET:
buf[1], buf[2] = le16(len(buf)+2)
buf[1] = (buf[1] << 3)
buf[3] = crc.crc8(buf[0:3])
buf[7], buf[8] = le16(seq_num)
self.add_int16(crc.crc16(buf))
def fixup(self, seq_num=0):
buf = self.get_buffer()
if buf[0] == START_OF_PACKET:
buf[1], buf[2] = le16(len(buf)+2)
buf[1] = (buf[1] << 3)
buf[3] = crc.crc8(buf[0:3])
buf[7], buf[8] = le16(seq_num)
self.add_int16(crc.crc16(buf))
def send_value(self, value):
value = int(value)
code = [
ArduinoServo.sync_bytes[self.out_sync], value & 0xff,
(value >> 8) & 0xff
]
b = '%c%c%c' % (code[1], code[2], crc8(code))
os.write(self.fd, b)
self.out_sync += 1
def __calc_crc(self, p, n=32, polynomial=0x04C11DB7):
"""
Create n-bit CRC Checksum
:param n the number of bits in the checksum (default is 32)
:param p the bit sequence as a hex string, or int to create the checksum for
For example, bytes 0 to 46 in the 51 byte sensor packet
:param polynomial the bit string of the CRC polynomial to use
Default is the typical CRC-32 polynomial. CRC-4 often uses 0x03
:return The n-bit checksum as a hexadecimal string
"""
if n == 8:
return crc.crc8(p, polynomial, self.crc8_table)
elif n == 32:
csum = crc.crc32(p, polynomial, self.crc32_table)
return [(csum & 0xFF000000) >> 24, (csum & 0x00FF0000) >> 16,
(csum & 0x0000FF00) >> 8, csum & 0x000000FF]
def read_packet(self):
s = self.packet_size - 1
try:
x = self.spi.xfer(self.packet(self.next_packet))
self.next_packet = [0] * s
except Exception as e:
self.close(e)
return False
for i in range(s + 1):
if not any(x):
return False
ck = crc.crc8(x[:s])
if ck == x[s]:
break
if i == s:
print('failed to syncronize spi packet')
return False
try:
y = self.spi.xfer([0])
except Exception as e:
self.close(e)
return False
x = x[1:] + y
command = x[0]
if x[0] == RF:
key = 'rf%02X%02X%02X' % (x[1], x[2], x[3])
count = x[4]
elif x[0] == IR:
key = 'ir%d' % x[1]
count = x[4]
elif x[0] == AR:
key = 'gpio' % x[1]
count = x[4]
else:
return False
self.events.append((key, count))
return True
def _check_for_msg(self, msg_cb):
while len(self.buf):
# messages must start with 0x11 0x94
while True:
if len(self.buf) > 1 and (ord(self.buf[0]) != 0x11 or ord(self.buf[1]) != 0x94):
self.buf.popleft()
elif len(self.buf) > 0 and ord(self.buf[0]) != 0x11:
self.buf.popleft()
else:
break
# messages are at least 5 bytes long
if len(self.buf) < 5:
# out of bytes, break
break
# payload length is specified in protocol
payload_len = ord(self.buf[2])
msg_len = 5+payload_len
if len(self.buf) < msg_len:
# out of bytes, break
break
msg_str = bytearray(self.buf)[0:msg_len]
msg_struct_fmt = "<HBB%uBB" % (payload_len)
msg_struct = struct.unpack(msg_struct_fmt, msg_str)
msg_crc = msg_struct[-1]
expected_crc = crc.crc8(bytes(msg_str[0:-1]))
if msg_crc == expected_crc:
msg_id = msg_struct[2]
payload = bytearray(msg_struct[3:3+payload_len])
if msg_cb is not None:
msg_cb(msg_id,payload)
else:
print "failed crc"
# remove this message from the queue
for i in range(msg_len):
self.buf.popleft()
def check_crc(self, crcval, p, n=32):
"""
Check crc Checksum with 8 or 32 bits
:param crc the n bit checksum as a list of bytes (ints) or an int
:param p the list of bytes to compare to the checksum. (This is bytes 0 to 46 in the 53 byte sensor packet)
:param polynomial the bit string of the crc polynomial to use
Default is 0x04C11DB7 which is what we use for n=32. For n=8, 0x07 is used
:return True if the checksum matches, False otherwise
"""
if type(p) == int:
p = self.toBytesList(self.toHex(p))
if type(crcval) != int:
crcval = self.to_int(crcval)
if n == 8:
checksum = crc.crc8(p, table=self.crc8_table)
elif n == 32:
checksum = crc.crc32(p, table=self.crc32_table)
return checksum == crcval
def poll(self):
if len(self.in_buf) < 3:
if not self.poller.poll(0):
return False
try:
c = os.read(self.fd, 12)
except:
return -1
self.in_buf += map(ord, c)
if len(self.in_buf) < 3:
return 0
ret = 0
while len(self.in_buf) >= 3:
code = [ArduinoServo.sync_bytes[self.in_sync]] + self.in_buf[:2]
crc = crc8(code)
#print 'got code', code, self.in_buf
if crc == self.in_buf[2]:
if self.in_sync_count == 2:
value = self.in_buf[0] + (self.in_buf[1] << 8)
if self.in_sync > 0:
self.current = value * 1.1 / .05 / 65536
ret |= ServoTelemetry.CURRENT
else:
self.voltage = (value >> 4) * 1.1 * 10560 / 560 / 4096
self.flags = value & 0xf
ret |= ServoTelemetry.VOLTAGE | ServoTelemetry.FLAGS
self.in_sync += 1
if self.in_sync == len(ArduinoServo.sync_bytes):
self.in_sync = 0
if self.in_sync_count < 2:
self.in_sync_count += 1
self.in_buf = self.in_buf[3:]
else:
self.in_sync = self.in_sync_count = 0
self.in_buf = self.in_buf[1:]
return ret
from crc import crc16, crc8
if __name__ == "__main__":
print(crc8("ss"))
val = crc8("Hello " + "World", initial=0, finalXorVal=1)
print(chr(val))
print(crc16("World"))
print(crc16("Hello " + "World"))
val = crc16("Hello " + chr(crc16("World")), initial=0)
print(val)
def test_cont(s):
calMatrix = np.eye(6)
count = 0
outer_timeout = N_SAMPLES * 10
#Generate byte message with sample rate
hz = toHex(HZ)
print(hz)
while not len(hz) == 4:
hz = '0' + hz
b = b'' + toStr([0x10, int(hz[:2], 16), int(hz[2:], 16)])
#Start timing, write, and read
startt = time.time()
s.write(b)
print('Started transmission')
times = []
while count < N_SAMPLES and outer_timeout != 0:
timeout = 100
start = time.time()
d = s.read(1)
while d != b'\xAA' and timeout != 0:
d = s.read(1)
timeout -= 1
# print('Read and ignored 1 byte: ' + str(d))
if timeout == 0:
outer_timeout -= 1
continue
d = codecs.encode(s.read(2), 'hex') #Read 2 bytes (counter and crc)
# print(d)
if len(d) == 4 and int(d[2:], 16) == crc.crc8(
int('aa' + d[:2], 16), table=crc8_table): #Check CRC 8
data = read_in(s, 53)
if len(data) == 53 and to_int(data[49:]) == crc.crc32(
data[:49], table=crc32_table): #Check CRC 32
try:
rid = rep_ids.get(data[36])
except KeyError:
print('Invalid report ID key')
out.sensor_output(None, data, rid, calMatrix) #.string()
count += 1
else:
print('CRC-32 failed')
else:
print('CRC-8 failed')
outer_timeout -= 1
times.append(time.time() - start)
s.write(b'\x11\x00\x00\xC9') #stop transmission
count = 1
while count < 100 and s.read(1) != '':
s.write(b'\x11\x00\x00\xC9') #stop transmission
count += 1
s.ftdi_fn.ftdi_usb_purge_buffers()
if count == 100:
print('Failed to stop. Still sending data.')
print(time.time() - startt)
return times
def packet(self, d):
if len(d) != self.packet_size - 1:
raise 'invalid packet'
return d + [crc.crc8(d)]
def toStr(byte_list):
string = b''
for i in range(len(byte_list)):
string += struct.pack("B", byte_list[i])
string += struct.pack("B", crc.crc8(byte_list))
return string
sys.stdout.flush()
times = []
while count < N_SAMPLES and outer_timeout != 0:
timeout = 100
start = time.time()
d = s.read(1)
while d != b'\xAA' and timeout != 0:
d = s.read(1)
timeout -= 1
if timeout == 0:
outer_timeout -= 1
continue
d = codecs.encode(s.read(2), 'hex') #Read 2 bytes (counter and crc)
if len(d) == 4 and int(d[2:], 16) == crc.crc8(
int('aa' + d[:2], 16), table=crc8_table): #Check CRC 8
data = read_in(s, 53)
if len(data) == 53 and to_int(data[49:]) == crc.crc32(
data[:49], table=crc32_table): #Check CRC 32
try:
rid = rep_ids.get(data[36])
except KeyError:
print('Invalid report ID key')
dat = out.sensor_output(None, data, rid, calMatrix)
temp.append(dat.temperature)
atiData.append(atiSensor.read())
diff.append(dat.differential)
sums.append(dat.sum)
pos2.append(p.get_current_joint_position()[2])
pos3.append(p.get_current_joint_position()[3])
def _pack_msg(self, msg_id, payload):
msg = struct.pack("<HBB%uB" % (len(payload)), (0x94<<8)|0x11, len(payload), msg_id, *bytearray(payload))
msg += struct.pack("<B", crc.crc8(msg))
return msg
def read(self):
self.posData = []
self.cartPosData = []
self.cartOrnData = []
self.effData = []
self.diff = []
self.sums = []
self.temp = []
self.atiData = []
sPos = self.insertion
outer_timeout = 45 * 1000 * 10
while self.insertion >= 0.5 * sPos and (outer_timeout != 0):
timeout = 100
d = self.s.read(1)
while d != b'\xAA' and timeout != 0:
d = self.s.read(1)
timeout -= 1
if timeout == 0:
outer_timeout -= 1
continue
d = codecs.encode(self.s.read(2),
'hex') #Read 2 bytes (counter and crc)
if len(d) == 4 and int(d[2:], 16) == crc.crc8(
int('aa' + d[:2],
16), table=self.crc8_table): #Check CRC 8
data = read_in(self.s, 53)
if len(data) == 53 and to_int(data[49:]) == crc.crc32(
data[:49], table=self.crc32_table): #Check CRC 32
try:
rid = self.rep_ids.get(data[36])
except KeyError:
print('Invalid report ID key')
dat = out.sensor_output(None, data, rid, self.calMatrix)
pos = np.append(self.arm.get_current_joint_position(),
self.arm.get_current_jaw_position())
cartesianPos = self.arm.get_current_position()
cartPos = [float(i) for i in cartesianPos.p]
cartOr = cartesianPos.M.GetQuaternion()
vel = np.append(self.arm.get_current_joint_velocity(),
self.arm.get_current_jaw_velocity())
eff = np.append(self.arm.get_current_joint_effort(),
self.arm.get_current_jaw_effort())
self.posData.append(pos)
self.cartPosData.append(cartPos)
self.cartOrnData.append(cartOr)
self.velData.append(vel)
self.effData.append(eff)
self.atiData.append(self.atiSensor.read())
self.temp.append(dat.temperature)
self.diff.append(dat.differential)
self.sums.append(dat.sum)
else:
print('CRC-32 failed')
else:
print('CRC-8 failed')
if outer_timeout == 0:
print('outer timeout')
self.numdatPoints += len(self.diff)
