def __init__(self,
dev_name=DEFAULT_DEV_NAME,
baud_rate=DEFAULT_BAUD_RATE,
dest_addr=DEFAULT_DEST_ADDR):
'''
Description:
Initiate the 802.15.4 connection and configure the target.
Class must be instantiated with a connection name. On Windows this is
typically of the form "COMX". On Mac, the serial device connection
string is typically '/dev/tty.DEVICE_NAME-SPP-(1)' where the number at
the end is optional depending on the version of the OS.
Inputs:
dev_name: The device name to connect to. Examples are COM5 or
/dev/tty.usbserial.
print ord(datum)
'''
self.dest_addr = dest_addr
self.last_packet = None
self.data_cnt = 0
self.state_data = []
self.last_sample_count = 0
self.radio = BaseStation(dev_name, baud_rate, dest_addr, self.receive)
self.receive_callback = []
def __init__(self, dev_name=DEFAULT_DEV_NAME, baud_rate=DEFAULT_BAUD_RATE, dest_addr=DEFAULT_DEST_ADDR):
'''
Description:
Initiate the 802.15.4 connection and configure the target.
Class must be instantiated with a connection name. On Windows this is
typically of the form "COMX". On Mac, the serial device connection
string is typically '/dev/tty.DEVICE_NAME-SPP-(1)' where the number at
the end is optional depending on the version of the OS.
Inputs:
dev_name: The device name to connect to. Examples are COM5 or
/dev/tty.usbserial.
print ord(datum)
'''
self.dest_addr = dest_addr
self.last_packet = None
self.data_cnt = 0
self.state_data = []
self.last_sample_count = 0
self.radio = BaseStation(dev_name, baud_rate, dest_addr, self.receive)
class DynaRoach(object):
'''Class representing the dynaRoACH robot'''
def __init__(self,
dev_name=DEFAULT_DEV_NAME,
baud_rate=DEFAULT_BAUD_RATE,
dest_addr=DEFAULT_DEST_ADDR):
'''
Description:
Initiate the 802.15.4 connection and configure the target.
Class must be instantiated with a connection name. On Windows this is
typically of the form "COMX". On Mac, the serial device connection
string is typically '/dev/tty.DEVICE_NAME-SPP-(1)' where the number at
the end is optional depending on the version of the OS.
Inputs:
dev_name: The device name to connect to. Examples are COM5 or
/dev/tty.usbserial.
print ord(datum)
'''
self.dest_addr = dest_addr
self.last_packet = None
self.data_cnt = 0
self.state_data = []
self.last_sample_count = 0
self.radio = BaseStation(dev_name, baud_rate, dest_addr, self.receive)
self.receive_callback = []
def add_receive_callback(self, callback):
self.receive_callback.append(callback)
def receive(self, packet):
self.last_packet = packet
pld = Payload(packet.get('rf_data'))
typeID = pld.type
data = pld.data
for callback in self.receive_callback:
callback(pld)
if typeID == cmd.TEST_ACCEL or typeID == cmd.TEST_GYRO:
print unpack('<3h', data)
elif typeID == cmd.TEST_DFLASH:
print ''.join(data)
elif typeID == cmd.TEST_BATT:
print unpack('2H', data)
elif typeID == cmd.TX_SAVED_DATA:
datum = list(unpack('<L3f3h2HB4H', data))
self.state_data.append(datum)
self.data_cnt += 1
if self.data_cnt % 100 == 0:
print self.data_cnt, "/", self.last_sample_count
elif typeID == cmd.GET_SAMPLE_COUNT:
self.last_sample_count = unpack('H', data)[0]
print('Last sample count %d' % self.last_sample_count)
elif typeID == cmd.GET_GYRO_CALIB_PARAM:
self.gyro_offsets = list(unpack('<fff', data))
print(self.gyro_offsets)
elif cmd.DATA_STREAMING:
if (len(data) == 35):
datum = list(unpack('<L3f3h2HB4H', data))
# print datum[6:]
def echo(self):
'''
Description:
This test sends three packets to the target requesting
it echo them back. The results should be the
receipt of three packets. The payloads of those three packets
should print as consecutive integers 0-9, 10-19, and 20-29
respectively.
'''
for i in range(1, 4):
data_out = ''.join(
[chr(datum) for datum in range((i - 1) * 10, i * 10)])
print("Transmitting data " + str(i) + "...")
self.radio.send(0, cmd.ECHO, data_out)
time.sleep(0.2)
self.print_packet(self.last_packet)
print('\n')
print('\n')
time.sleep(1)
def set_motor_config(self, rising_duty_cycle, falling_duty_cycle):
'''
Set the motor config for rising and falling strides.
Rising_duty_cycle and falling_duty_cycle are floats from 0-1
'''
max_int = 2**15
cmd_data = str(pack('h', int(max_int * rising_duty_cycle)))
cmd_data += str(pack('h', int(max_int * falling_duty_cycle)))
self.radio.send(cmd.STATUS_UNUSED, cmd.MOTOR_CONFIG, cmd_data)
time.sleep(0.5)
def set_data_streaming(self, val):
data = ''.join(chr(val))
self.radio.send(cmd.STATUS_UNUSED, cmd.DATA_STREAMING, data)
time.sleep(0.5)
def reset(self, do_wait=True):
self.radio.send(cmd.STATUS_UNUSED, cmd.RESET, [])
if do_wait:
time.sleep(11)
def configure_trial(self, trial):
'''
Description:
Configure trial parameters.
Parameters:
trial: The trial configuration to be saved.
'''
data_out = trial.to_cmd_data()
print("Configuring trial...")
self.radio.send(cmd.STATUS_UNUSED, cmd.CONFIG_TRIAL, data_out)
def configure_sma(self, sma):
'''
Description:
Configure SMA actuation parameters
Parameters:
sma: the sma configuration to be saved.
'''
data_out = sma.to_cmd_data()
self.state_data = []
self.data_cnt = 0
self.radio.send(cmd.STATUS_UNUSED, cmd.CONFIG_SMA, data_out)
def run_trial(self):
'''
Description:
Start a trial running.
Parameters:
trial: The configured trial to be executed.
'''
self.radio.send(cmd.STATUS_UNUSED, cmd.RUN_TRIAL, [])
def run_gyro_calib(self, num_samples='2000'):
print("Calibrating gyroscope...")
self.radio.send(cmd.STATUS_UNUSED, cmd.RUN_GYRO_CALIB, [num_samples])
def get_gyro_calib_param(self):
print("Requesting gyro calibration parameters...")
self.radio.send(cmd.STATUS_UNUSED, cmd.GET_GYRO_CALIB_PARAM, [])
self.gyro_offsets = None
def test_gyro(self):
'''
Description:
Read the XYZ values from the gyroscope.
'''
print("Testing gyroscope...")
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_GYRO, [])
def test_accel(self):
'''
Description:
Read the XYZ values from the accelerometer.
'''
print("Testing accelerometer...")
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_ACCEL, [])
def test_dflash(self):
'''
Description:
Read out a set of strings that have been written to and read from
memory.
'''
print("Testing data flash...")
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_DFLASH, [])
# def test_motor(self, motor_id, time, duty_cycle, direction, return_emf=0):
# '''
# Description:
# Turn on a motor.
# Parameters:
# motor_id : The motor number to turn on
# time : The amount of time to turn the motor on for (in
# seconds)
# duty_cycle : The duty cycle of the PWM signal used to control the
# motor in percent (0 - 100)
# direction : The direction to spin the motor. There are *three*
# options for this parameter. 0 - reverse, 1 - forward,
# 2 high impedance motor controller output = braking
# return_emf : Send the back emf readings over the radio channel.
# '''
#
# if direction >= 2:
# direction = 2
# elif direction <= 0:
# direction = 0
# else:
# direction = 1
#
#
# if return_emf != 1:
# return_emf = 0
#
# data_out = chr(cmd.STATUS_UNUSED) + chr(cmd.TEST_MOTOR) + chr(motor_id) + \
# chr(time) + chr(duty_cycle) + chr(direction) + \
# chr(return_emf)
# if(self.check_conn()):
# self.radio.tx(dest_addr=self.dest_addr, data=data_out)
# def test_sma(self, chan_id, time, duty_cycle):
# '''
# Description:
# Turn on an SMA
# Parameters:
# chan_id : The SMA channel to turn on
# time : The amount of time to turn the SMA on for (in
# seconds)
# duty_cycle : The duty cycle of the PWM signal used to control the
# SMA in percent (0 - 100)
# '''
#
# if(duty_cycle < 0 or duty_cycle > 100):
# print("You entered an invalid duty cycle.")
# return
#
# data_out = chr(cmd.STATUS_UNUSED) + chr(cmd.TEST_SMA) + chr(chan_id) + \
# chr(time) + chr(duty_cycle)
#
# if(self.check_conn()):
# self.radio.tx(dest_addr=self.dest_addr, data=data_out)
def test_hall(self):
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_HALL, [])
def test_batt(self):
data = ''.join(chr(0) for i in range(4))
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_BATT, data)
def get_sample_count(self):
self.radio.send(cmd.STATUS_UNUSED, cmd.GET_SAMPLE_COUNT, pack('H', 0))
def transmit_saved_data(self):
if (self.last_sample_count == 0):
self.get_sample_count()
time.sleep(0.5)
if (self.last_sample_count == 0):
print("There is no previously saved data.")
return
else:
self.data_cnt = 0
start_page = 0x200
print("Transmitting saved data...")
self.state_data = []
self.data_cnt = 0
self.radio.send(
cmd.STATUS_UNUSED, cmd.TX_SAVED_DATA,
pack('3H', start_page, self.last_sample_count, SAMPLE_BYTES))
def erase_mem_sector(self, sector):
print("Erasing memory sector " + str(sector))
self.radio.send(cmd.STATUS_UNUSED, cmd.ERASE_MEM_SECTOR,
pack('H', sector))
def print_packet(self, packet):
'''
Description:
Print the contents of a packet to the screen. This function
will primarily be used for debugging purposes. May need to
replace print with stdio or stderr to accommodate GUI test
suite.
'''
if (packet is not None):
print("Received the following: ")
print("RSSI: " + str(ord(packet.get('rssi'))))
(src_h, src_l) = unpack('cc', packet.get('source_addr'))
print("Source Address: 0x%.2X 0x%.2X" % (ord(src_h), ord(src_l)))
print("ID: " + (packet.get('id')))
print("Options: " + str(ord(packet.get('options'))))
rf_data = packet.get('rf_data')
print("Status Field: " + str(ord(rf_data[0])))
print("Type Field: " + str(ord(rf_data[1])))
print("Payload Data: " +
''.join([str(ord(i)) for i in rf_data[2:]]))
def save_trial_data(self, fname):
print('%d total state data were received.' % self.data_cnt)
state_data_arr = np.array(self.state_data)
print state_data_arr
fmt = '%d, %f, %f, %f, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d'
np.savetxt(fname, state_data_arr, fmt)
#np.savetxt(self.datestring() + ".csv", state_data_arr, fmt)
print("State data saved to file: " + fname)
def __del__(self):
'''
Description:
Clean up the connection when the object is deleted.
'''
try:
self.radio.close()
except AttributeError:
print('Caught Attribute Error')
pass
#Handle packet in whatever way is appropriate
print command
print len(parameter)
#The packet is data recieved from the radio
elif name == 'rx':
src_addr = packet.get('source_addr')
rssi = packet.get('rssi')
options = packet.get('options')
data = packet.get('rf_data')
#Handle packet in whatever way is appropriate
print 'Packet received'
#Initialize the basestation and the helper functions
xb = BaseStation('/dev/tty.usbserial-A800fdFZ', 230400, DEST_ADDR, xbee_received)
base = BaseFunctions(xb)
robot = RobotFunctions(xb)
if __name__ == '__main__':
base.getChannel('a')
base.getSrcAddr('b')
base.getPanID('c')
base.setChannel('\x17')
robot.go(40)
time.sleep(1)
class DynaRoach():
'''Class representing the dynaRoACH robot'''
def __init__(self, dev_name=DEFAULT_DEV_NAME, baud_rate=DEFAULT_BAUD_RATE, dest_addr=DEFAULT_DEST_ADDR):
'''
Description:
Initiate the 802.15.4 connection and configure the target.
Class must be instantiated with a connection name. On Windows this is
typically of the form "COMX". On Mac, the serial device connection
string is typically '/dev/tty.DEVICE_NAME-SPP-(1)' where the number at
the end is optional depending on the version of the OS.
Inputs:
dev_name: The device name to connect to. Examples are COM5 or
/dev/tty.usbserial.
print ord(datum)
'''
self.dest_addr = dest_addr
self.last_packet = None
self.data_cnt = 0
self.state_data = []
self.last_sample_count = 0
self.radio = BaseStation(dev_name, baud_rate, dest_addr, self.receive)
def receive(self, packet):
self.last_packet = packet
pld = Payload(packet.get('rf_data'))
typeID = pld.type
data = pld.data
if typeID == cmd.TEST_ACCEL or typeID == cmd.TEST_GYRO:
print unpack('<3h', data)
elif typeID == cmd.TEST_DFLASH:
print ''.join(data)
elif typeID == cmd.CMD_RUN_CAM:
camint = unpack('>i',data)
print camint
elif typeID == cmd.TEST_BATT:
print unpack('2H', data)
elif typeID == cmd.TX_SAVED_DATA:
datum = list(unpack('<L3f3h2HB4H', data))
self.state_data.append(datum)
self.data_cnt += 1
if self.data_cnt % 100 == 0:
print self.data_cnt, "/", self.last_sample_count
elif typeID == cmd.GET_SAMPLE_COUNT:
self.last_sample_count = unpack('H', data)[0]
print('Last sample count %d' % self.last_sample_count)
elif typeID == cmd.GET_GYRO_CALIB_PARAM:
self.gyro_offsets = list(unpack('<fff', data))
print(self.gyro_offsets)
elif cmd.DATA_STREAMING:
if (len(data) == 35):
datum = list(unpack('<L3f3h2HB4H', data))
print datum[6:]
def readimage(path):
count= os.stat(path).st_size/2
with open(path,"rb") as f:
return bytearray(f.read())
bytes= readimage(path+extension)
image= Image.open(io.BytesIO(bytes))
image.save(savepath)
def test_LED(self):
self.radio.send(cmd.STATUS_UNUSED, cmd.CMD_TEST_LED, [])
def test_dynacam(self):
#todo: find a way to control timing.
Rows= 120
Columns= 160
imagearray= np.zeros((Rows, Columns,3), dtype= np.uint8)
#outputarray= np.zeros((120,160))
#Native Rows = 120 Native Columns= 160
print ('testing DynaCam')
self.radio.send(cmd.STATUS_UNUSED, cmd.CMD_RUN_CAM, [])
self.print_packet(self.last_packet)
data = self.last_packet('rf_data')
for i in range(0,Rows):
for j in range(0, Columns):
imagearray [i][j] = data[j]
def echo(self):
'''
Description:
This test sends three packets to the target requesting
it echo them back. The results should be the
receipt of three packets. The payloads of those three packets
should print as consecutive integers 0-9, 10-19, and 20-29
respectively.
'''
for i in range(1, 4):
data_out = ''.join([chr(datum) for datum in range((i-1)*10,i*10)])
print("Transmitting data " + str(i) + "...")
self.radio.send(0, cmd.ECHO, data_out)
time.sleep(0.2)
self.print_packet(self.last_packet)
print('\n')
print('\n')
time.sleep(1)
def set_motor_config(self, rising_duty_cycle, falling_duty_cycle):
'''
Set the motor config for rising and falling strides.
Rising_duty_cycle and falling_duty_cycle are floats from 0-1
'''
max_int = 2**15;
cmd_data = str(pack('h', int(max_int * rising_duty_cycle)))
cmd_data += str(pack('h', int(max_int * falling_duty_cycle)))
self.radio.send(cmd.STATUS_UNUSED, cmd.MOTOR_CONFIG, cmd_data)
time.sleep(0.5)
def set_data_streaming(self, val):
data = ''.join(chr(val));
self.radio.send(cmd.STATUS_UNUSED, cmd.DATA_STREAMING, data)
time.sleep(0.5)
def reset(self, do_wait=True):
self.radio.send(cmd.STATUS_UNUSED, cmd.RESET, [])
if do_wait:
time.sleep(11)
def configure_trial(self, trial):
'''
Description:
Configure trial parameters.
Parameters:
trial: The trial configuration to be saved.
'''
data_out = trial.to_cmd_data()
print("Configuring trial...")
self.radio.send(cmd.STATUS_UNUSED, cmd.CONFIG_TRIAL, data_out)
def configure_sma(self, sma):
'''
Description:
Configure SMA actuation parameters
Parameters:
sma: the sma configuration to be saved.
'''
data_out = sma.to_cmd_data()
self.state_data = []
self.data_cnt = 0
self.radio.send(cmd.STATUS_UNUSED, cmd.CONFIG_SMA, data_out)
def run_trial(self):
'''
Description:
Start a trial running.
Parameters:
trial: The configured trial to be executed.
'''
self.radio.send(cmd.STATUS_UNUSED, cmd.RUN_TRIAL, [])
def run_gyro_calib(self, num_samples='2000'):
print("Calibrating gyroscope...")
self.radio.send(cmd.STATUS_UNUSED, cmd.RUN_GYRO_CALIB, [num_samples])
def get_gyro_calib_param(self):
print("Requesting gyro calibration parameters...")
self.radio.send(cmd.STATUS_UNUSED, cmd.GET_GYRO_CALIB_PARAM, [])
self.gyro_offsets = None
def test_gyro(self):
'''
Description:
Read the XYZ values from the gyroscope.
'''
print("Testing gyroscope...")
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_GYRO, [])
def test_accel(self):
'''
Description:
Read the XYZ values from the accelerometer.
'''
print("Testing accelerometer...")
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_ACCEL, [])
def test_dflash(self):
'''
Description:
Read out a set of strings that have been written to and read from
memory.
'''
print("Testing data flash...")
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_DFLASH, [])
# def test_motor(self, motor_id, time, duty_cycle, direction, return_emf=0):
# '''
# Description:
# Turn on a motor.
# Parameters:
# motor_id : The motor number to turn on
# time : The amount of time to turn the motor on for (in
# seconds)
# duty_cycle : The duty cycle of the PWM signal used to control the
# motor in percent (0 - 100)
# direction : The direction to spin the motor. There are *three*
# options for this parameter. 0 - reverse, 1 - forward,
# 2 high impedance motor controller output = braking
# return_emf : Send the back emf readings over the radio channel.
# '''
#
# if direction >= 2:
# direction = 2
# elif direction <= 0:
# direction = 0
# else:
# direction = 1
#
#
# if return_emf != 1:
# return_emf = 0
#
# data_out = chr(cmd.STATUS_UNUSED) + chr(cmd.TEST_MOTOR) + chr(motor_id) + \
# chr(time) + chr(duty_cycle) + chr(direction) + \
# chr(return_emf)
# if(self.check_conn()):
# self.radio.tx(dest_addr=self.dest_addr, data=data_out)
# def test_sma(self, chan_id, time, duty_cycle):
# '''
# Description:
# Turn on an SMA
# Parameters:
# chan_id : The SMA channel to turn on
# time : The amount of time to turn the SMA on for (in
# seconds)
# duty_cycle : The duty cycle of the PWM signal used to control the
# SMA in percent (0 - 100)
# '''
#
# if(duty_cycle < 0 or duty_cycle > 100):
# print("You entered an invalid duty cycle.")
# return
#
# data_out = chr(cmd.STATUS_UNUSED) + chr(cmd.TEST_SMA) + chr(chan_id) + \
# chr(time) + chr(duty_cycle)
#
# if(self.check_conn()):
# self.radio.tx(dest_addr=self.dest_addr, data=data_out)
def test_hall(self):
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_HALL, [])
def test_batt(self):
data = ''.join(chr(0) for i in range(4))
self.radio.send(cmd.STATUS_UNUSED, cmd.TEST_BATT, data)
def get_sample_count(self):
self.radio.send(cmd.STATUS_UNUSED, cmd.GET_SAMPLE_COUNT, pack('H', 0))
def transmit_saved_data(self):
if(self.last_sample_count == 0):
self.get_sample_count()
time.sleep(0.5)
if(self.last_sample_count == 0):
print("There is no previously saved data.")
return
else:
self.data_cnt = 0
start_page = 0x200
print("Transmitting saved data...")
self.state_data = []
self.data_cnt = 0
self.radio.send(cmd.STATUS_UNUSED, cmd.TX_SAVED_DATA, pack('3H', start_page, self.last_sample_count, SAMPLE_BYTES))
def erase_mem_sector(self, sector):
print("Erasing memory sector " + str(sector))
self.radio.send(cmd.STATUS_UNUSED, cmd.ERASE_MEM_SECTOR, pack('H', sector))
def print_packet(self, packet):
'''
Description:
Print the contents of a packet to the screen. This function
will primarily be used for debugging purposes. May need to
replace print with stdio or stderr to accommodate GUI test
suite.
'''
if(packet is not None):
print("Received the following: ")
print("RSSI: " + str(ord(packet.get('rssi'))))
(src_h, src_l) = unpack('cc', packet.get('source_addr'))
print("Source Address: 0x%.2X 0x%.2X" % (ord(src_h),
ord(src_l)))
print("ID: " + (packet.get('id')))
print("Options: " + str(ord(packet.get('options'))))
rf_data = packet.get('rf_data')
print("Status Field: " + str(ord(rf_data[0])))
print("Type Field: " + str(ord(rf_data[1])))
print("Payload Data: " + ''.join([str(ord(i)) for i in rf_data[2:]]))
def save_trial_data(self, fname):
print('%d total state data were received.' % self.data_cnt)
state_data_arr = np.array(self.state_data)
print state_data_arr
fmt = '%d, %f, %f, %f, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d'
np.savetxt(fname, state_data_arr, fmt)
#np.savetxt(self.datestring() + ".csv", state_data_arr, fmt)
print("State data saved to file: " + fname)
def __del__(self):
'''
Description:
Clean up the connection when the object is deleted.
'''
try:
self.radio.close()
except AttributeError:
print('Caught Attribute Error')
pass