def __init__(self, GainIndex=0):
self.connect()
self.config = self.sensor.configU6()
self.gainindex = GainIndex
self.last_read = None
# define read commands
self.comm = {
'LJTemp': u6.AIN24(14),
'pH': u6.AIN24(2, ResolutionIndex=12, GainIndex=self.gainindex),
'Temp': u6.AIN24(0, ResolutionIndex=9, GainIndex=self.gainindex)
}
def __init__(self): self.device = u6.U6() print self.device.configU6() #for the labjack self.numChannels = 12 self.firstChannel = 2 self.resolutionIndex = 4 self.gainIndex = 0 self.settlingFactor = 0 self.differential = False self.latestAinValues = np.array([0]*self.numChannels, dtype='float') self.lastForceMoments = np.array([0]*self.numChannels, dtype='float') self.latestForceMoments = np.array([0]*self.numChannels, dtype='float') self.zero = np.array([0]*self.numChannels, dtype='float') self.lastTime = 0 self.latestTime = 0 self.lastLeftOn = False self.latestLeftOn = False self.lastLeftCOP = [0, 0] self.latestLeftCOP = [0, 0] self.lastRightOn = False self.latestRightOn = False self.lastRightCOP = [0, 0] self.latestRightCOP = [0, 0] FIOEIOAnalog = ( 2 ** self.numChannels ) - 1; fios = FIOEIOAnalog & (0xFF) eios = FIOEIOAnalog/256 self.device.getFeedback(u6.PortDirWrite(Direction = [0, 0, 0], WriteMask = [0, 0, 15])) self.feedbackArguments = [] self.feedbackArguments.append(u6.DAC0_8(Value = 125)) self.feedbackArguments.append(u6.PortStateRead()) for i in range(self.firstChannel, self.numChannels+self.firstChannel): self.feedbackArguments.append( u6.AIN24(i, self.resolutionIndex, self.gainIndex, self.settlingFactor, self.differential) ) self.task = viztask.schedule(self.__update) self.going = True self.history = [] self.recording = False #magic numbers to turn volts into Newtons and Newton.meters #left is stored in first 6 (x,y,z,mx,my,mz) and right in second 6 self.M = np.array([[0, -2.309900 , 0.000000 , 1.308000 , 0.000000 , 2.306800 , 0.000000 , -495.780000 , 0.000000 , -494.040000 , 0.000000 , -2.034400],\ [0, 6.308900 , 0.000000 , 5.913600 , 0.000000 , 11.633000 , 0.000000 , -2.295700 , 0.000000 , -13.079000 , 0.000000 , 499.990000],\ [0, -491.360000 , 0.000000 , -488.510000 , 0.000000 , -488.270000 , 0.000000 , 5.064000 , 0.000000 , 6.732400 , 0.000000 , 0.391790],\ [0, 48.162000 , 0.000000 , -298.930000 , 0.000000 , 299.410000 , 0.000000 , -1.565600 , 0.000000 , 4.956800 , 0.000000 , 54.786000],\ [0, -254.760000 , 0.000000 , -26.647000 , 0.000000 , -25.153000 , 0.000000 , 42.462000 , 0.000000 , 42.087000 , 0.000000 , 1.601900],\ [0, -7.032800 , 0.000000 , 1.345700 , 0.000000 , -1.140100 , 0.000000 , -243.640000 , 0.000000 , 354.550000 , 0.000000 , -4.020500],\ [-2.688400 , 0.000000 , -0.083334 , 0.000000 , 1.258500 , 0.000000 , 491.220000 , 0.000000 , -495.750000 , 0.000000 , -6.723600 , 0],\ [-7.642900 , 0.000000 , -5.959700 , 0.000000 , -3.659100 , 0.000000 , 15.847000 , 0.000000 , -12.401000 , 0.000000 , 507.580000 , 0],\ [-490.840000 , 0.000000 , -490.690000 , 0.000000 , -492.520000 , 0.000000 , -6.428000 , 0.000000 , 3.723600 , 0.000000 , 4.807000 , 0],\ [300.830000 , 0.000000 , -300.260000 , 0.000000 , 48.265000 , 0.000000 , -5.152500 , 0.000000 , -1.434500 , 0.000000 , 49.278000 , 0],\ [26.955000 , 0.000000 , 27.005000 , 0.000000 , 253.680000 , 0.000000 , -40.116000 , 0.000000 , 41.703000 , 0.000000 , -1.501700 , 0],\ [-1.380200 , 0.000000 , -2.450800 , 0.000000 , -1.349500 , 0.000000 , -348.250000 , 0.000000 , -245.680000 , 0.000000 , 10.366000 , 0]],\ dtype='float')
def voltage_sample(lj):
channel = 0
resolution_index = 4
gain_index = 0
settling_factor = 0
differential = False
cmd = u6.AIN24( channel,
resolution_index,
gain_index,
settling_factor,
differential)
slope = lj.calInfo.ain10vSlope
offset = lj.calInfo.ain10vOffset
# Following U6 data sheet directions for calculating AIN value
# All readings and the calibration constants are 16-bit aligned.
# This means that 24-bit values must be justified to 16-bit values before applying a calibration.
raw = float(lj.getFeedback(cmd)[0])/256.0
return (slope * raw) + offset
#Configure the IOs before the test starts
FIOEIOAnalog = (2**numChannels) - 1
fios = FIOEIOAnalog & (0xFF)
eios = FIOEIOAnalog / 256
d.getFeedback(u6.PortDirWrite(Direction=[0, 0, 0], WriteMask=[0, 0, 15]))
feedbackArguments = []
feedbackArguments.append(u6.DAC0_8(Value=125))
feedbackArguments.append(u6.PortStateRead())
for i in range(numChannels):
feedbackArguments.append(
u6.AIN24(i, resolutionIndex, gainIndex, settlingFactor,
differential))
start = datetime.now()
# Call Feedback 1000 times
i = 0
while i < numIterations:
results = d.getFeedback(feedbackArguments)
for j in range(numChannels):
latestAinValues[j] = d.binaryToCalibratedAnalogVoltage(
gainIndex, results[2 + j])
i += 1
end = datetime.now()
delta = end - start
print "Time difference: ", delta
try:
# Configure the IOs before the test starts
rospy.loginfo("Preparing LabJack U6 ...")
FIOEIOAnalog = (2**numChannels) - 1
fios = FIOEIOAnalog & 0xFF
eios = FIOEIOAnalog // 256
d.getFeedback(u6.PortDirWrite(Direction=[0, 0, 0], WriteMask=[0, 0, 15]))
feedbackArguments = []
feedbackArguments.append(u6.DAC0_8(Value=125))
feedbackArguments.append(u6.PortStateRead())
feedbackArguments.append(
u6.AIN24(0, resolutionIndex, gainIndex, settlingFactor, True))
feedbackArguments.append(
u6.AIN24(2, resolutionIndex, gainIndex, settlingFactor, True))
cycle = 0
calibrated = False
rospy.loginfo(
"Calibrating sensors using {} samples ...".format(calibrationTime))
while not rospy.is_shutdown():
results = d.getFeedback(feedbackArguments)
processed_values = []
for i in range(numChannels):
latest_values[i].append(
#print(len(sys.argv))
for r in xrange(2, len(sys.argv)):
#print(r)
Channel.append(int(sys.argv[r]))
Gain.append(0)
Resolution.append(12)
Settling.append(0)
feedbackArguments = []
print('Time, PT1-Pin {0}, PT2-pin {1}, PT3-Pin {2}, PT4-Pin {3}'.format(
sys.argv[2], sys.argv[3], sys.argv[4], sys.argv[5]))
for i in xrange(len(sys.argv) - 2):
feedbackArguments.append(
u6.AIN24(PositiveChannel=Channel[i],
ResolutionIndex=Resolution[i],
GainIndex=Gain[i],
SettlingFactor=Settling[i]))
#print feedbackArguments
for x in xrange(1, dataPoints):
time.sleep(.01)
ainBits = d.getFeedback(feedbackArguments)
print(time.clock(), end=',')
for e in xrange(0, len(sys.argv) - 2, 4):
print(d.binaryToCalibratedAnalogVoltage(0,
ainBits[e],
is16Bits=False,
resolutionIndex=0),
end=',')
print(d.binaryToCalibratedAnalogVoltage(0,
ainBits[e + 1],
is16Bits=False,
try:
# Configure the IOs before the test starts
rospy.loginfo("Preparing LabJack U6 ...")
FIOEIOAnalog = (2**numChannels) - 1
fios = FIOEIOAnalog & 0xFF
eios = FIOEIOAnalog // 256
d.getFeedback(u6.PortDirWrite(Direction=[0, 0, 0], WriteMask=[0, 0, 15]))
feedbackArguments = []
feedbackArguments.append(u6.DAC0_8(Value=125))
feedbackArguments.append(u6.PortStateRead())
# channel, resolutionIndex, gainIndex, settingFactor, differentialReadout?
feedbackArguments.append(u6.AIN24(2, 0, 0, 0, False))
feedbackArguments.append(
u6.AIN24(0, resolutionIndex, gainIndex, settlingFactor, True))
loop = 0
in_contact = False
did_trigger = False
startTime = time.time()
loop_diff = 0
rospy.loginfo("Starting ...")
while not rospy.is_shutdown():
startTime = datetime.now()
results = d.getFeedback(feedbackArguments)
