def InitAPI(self,rate):
self.rate = rate
print("Get API")
self.api = GloveAPI()
print("Init HW")
self.api.initHardware()
print("Get Baud")
self.api.getBaud()
self.api.rate(rate)
for x in range(0,3):
val = int(rdvals[x*2+1],16) << 8 + int(rdvals[x*2],16)
vals.append(val)
return vals
else:
return None
def Rd(self,addr,n=1):
rdvals = self.api.i2crd(self.port,self.ID,addr,n)
return rdvals
def Wr(self,addr,data):
self.api.i2cwr(self.port,self.ID,addr,data)
if __name__ == "__main__":
api = GloveAPI()
api.initHardware()
imu1 = IMU(api,1,0xD0,0x32)
#imu2 = IMU(api,2,0xD2,0x30)
imu1.Configure()
#imu2.Configure()
#x = imu1.imu.Status()
while (1):
try:
v = imu1.Read()
if len(v) > 5:
print ("Temp:{0}".format(v[0]))
type="string",
default="arr",
help="Name to use for the matlab data")
parser.add_option("-v", action="store_false", dest="verbose")
parser.add_option("-s", action="store_true", dest="stop")
(options, args) = parser.parse_args()
if options.outfile == "":
options.outfile = os.path.join(options.path, options.dataname + ".mat")
#outfile = AutoIncrementFile(options.outfile)
outfile = options.outfile
print("Get API")
api = GloveAPI()
print("Init HW")
api.initHardware()
print("Get Baud")
api.getBaud()
if options.stop:
api.streamstop()
sys.exit(0)
numToRead = options.count
if options.outfile:
writeFile = True
else:
writeFile = False
def __init__(self,parent=None):
super(IMU_Display, self).__init__(parent)
self._running = False
self.ser = None
self.tval = 0
self.setupUi(self)
self.api = GloveAPI()
self.api.initHardware()
self.imumgr = IMUManager(self.api)
self.imumgr.Configure()
self.pbStartStop.setText("Start")
self.connect(self.pbStartStop, SIGNAL("clicked()"),
self.StartStop)
self.comboIMU.addItem('Hand')
self.comboIMU.addItem('Pinkie')
self.comboIMU.addItem('Ring')
self.comboIMU.addItem('Middle')
self.comboIMU.addItem('Index')
self.comboIMU.addItem('Thumb')
self.connect(self.comboIMU,SIGNAL("currentIndexChanged(int)"), self.IMUChanged)
self.IMUIndex = self.comboIMU.currentIndex()
self.intDataSelect = 0 # 0 = Gyros, 1 = Accelerometers
self.connect(self.btnSelect,SIGNAL("clicked()"), self.DataSelect)
self.btnSelect.setText('Gyro')
self.size = 1000
self.step = 0.020 # 5ms.
#self.t = arange(0.0,self.size*self.step,self.step)
#self.x = zeros(self.size,Float)
#self.y = zeros(self.size,Float)
#self.z = zeros(self.size,Float)
self.t = np.arange(0,self.size*self.step,self.step)
self.x = np.zeros(self.size)
self.y = np.zeros(self.size)
self.z = np.zeros(self.size)
self.nPos = 0
self.pxAccel = Qwt.QwtPlot()
self.vertPlots.addWidget(self.pxAccel)
self.pyAccel = Qwt.QwtPlot()
self.vertPlots.addWidget(self.pyAccel)
self.pzAccel = Qwt.QwtPlot()
self.vertPlots.addWidget(self.pzAccel)
self.pxAccel.setTitle("X Gyroscope")
self.pyAccel.setTitle("Y Gyroscope")
self.pzAccel.setTitle("Z Gyroscope")
self.curveX = Qwt.QwtPlotCurve("X Accel")
#self.curveX.setStyle(Qwt.QwtPlotCurve.Sticks)
self.curveX.attach(self.pxAccel)
self.curveY = Qwt.QwtPlotCurve("Y Accel")
self.curveY.attach(self.pyAccel)
self.curveZ = Qwt.QwtPlotCurve("Z Accel")
self.curveZ.attach(self.pzAccel)
self.curveX.setPen(Qt.QPen(Qt.Qt.red))
self.curveY.setPen(Qt.QPen(Qt.Qt.blue))
self.curveZ.setPen(Qt.QPen(Qt.Qt.green))
self.lock = None
self.data = []
self.worker = None
class IMU_Display(QMainWindow,
ui_imu_display.Ui_IMU_Display):
def __init__(self,parent=None):
super(IMU_Display, self).__init__(parent)
self._running = False
self.ser = None
self.tval = 0
self.setupUi(self)
self.api = GloveAPI()
self.api.initHardware()
self.imumgr = IMUManager(self.api)
self.imumgr.Configure()
self.pbStartStop.setText("Start")
self.connect(self.pbStartStop, SIGNAL("clicked()"),
self.StartStop)
self.comboIMU.addItem('Hand')
self.comboIMU.addItem('Pinkie')
self.comboIMU.addItem('Ring')
self.comboIMU.addItem('Middle')
self.comboIMU.addItem('Index')
self.comboIMU.addItem('Thumb')
self.connect(self.comboIMU,SIGNAL("currentIndexChanged(int)"), self.IMUChanged)
self.IMUIndex = self.comboIMU.currentIndex()
self.intDataSelect = 0 # 0 = Gyros, 1 = Accelerometers
self.connect(self.btnSelect,SIGNAL("clicked()"), self.DataSelect)
self.btnSelect.setText('Gyro')
self.size = 1000
self.step = 0.020 # 5ms.
#self.t = arange(0.0,self.size*self.step,self.step)
#self.x = zeros(self.size,Float)
#self.y = zeros(self.size,Float)
#self.z = zeros(self.size,Float)
self.t = np.arange(0,self.size*self.step,self.step)
self.x = np.zeros(self.size)
self.y = np.zeros(self.size)
self.z = np.zeros(self.size)
self.nPos = 0
self.pxAccel = Qwt.QwtPlot()
self.vertPlots.addWidget(self.pxAccel)
self.pyAccel = Qwt.QwtPlot()
self.vertPlots.addWidget(self.pyAccel)
self.pzAccel = Qwt.QwtPlot()
self.vertPlots.addWidget(self.pzAccel)
self.pxAccel.setTitle("X Gyroscope")
self.pyAccel.setTitle("Y Gyroscope")
self.pzAccel.setTitle("Z Gyroscope")
self.curveX = Qwt.QwtPlotCurve("X Accel")
#self.curveX.setStyle(Qwt.QwtPlotCurve.Sticks)
self.curveX.attach(self.pxAccel)
self.curveY = Qwt.QwtPlotCurve("Y Accel")
self.curveY.attach(self.pyAccel)
self.curveZ = Qwt.QwtPlotCurve("Z Accel")
self.curveZ.attach(self.pzAccel)
self.curveX.setPen(Qt.QPen(Qt.Qt.red))
self.curveY.setPen(Qt.QPen(Qt.Qt.blue))
self.curveZ.setPen(Qt.QPen(Qt.Qt.green))
self.lock = None
self.data = []
self.worker = None
def IMUChanged(self,int):
self.IMUIndex = self.comboIMU.currentIndex()
def DataSelect(self):
if self.intDataSelect == 0:
self.intDataSelect = 1
self.btnSelect.setText("Acc")
self.pxAccel.setTitle("X Accelerometer")
self.pyAccel.setTitle("Y Accelerometer")
self.pzAccel.setTitle("Z Accelerometer")
else:
self.intDataSelect = 0
self.btnSelect.setText("Gyro")
self.pxAccel.setTitle("X Gyroscope")
self.pyAccel.setTitle("Y Gyroscope")
self.pzAccel.setTitle("Z Gyroscope")
def UpdateData(self,vals):
if self.nPos < self.size:
'''
Insert more data.. the initial data is all zeros
'''
self.x[self.nPos] = vals[0]
self.y[self.nPos] = vals[1]
self.z[self.nPos] = vals[2]
self.nPos = self.nPos + 1
else:
''' Shift the data down '''
#self.t = np.concatenate((self.t[1:],[self.t[-1]+self.step]))
self.x = np.concatenate((self.x[1:],[vals[0]]))
self.y = np.concatenate((self.y[1:],[vals[1]]))
self.z = np.concatenate((self.z[1:],[vals[2]]))
self.curveX.setData(self.t, self.x)
self.curveY.setData(self.t, self.y)
self.curveZ.setData(self.t, self.z)
self.pxAccel.replot()
self.pyAccel.replot()
self.pzAccel.replot()
def AddData(self,v=[]):
self.lock.acquire()
self.data.append(v)
self.lock.release()
def timerEvent(self, e):
if self._running == False:
return
self.lock.acquire()
ldata = self.data
self.data = []
self.lock.release()
#print("Updating with %d values" % len(ldata))
if len(ldata) > 0:
#self.StartStop()
try:
for v in ldata:
self.UpdateData(np.array(v))
except:
print ("Error...")
#self.StartStop()
def StartStop(self):
if self._running:
#self.api.streamOn(False)
self.pbStartStop.setText("Start")
self._running = False
''' Signal the worker thread to stop, then wait for it '''
print("Telling worker to stop..")
self.worker.stopme = True
self.worker.join()
print("Worker done")
self.worker = None
self.killTimer(self.timerid)
else:
self._running = True
self.pbStartStop.setText("Stop")
#self.api.streamOn(True)
self.lock = Lock()
'''
The speed here is not critical, since we can process multiple
values for each timer even. The timer locks the data, grabs all
of the values present, then unlocks it.. running this loop faster
would probably only serve to increase the overhead. It might make
the viewer smoother, but I doubt it, since a 25ms update rate is
faster than we can really discern anyway, assume we can discern a
30Hz update rate...
'''
self.timerid = self.startTimer(25)
self.worker = DataWorker(self.api,self,self.step)
self.worker.start()
class GloveChar(object):
def __init__(self):
super(GloveChar,self).__init__()
self.api = None
self.rate = 100
def printStatus(self,s):
sys.stdout.write("\b"*5 + "%5s" % s)
def InitAPI(self,rate):
self.rate = rate
print("Get API")
self.api = GloveAPI()
print("Init HW")
self.api.initHardware()
print("Get Baud")
self.api.getBaud()
self.api.rate(rate)
def showPacket(self,id,imudata):
s = "\t".join([str(x) for x in imudata.ravel()])
print("Id:%d %s" % (id,s))
def initSequence(self):
self.api.clearIMUPacketEngine()
self.api.streamstart(True)
self.api.StreamWD()
def endSequence(self):
self.api.streamstop()
def contiueSequence(self,count):
"""
This assumes the sequence has already been started.
"""
data = np.zeros((count,1+6*6))
self.api.StreamWD()
print "Percent Complete:",
for x in range(0,count):
self.api.StreamWD()
p = self.api.getIMUPacket()
'''
Reset the watchdog each time. No reason not to since the serial link to the
unit is not used for anything except this.
'''
self.api.StreamWD()
if p:
'''
Append the data to my potential Matlab array
'''
imudata = p.IMUData()
if not imudata == None:
#self.showPacket(p.pkID, imudata)
imuidx = 0
data[x][0] = p.pkID
data[x][1:] = imudata.ravel()
if x and x % 100 == 0:
sys.stdout.write(".")
sys.stdout.flush()
else:
print ("IMU Data returned")
else:
print "Bad Packet"
print("Done")
return data
def captureSequence(self,count):
self.initSequence()
data = self.contiueSequence(count)
self.endSequence()
return data
def logData(self,data,destDir,filePrefix,nRunIndex,rate):
fname = "%s%02d.mat" % (filePrefix,nRunIndex)
outfile = os.path.join(destDir,fname)
if os.path.isfile(outfile):
print ("Destination file exists")
os.unlink(outfile)
print "Saving data to %s" % outfile
sp.io.savemat(outfile, mdict={
'Data': data,
'Name':"CharRun%d" % nRunIndex,
'T':1.0/float(rate)})
def runLoop(self,runDelay,nLoops,nStart,nPerRun,destDir,filePrefix):
for nRunIndex in range(nStart,nLoops+1):
data = self.captureSequence(nPerRun)
if data == None:
# No data returned!!
return
"""
Good data returned. Log it to a file and then
delay for the next sequence.
"""
self.logData(data,destDir,
filePrefix,
nRunIndex,
self.rate)
if runDelay:
self.Delay(runDelay)
def Delay(self,dly):
"""
Delay time in minutes. Wake up every 5 minutes and print something.
"""
totalDelay = 0
while totalDelay < dly:
currDelay = min(5,dly - totalDelay)
totalDelay = totalDelay + currDelay
print("Delay time left:%d" % (dly - totalDelay))
time.sleep(currDelay*60)
print("Delay Done")
def run(self):
'''
Do the hard work..
'''
print("worker started..")
api = GloveAPI()
while not api.openPort():
print("Open port failed.. retry")
print("Clear Packet Engine")
api.clearIMUPacketEngine()
print("Set Rate")
api.rate(self.rate)
print("Start Stream")
api.streamstart(True)
print("Set WD")
api.StreamWD()
while self.stopme == False:
packet = api.getIMUPacket()
if packet:
data = packet.MeasuredData()
self.dataObj.newPacket(packet.numIMUs,packet.pkID, data)
api.StreamWD()
else:
'''
A Timeout occured, see if I can recover
'''
print("Empty data returned")
api.streamstart(True)
api.StreamWD()
print("Worker asked to quit")