def __init__(self, sensorNum):
self.sensorNum = sensorNum
self.crc8_table = crc.calculate_CRC8_table()
self.crc32_table = crc.calculate_CRC32_table()
#Write commands from the user
# self.user_cmds = rospy.Publisher('user_commands' + str(sensorNum), ByteMsg, queue_size=1)
#Start or stop continuous data transfer
self.run_flag = rospy.Publisher('continuous_data_flag' +
str(sensorNum),
FlagMsg,
queue_size=1)
rospy.wait_for_service('poll' + str(sensorNum))
self.pollProxy = rospy.ServiceProxy('poll' + str(sensorNum),
SensorOutputRequest,
persistent=True)
rospy.wait_for_service('user_command' + str(sensorNum))
self.commandProxy = rospy.ServiceProxy('user_command' + str(sensorNum),
ByteSrv,
persistent=True)
self.inBuf = [False] * 6
self.adsGain = [1] * 6
self.adsRate = [30e3] * 6
self.vref = 2.5
self.OFC = [1] * 6
self.FSC = [1] * 6
for i in range(6):
o, f = self.ads_report_registers(i)
self.OFC[i] = o
self.FSC[i] = f
def __initOFS(self):
self.calMatrix = np.eye(6)
sensor = Sensor(self.calMatrix)
# test all modules are working
time.sleep(1)
sample = sensor.poll()
for cmmd in sample.sum:
if cmmd < 0.5:
cont = False
print('Sum Signal too low (' + str(cmmd) + 'V)')
sensor.disconnect()
self.rep_ids = {
0x1: 'Accelerometer',
0x2: 'Gyroscope',
0x4: 'Linear Acceleration',
0x5: 'Rotation Vector',
0x8: 'Game Rotation Vector'
}
self.crc8_table = crc.calculate_CRC8_table()
self.crc32_table = crc.calculate_CRC32_table()
def __init__(self, portNum=2):
#Now set up the real ports
self.port = ftdi.Device('USB-COM485 Plus2', interface_select=portNum)
self.port.ftdi_fn.ftdi_set_latency_timer(1)
# self.port.setTimeouts(3,3)
# self.port.setUSBParameters(64)
self.port.baudrate = 5000000
self.reset()
self.port.ftdi_fn.ftdi_usb_reset()
print(self.port)
self.num_errors = 0
self.send_flag = False
self.counter = 0
self.counter2 = 0
self.commands = {
0x12: self.sendPackage, #Data request
0x10: self.setSendFlag, #Start data transfer
0x11: self.setSendFlag, #Stop data transfer
0xE1: self.deac_transducer,
0xE2: self.deac_transducer,
0xE3: self.deac_transducer,
0xE4: self.deac_transducer,
0xE5: self.deac_transducer,
0xE6: self.deac_transducer,
0xF0: self.reset, #Full sensor reset
0xF1: self.reset_transducer,
0xF2: self.reset_transducer,
0xF3: self.reset_transducer,
0xF4: self.reset_transducer,
0xF5: self.reset_transducer,
0xF6: self.reset_transducer,
0xFA: self.reset_imu,
0xFB: self.reset_dac
}
self.crc8_table = crc.calculate_CRC8_table()
self.crc32_table = crc.calculate_CRC32_table()
def __init__(self, path, baud=3000000, portNum=1, rate=1500):
self.num_errors = 0
self.crc8_table = crc.calculate_CRC8_table()
self.crc32_table = crc.calculate_CRC32_table()
self.garbage_response = {
'differential': [0, 0, 0, 0, 0, 0],
'differential_raw': [0, 0, 0, 0, 0, 0],
'sum': [0, 0, 0, 0, 0, 0],
'sum_raw': [0, 0, 0, 0, 0, 0],
'imu': [0, 0, 0, 0],
'quaternion': [0, 0, 0, 0],
'saturated': [0, 0, 0, 0, 0, 0],
'temperature': 0,
'report_id': 'NA'
}
self.baud = baud
self.conti_read_flag = False
self.report_ids = {
0x01: 'Accelerometer',
0x02: 'Gyroscope',
0x04: 'Linear Acceleration',
0x05: 'Rotation Vector',
0x08: 'Game Rotation Vector'
}
#Find calibration matrix
self.calMatrix = np.eye(6)
self.filename = path + '/src/calibration.cal'
self.get_cal_matrix()
###Initialize serial port
try:
self.port = ftdi.Device('USB-COM485 Plus2',
interface_select=portNum)
except ftdi.FtdiError:
rospy.logerr('Failed to open port. Quitting...')
sys.exit(0)
self.port.ftdi_fn.ftdi_set_latency_timer(1)
self.port.ftdi_fn.ftdi_set_line_property(8, 1, 0)
self.port.baudrate = self.baud
self.portNum = portNum
rospy.loginfo('Opened port ' + str(self.portNum))
self.reset() #reset transmit and receive buffers
self.ab = {
2.5: (0x5DC000, 2.7304),
5: (0x5DC000, 2.7304),
10: (0x5DC000, 2.7304),
15: (0x3E8000, 1.8202),
25: (0x4B0000, 2.1843),
30: (0x3E8000, 1.8202),
50: (0x4B0000, 2.1843),
60: (0x3E8000, 1.8202),
100: (0x4B0000, 2.1843),
500: (0x3C0000, 1.7474),
1000: (0x3C0000, 1.7474),
2000: (0x3C0000, 1.7474),
3750: (0x400000, 1.8639),
7500: (0x400000, 1.8639),
15000: (0x400000, 1.8639),
30000: (0x400000, 1.8639)
}
self.inBuf = [False] * 6
self.adsGain = [1] * 6
self.adsRate = [30e3] * 6
self.vref = 2.5
self.OFC = [1] * 6
self.FSC = [1] * 6
###Set up ROS
self.sampleRate = rate
self.rate = rospy.Rate(self.sampleRate)
#queue_size limits the number of queued messages if a subscriber is reading too slowly
#Create publisher to write continuous data to topic using custom message type
self.force_data_pub = rospy.Publisher('force_data' + str(portNum),
SensorOutput,
queue_size=1)
#Listen to user commands and interrupt action to carry out the command
self.user_cmds = rospy.Service('user_command' + str(portNum), ByteSrv,
self.send_byte)
#Listen to user commands to dstart or stop continuous data transfer
self.run_flag = rospy.Subscriber('continuous_data_flag' + str(portNum),
FlagMsg, self.changeFlag)
#Service for reading and parsing one measurement
self.measure_srv = rospy.Service('poll' + str(portNum),
SensorOutputRequest, self.poll)
#Publisher for IMU Calibration
self.imu_cal_pub = rospy.Publisher('imu_calibration' + str(portNum),
KeyValue,
queue_size=1)
#Let all the topics start properly
rospy.sleep(1)
self.times = []
count = 0
##Run loop - do this forever in the background
while not rospy.is_shutdown():
self.rate.sleep()
if self.conti_read_flag == 1: #Continuous data transfer
#Wait for initialization bit and matching crc-4
if self.wait_for_packet(verbose=False):
parsed = self.found_init_byte()
try:
if parsed != None:
self.force_data_pub.publish(parsed)
count += 1
else:
rospy.logwarn('Error parsing data')
except rospy.ROSSerializationException:
rospy.logwarn(
"Error publishing. Serialization Exception caught. Continuing..."
)
elif self.conti_read_flag == 0 and count != 0: #Data transmission has just been stopped
rospy.loginfo('Stopping Continuous Data Transfer...')
self.port.write(
b'\x11\x00\x00\xC9'
) #11 is the byte command, C9 is the matching crc8
self.purge()
rospy.sleep(0.25)
timeout = 0
count = 0
while self.readBytes(1) != []:
self.port.write(b'\x11\x00\x00\xC9')
self.purge()
rospy.sleep(0.25)
timeout += 1
if timeout == 100:
rospy.logwarn(
'Failed to stop transmission. Please try again')
break
elif self.conti_read_flag == 2: #Calibrate IMU
self.calibrate_imu
self.port.close()
self.measure_srv.shutdown('Finished')
self.user_cmds.shutdown('Finished')
return num
atiSensor = ati.Ati('/home/david/Documents/forcesensor', 'FT24093', 1)
timestamp = makeTimestamp()
calMatrix = np.eye(6)
count = 0
outer_timeout = N_SAMPLES * 10
rep_ids = {
0x1: 'Accelerometer',
0x2: 'Gyroscope',
0x4: 'Linear Acceleration',
0x5: 'Rotation Vector',
0x8: 'Game Rotation Vector'
}
crc8_table = crc.calculate_CRC8_table()
crc32_table = crc.calculate_CRC32_table()
diff = []
sums = []
atiData = []
temp = []
pos2 = []
pos3 = []
pos4 = []
pos5 = []
jawPos = []
jawEffort = []
sensor = Sensor(calMatrix)
# test all modules are working
def __init__(self, calMatrix, portNum=2, quick=False):
"""
Opens a connection to a serial device and creates a serial object with which to control it
:param portNum Index of port of device to open. Default value is 1, but 2 is also common
"""
#Dictionary of report IDs
self.rep_ids = {
1: 'Accelerometer',
2: 'Gyroscope',
4: 'Linear Acceleration',
5: 'Rotation Vector',
8: 'Game Rotation Vector'
}
self.ab = {
2.5: (0x5DC000, 2.7304),
5: (0x5DC000, 2.7304),
10: (0x5DC000, 2.7304),
15: (0x3E8000, 1.8202),
25: (0x4B0000, 2.1843),
30: (0x3E8000, 1.8202),
50: (0x4B0000, 2.1843),
60: (0x3E8000, 1.8202),
100: (0x4B0000, 2.1843),
500: (0x3C0000, 1.7474),
1000: (0x3C0000, 1.7474),
2000: (0x3C0000, 1.7474),
3750: (0x400000, 1.8639),
7500: (0x400000, 1.8639),
15000: (0x400000, 1.8639),
30000: (0x400000, 1.8639)
}
self.crc8_table = crc.calculate_CRC8_table()
self.crc32_table = crc.calculate_CRC32_table()
self.calMatrix = calMatrix
self.num_errors = 0
self.port = ftdi.Device('USB-COM485 Plus2', interface_select=portNum)
self.port.ftdi_fn.ftdi_set_latency_timer(1)
self.port.ftdi_fn.ftdi_set_line_property(8, 1, 0)
self.port.baudrate = 3000000
self.threadLock = threading.Lock()
self.thread = read_thread(parent=self,
port=self.port,
threadLock=self.threadLock)
self.block = False
self.inBuf = [False] * 6
self.data_rate = 1500
self.adsGain = [1] * 6
self.adsRate = [30e3] * 6
self.vref = 2.5
self.OFC = [1] * 6
self.FSC = [1] * 6
if not quick:
for i in range(6):
o, f = self.ads_report_registers(i)
self.OFC[i] = o
self.FSC[i] = f
