def test_process_sensor_pkt_long(self):
#data = b'\xff\xfe\x03\x00\x0b\x00\x00\x00\x00\x00d\x00\x00\x00\x00\x8d'
data = b'\xff\xfe\x03\x00\x0b\xde\xad\xbe\xef\x00\x42\xde\xad\xbe\xef\x8d'
masks = [
{
'len': 2,
'callback': MyTestCase.process_2_tuple
},
{
'len': 1,
'callback': MyTestCase.process_1
},
{
'len': 2,
'callback': MyTestCase.process_2_tuple2
},
]
s = DelegateObj(None, None)
expected2a = (-8531, -16657)
expected1 = 66
expected2b = (-8531, -16657)
s.process_sensor_pkt(masks, data)
self.assertEqual(expected2a, MyTestCase.tuple_2_result)
self.assertEqual(expected1, MyTestCase.process_1_result)
self.assertEqual(expected2b, MyTestCase.tuple_2a_result)
def connect(self):
'''
Connects the sphero with the address given in the constructor
:return: True if it succeeds, False if it times out
'''
try:
with Timeout(5):
self._device = bluepy.btle.Peripheral(
self._addr, addrType=bluepy.btle.ADDR_TYPE_RANDOM)
except Timeout:
raise TimeoutError("Device Timed out")
self._notifier = DelegateObj(self, self._notification_lock)
#set notifier to be notified
self._device.withDelegate(self._notifier)
self._devModeOn()
self._connected = True #Might need to change to be a callback format
#get the command service
cmd_service = self._device.getServiceByUUID(RobotControlService)
self._cmd_characteristics = {}
characteristic_list = cmd_service.getCharacteristics()
for characteristic in characteristic_list:
uuid_str = binascii.b2a_hex(
characteristic.uuid.binVal).decode('utf-8')
self._cmd_characteristics[uuid_str] = characteristic
return True
def test_process_sensor_pkt(self):
data = b'\xff\xfe\x03\x00\x07\xff0\x00[\x0f\xefm'
masks = [{'len': 3, 'callback': MyTestCase.process_3_tuple}]
s = DelegateObj(None, None)
expected3 = (-208, 91, 4079)
s.process_sensor_pkt(masks, data)
self.assertEqual(expected3, MyTestCase.tuple_3_result)
def test_verify_checksum_malformed(self):
# Arrange
input = b'\xff\xfe\x03\x00\x07\xef0\x00[\x0f\xefm'
s = DelegateObj(None, None)
expected = False
# Act
actual = s.verify_checksum(input)
# Assert
self.assertEqual(expected, actual)
def connect(self, addr):
"""
Connects the sphero with the address given in the constructor
"""
self._device = bluepy.btle.Peripheral(addr, addrType=bluepy.btle.ADDR_TYPE_RANDOM)
self._notifier = DelegateObj(self, self._notification_lock)
#set notifier to be notified
self._device.withDelegate(self._notifier)
self._devModeOn()
self._connected = True #Might need to change to be a callback format
#get the command service
cmd_service = self._device.getServiceByUUID(RobotControlService)
self._cmd_characteristics = {}
characteristic_list = cmd_service.getCharacteristics()
for characteristic in characteristic_list:
uuid_str = binascii.b2a_hex(characteristic.uuid.binVal).decode('utf-8')
self._cmd_characteristics[uuid_str] = characteristic
self._listening_flag = True
self._listening_thread = threading.Thread(target=self._listening_loop)
self._listening_thread.start()
class Sphero(object):
RAW_MOTOR_MODE_OFF = "00"
RAW_MOTOR_MODE_FORWARD = "01"
RAW_MOTOR_MODE_REVERSE = "02"
RAW_MOTOR_MODE_BRAKE = "03"
RAW_MOTOR_MODE_IGNORE = "04"
MASK_ORDER = [
{"name":"accel_raw", "size":3},
{"name":"gyro_raw", "size":3},
{"name":"emf_raw", "size":2},
{"name":"pwm_raw", "size":2},
{"name":"imu_filtered", "size":3},
{"name":"accel_filtered", "size":3},
{"name":"gyro_filtered", "size":3},
{"name":"emf_filtered", "size":2},
{"name":"quarternion", "size":4},
{"name":"odometer", "size":2},
{"name":"accelone", "size":1},
{"name":"velocity", "size":2},
]
def __init__(self, addr=None):
if(addr == None):
#search for sphero
sphero_list = search_for_sphero()
if(len(sphero_list) == 0):
raise Exception("No Sphero Found in Vicinity")
addr = sphero_list[0]
self._addr = addr
self._connected = False
self._seq_counter = 0
self._stream_rate = 10
#load the mask list
with open(os.path.join(os.path.dirname(__file__),'data','mask_list1.yaml'),'r') as mask_file1:
self._mask_list1 = yaml.load(mask_file1)
with open(os.path.join(os.path.dirname(__file__),'data','mask_list2.yaml'),'r') as mask_file2:
self._mask_list2 = yaml.load(mask_file2)
self._data_mask1 = bytes.fromhex("0000 0000")
self._data_mask2 = bytes.fromhex("0000 0000")
self._active_masks = {}
self._active_mask_callbacks = []
self._notification_lock = threading.RLock()
#start a listener loop
def connect(self, addr):
"""
Connects the sphero with the address given in the constructor
"""
self._device = bluepy.btle.Peripheral(addr, addrType=bluepy.btle.ADDR_TYPE_RANDOM)
self._notifier = DelegateObj(self, self._notification_lock)
#set notifier to be notified
self._device.withDelegate(self._notifier)
self._devModeOn()
self._connected = True #Might need to change to be a callback format
#get the command service
cmd_service = self._device.getServiceByUUID(RobotControlService)
self._cmd_characteristics = {}
characteristic_list = cmd_service.getCharacteristics()
for characteristic in characteristic_list:
uuid_str = binascii.b2a_hex(characteristic.uuid.binVal).decode('utf-8')
self._cmd_characteristics[uuid_str] = characteristic
self._listening_flag = True
self._listening_thread = threading.Thread(target=self._listening_loop)
self._listening_thread.start()
def _devModeOn(self):
"""
A sequence of read/write that enables the developer mode
"""
service = self._device.getServiceByUUID(BLEService)
characteristic_list = service.getCharacteristics()
#make it into a dict
characteristic_dict = {}
for characteristic in characteristic_list:
uuid_str = binascii.b2a_hex(characteristic.uuid.binVal).decode('utf-8')
characteristic_dict[uuid_str] = characteristic
characteristic = characteristic_dict[AntiDosCharacteristic]
characteristic.write("011i3".encode(),True)
characteristic = characteristic_dict[TXPowerCharacteristic]
characteristic.write((7).to_bytes(1, 'big'),True)
characteristic = characteristic_dict[WakeCharacteristic]
characteristic.write((1).to_bytes(1, 'big'),True)
def command(self, cmd, data, resp=True):
"""
cmd - (str) Hex String that is the command's code(ff, no need to put \\x in front)
data - [bytes/str/int] an array of values with what to send. We will reformat int and string
resp - (bool) whether the command will only return after we get an acknowledgement from Sphero. If set to false, sphero will be set to NOT even send a response to save bandwidth
-----
return - (tuple) A tuple with the first element being sequence number and second element being the response if blocked, None if not
"""
#format data
data_list = self._format_data_array(data)
#set the sop2 based on the blocking command
sop2 = "ff" if resp else "fe"
#send command
seq_num = self._send_command(sop2, "02", cmd, data_list)
#check if blocking
if(resp):
resp = self._notifier.wait_for_resp(seq_num)
#return the sequence number and response
return (seq_num, resp)
else:
return (seq_num, None)
def _send_command(self,sop2,did,cid,data_list):
sop1 = binascii.a2b_hex("ff")
sop2 = binascii.a2b_hex(sop2)
did = binascii.a2b_hex(did)
cid = binascii.a2b_hex(cid)
seq_val = self._get_sequence()
seq = seq_val.to_bytes(1,"big")
dlen = (count_data_size(data_list)+1).to_bytes(1,"big")#add one for checksum
packet = [sop1,sop2,did,cid,seq,dlen] + data_list
packet += [cal_packet_checksum(packet[2:]).to_bytes(1,'big')] #calculate the checksum
#write the command to Sphero
#print("cmd:{} packet:{}".format(cid, b"".join(packet)))
with self._notification_lock:
self._cmd_characteristics[CommandsCharacteristic].write(b"".join(packet))
return seq_val
def _listening_loop(self):
pass
#while(self._listening_flag):
#with self._notification_lock:
#self._device.waitForNotifications(0.001)
def _get_sequence(self):
val = self._seq_counter
self._seq_counter += 1
self._seq_counter = self._seq_counter%256
return val
def _format_data_array(self, arr):
"""
helper function that converts int or string to bytes, just want to decrease the number of codes
"""
if isinstance(arr,list):
for i,value in enumerate(arr):
if isinstance(value, str):
arr[i] = binascii.a2b_hex(value)
elif isinstance(value, int):
arr[i] = value.to_bytes(1,'big')
return arr
""" CORE functionality """
def ping(self):
return self._send_command("ff","00","01",[])
def version(self):
#NOTE returning weird data not sure what's wrong
seq_num = self._send_command("ff","00","02",[])
response = self._notifier.wait_for_resp(seq_num)
data_response = response[5:-1]
version_data = {}
version_data["RECV"] = hex(data_response[0])
version_data["MDL"] = hex(data_response[1])
version_data["HW"] = data_response[2]
version_data["MSA-ver"] = data_response[3]
version_data["MSA-rev"] = data_response[4]
version_data["BL"] = hex(data_response[5])
return version_data
def get_device_name(self):
seq_num = self._send_command("ff","00","11",[])
response = self._notifier.wait_for_resp(seq_num)
name_data = {}
name_data["name"] = str(response[5:21],'utf-8').rstrip(' \t\r\n\0')
name_data["bta"] = str(response[21:33],'utf-8')
name_data["color"] = str(response[33:36],'utf-8')
return name_data
""" Sphero functionality """
def roll(self, speed, heading, resp=False):
"""
Roll the ball towards the heading
speed - (int) speed
heading - (int) which direction, 0 - 359
resp - (bool) whether the code will wait for comfirmation from Sphero
"""
heading_bytes = heading.to_bytes(2,byteorder='big')
data = [speed,heading_bytes[0],heading_bytes[1], 1]
#send command
self.command('30',data, resp=resp)
def boost(self):
raise NotImplementedError
def set_heading(self, heading, resp=False):
"""
change the heading of the robot
heading - (int) heading in the range of 0-355
resp - (bool) Whether to receive comfirmation response from sphero
"""
heading_bytes = heading.to_bytes(2,byteorder='big')
data = [heading_bytes[0],heading_bytes[1]]
#send command
self.command("01",data, resp=resp)
def set_rgb_led(self, red, green, blue, resp=False):
"""
Set the color of Sphero's LED
red - (int) Color of red in range 0-255
green - (int) Color of green in range 0-255
blue - (int) Color of blue in range 0-255
resp - (bool) whether the code will wait for comfirmation from Sphero
"""
#set data
data = [red, green, blue, 0]
#send command
self.command("20", data, resp=resp)
def get_rgb_led(self):
"""
Get the color of Sphero's LED
----
return - tuple of the color in RGB
"""
#set the correct command
(seq_num, resp) = self.command("22",[])
#parse the response packet and make sure it's correct
if resp and resp[4] == 4:
MRSP = resp[2]
red = resp[5]
green = resp[6]
blue = resp[7]
return (red, green, blue)
else:
return None
def _handle_mask(self,group_name, mask=1, remove=False):
'''
Setup Mask for Data
:param group_name: Name of Group in Yaml File
:param mask: Which YAML file to use (1 or 2)
:param remove: Whether this is a remove action
:return:
'''
if(remove):
optr = XOR_mask
else:
optr = OR_mask
if(mask==1):
for i,group in enumerate(self._mask_list1):
if(group["name"] == group_name):
for i, value in enumerate(group["values"]):
self._data_mask1 = optr(self._data_mask1, bytes.fromhex(value["mask"]))
elif(mask==2):
for i,group in enumerate(self._mask_list2):
if(group["name"] == group_name):
for i, value in enumerate(group["values"]):
self._data_mask2 = optr(self._data_mask2, bytes.fromhex(value["mask"]))
def _send_data_command(self,rate,mask1,mask2,sample=1):
N = ((int)(400/rate)).to_bytes(2,byteorder='big')
#N = (40).to_bytes(2,byteorder='big')
M = (sample).to_bytes(2,byteorder='big')
PCNT = (0).to_bytes(1,'big')
#MASK2 = (mask2).to_bytes(4,'big')
data = [N,M, mask1 ,PCNT,mask2]
resp = self.command("11",data, resp=True) #make sure sphero actully receive this
return resp
def _stop_data_stream(self, group_name, mask_id = 1):
#handle mask
self._handle_mask(group_name, mask=mask_id, remove=True)
self._send_data_command(self._stream_rate, self._data_mask1, (0).to_bytes(4, 'big'))
def get_mask_order(self):
mask_order = []
mask_list = self._active_masks
for mask in Sphero.MASK_ORDER:
name = mask['name']
if(name in mask_list):
if(not mask_list[name] is None):
mask_order.append({'len': mask['size'], 'callback': self._active_masks[name]})
return mask_order
def add_mask(self, mask, callback):
self._active_masks[mask] = callback
def remove_mask(self, mask):
self._active_masks[mask] = None
def update_streaming(self, rate=10):
'''
Update Streaming
:param rate: Rate of Streaming (Make sure factor of 400Hz)
:return:
'''
self._stream_rate = rate
self._send_data_command(rate, self._data_mask1, self._data_mask2)
self._notifier.update_callbacks()
pass
def set_stream_callback(self, name, callback, mask_id = 1):
'''
Set a callback that streams the specified data to the callback
:param name: Name of Group, must match mask_list1 or mask_list2
:param callback: (function) function that we will pass the information when there is a callback
:param mask_id: Which mask (1 or 2)
:return:
'''
#first we register the callback with the notifier
self._notifier.register_async_callback(name,callback)
self.add_mask(name, callback)
# enable mask
self._handle_mask(name, mask=mask_id)
def remove_stream_callback(self, name, mask_id = 1):
self._handle_mask(name, mask=mask_id, remove=True)
def set_stabilization(self,bool_flag, resp=False):
"""
Enable/Disable stabilization of Sphero
bool_flag - (bool) stabilization on/off
resp[Optional] - (bool) whether the code will wait for comfirmation from Sphero, default to False
"""
data = ["01" if bool_flag else "00"]
self.command("02",data, resp=resp)
def set_raw_motor_values(self,lmode,lpower,rmode,rpower, resp=False):
"""
Set the raw motor values of Sphero
lmode - (str) the hex string(without \\x) of the mode
lpower - (int) the value of the power from 0-255
rmode - (str) the hex string(without \\x) of the mode
rpower - (int) the value of the power from 0-255
resp[Optional] - (bool) whether the code will wait for comfirmation from Sphero, default to False
"""
data = [lmode, int(lpower), rmode, int(rpower)]
#By default, we going to cancel it
self.command("33",data, resp=resp)
""" About MACRO """
def abort_macro(self, id_):
"""
Abort the current macro with the given ID
id - (int) the ID of the macro to stop
"""
data = [id_]
self.command("55",data)
def run_macro(self, id_):
"""
Start the macro with the given ID
id_ - (int) the 8-bit ID of the macro
"""
data = [id_]
self.command("50",data)
""" OrbBasic the programming language """
STORAGE_RAM = "00"
STORAGE_PERSISTENT = "01"
def erase_orb_basic_storage(self, area, block=True):
"""
Erase any existing program in the stored area
area - (str) hex name of the area to be cleaned
"""
data = [area]
seq = self.command("60", data)
if(block):
return self._notifier.wait_for_sim_response(seq)
else:
return True
def run_orb_basic_program(self, area, start_line):
"""
Run a the orb_basic program stored in that area
area - (str) hex name of the area
start_line - (int) the decimal line number to start
"""
data = [area,start_line.to_bytes(2,byteorder='big')]
seq = self.command("62", data)
return self._notifier.wait_for_sim_response(seq)
def abort_orb_basic_program(self):
"""
Abort the orb_basic program
"""
data = []
seq = self.command("63", data)
return self._notifier.wait_for_sim_response(seq)
def append_orb_basic_fragment(self, area,val):
"""
Append the value into ht orb basic given the area
val - (list of strings) the command broken down into a list of hex values
area - (str) hex name of the area
"""
val.insert(0, area)
data = val
seq = self.command("61",data)
if self._notifier.wait_for_sim_response(seq):
pass
else:
print("error in appending orbbasic fragments")
def append_orb_basic_line(self, area,code):
"""
Append the line to the existing code
"""
#first convert the line into list of bytes
code_list = []
for c in code:
code_list.append(bytes(c,encoding="UTF-8"))
if len(code_list) == 0:
code_list.append(b'\x00') # NULL in the end
#code_list.append(b'\x0a')#append the terminating line
#send it to next part of the program
self.append_orb_basic_fragment(area,code_list)