def test_packet_length():
pkts = [21, 22, 23, 24, 25]
packet_len = calc_query_data_len(pkts)
assert packet_len == 8
pkts = [100]
packet_len = calc_query_data_len(pkts)
assert packet_len == 80
def run(self):
self.cr.start()
self.cr.safe()
pkts = [46, 47, 48, 49, 50, 51]
sensor_pkt_len = calc_query_data_len(pkts)
while True:
# grab camera image
# if self.camera:
# ok, img = self.camera.read()
#
# if ok: # good image capture
# self.processImage(img)
# create sensor data
# raw = self.cr.query_list(pkts, sensor_pkt_len)
# if raw:
# for p in pkts:
# self.cr.decoder.decode_packet(p, raw, self.sensors)
# print('Sensors:')
# print(self.sensors)
# get joystick
# if self.js.valid:
# ps4 = self.js.get()
# x, y = ps4['leftStick']
# # rz, _ = ps4['rightStick']
# # this is a default for testing if no joystick is found
# else:
# x = 1
# y = 0
# x, y = (0, 0)
#
# vel = sqrt(x**2 + y**2)
# rot = atan2(x, y) # remember, x is up not z
# self.command(vel, rot)
sensor_state = self.cr.get_packet(100)
print(sensor_state)
# read imu
# if self.imu:
# a, m, g = self.imu.read()
# print('imu', a, m, g)
#
# # read IR sensors
# if self.adc:
# ir = self.adc.read()
# print('ir', ir)
time.sleep(0.05)
def get_sensors(self, pkts=None):
"""
pkts: an array of packets to read.
return: a hash of the roomba's sensors/variables requeted
WARNING: now this only returns pkt 100, everything. And it is the default
packet reques now.
"""
if not pkts:
pkts = [100]
length = len(pkts)
sensor_pkt_len = calc_query_data_len(pkts)
if length == 1:
opcode = OPCODES.SENSORS
cmd = tuple(pkts)
else:
opcode = OPCODES.QUERY_LIST
cmd = (len(pkts), ) + tuple(pkts)
self.SCI.write(opcode, cmd)
time.sleep(0.015) # wait 15 msec
packet_byte_data = list(self.SCI.read(sensor_pkt_len))
packet_byte_data = array.array('B', packet_byte_data).tostring()
# print('-'*60)
# print('returned data len({})'.format(len(packet_byte_data)))
# print('type:', type(packet_byte_data))
# print(packet_byte_data)
# if data was returned, then decode it
# if packet_byte_data:
# for p in pkts:
# self.decoder.decode_packet(p, packet_byte_data, sensors)
if pkts[0] == 100:
sensors = SensorPacketDecoder(packet_byte_data)
return sensors
def run(self):
# setup create 2
port = '/dev/ttyUSB0'
self.cr = pycreate2.Create2(port)
self.cr.start()
self.cr.safe()
pkts = [46, 47, 48, 49, 50, 51]
sensor_pkt_len = calc_query_data_len(pkts)
# setup pygecko
kb_sub = zmqSub(['twist_kb'], ('192.168.1.8', 9000), hwm=20)
while True:
raw = self.cr.query_list(pkts, sensor_pkt_len)
topic, msg = kb_sub.recv()
if msg:
# print('Msg:', msg)
vel = msg.linear.x
rot = msg.angular.z
print('raw', vel, rot)
if rot == 0.0:
# self.cr.drive_straight(vel)
pass
else:
i = int(rot * 10)
i = 0 if i < 0 else i
i = 10 if i > 10 else i
r = range(1100, 0, -100)
radius = r[i]
# self.cr.drive_turn(vel, radius)
print('cmd:', vel, radius)
else:
print('no command')