def __init__(self):
self.m = motetalk.motetalk(self.fmtstr, self.header, "COM4", debug=False,timeout=0.5)
startup(self.m)
errwrite( "Sniffing...\n")
#print "ts " + header
try:
(arr, t, crc) = self.m.nextline()
(arr, t, crc) = self.m.nextline()
(arr, t, crc) = self.m.nextline()
except:
pass
def sniff(header, format, verbose, graph):
m = motetalk.motetalk(format, header, "/dev/tty.usbmodem1431", debug=False)
startup(m)
sys.stderr.write("Starting up OSC...\n") if verbose else ""
client = OSC.OSCClient() if graph else ""
client.connect(("127.0.0.1", 8000)) if graph else "" # note that the argument is a tupple and not two arguments
sys.stderr.write("Sniffing...\n") if verbose else ""
print "ts " + header
try:
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
except:
sys.stderr.write("oops") if verbose else ""
pass
return client, m
fmtstr = '! H b b b H H H H H H h h h H H H H h h h B H B B'
# header = "addr len cmd type n mx my mz lx ly lz ti gx gy gz cm ct ca ce cp i0 i1 i2 i3 i4 i5 i6 i7 i8 i9 ia ib ic mychk chk lqi rssi"
# fmtstr = '! H b b b H h h h H H H H h h h H H H H H B B B B B B B B B B B B B B H B B'
header = "addr len cmd type n sx sy z1 z3 ta mx my mz lx ly lz ti gx gy gz cm ct ca ce cp mychk chk lqi rssi"
fmtstr = '! H b b b H H H H H H h h h H H H H h h h h h h h h B H B B'
#Joey's Header
header = "addr len cmd type n sx sy z1 z3 ta lx ly lz ti gx gy gz Address"
fmtstr = '! H b b b H H H H H H H H H H h h h H'
#Joey's Header
header = "addr len cmd type n sx sy z1 z3 ta lx ly lz ti gx gy gz mx my mz Address"
fmtstr = '! H b b b H H H H H H H H H H h h h h h h H'
m1 = motetalk.motetalk(fmtstr, header, com1, debug=False, timeout = .5)
startup(m1,chan1) #Channel 15
sys.stderr.write( "Sniffing...\n")
print "ts " + header + " Time"
try:
(arr, t, crc) = m1.nextline()
sys.stderr.write(com1)
(arr, t, crc) = m1.nextline()
sys.stderr.write(com1)
(arr, t, crc) = m1.nextline()
sys.stderr.write(com1)
except:
sys.stderr.write("oops")
# f.write(str("Header: " + tool + " " + act + " " + rep + " " + trl + " ") + "\n")
print "Filename testVideo-" + str(testnum) + ".txt"
f = open("testVideo-" + str(testnum) + ".txt", "w")
f.write(str("timestamp, accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ") + "\n")
done = 0
n = 0
oldarr = []
header = "addr len cmd type n sx sy z1 z3 ta lx ly lz ti gx gy gz mx my mz Address"
fmtstr = '! H b b b H H H H H H H H H H h h h h h h H'
m = motetalk.motetalk(fmtstr, header, "/dev/tty.usbmodem1431", debug=False)
startup(m)
sys.stderr.write( "Sniffing...\n")
print "ts " + header
try:
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
except:
sys.stderr.write("oops")
pass
while not done:
diff = a-lin_acc
return tuple(diff), tuple(lin_acc)
if __name__=="__main__":
#timestamp
t0 = time.time()
#initialize motetalk/serial port stuff
sport = "/dev/tty.usbmodemfa131" #raw_input('port \n')
chan = "15" #raw_input('Channel to sniff \n')
#filename = "data"
#filename = raw_input('Log file to write to \n')
#log = open(filename + ".txt", 'w')
m = motetalk.motetalk(sport=sport,brate=115200)
startup(m)
sys.stderr.write( "Sniffing...\n")
try:
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
except:
pass
frames = 50
print "lets get crackin"
fig, ax, lines = preparePlots(frames)
plt.show()
sys.stderr.write(repr(sys.exc_info()))
sys.stderr.write("\nQuitting... \n")
return -1, None
num_good = 0
num_bad = 0
num_skip = 0
done = 0
n = 0
oldarr = []
header = "addr len cmd type n sx sy z1 z3 ta mx my mz lx ly lz ti gx gy gz cm ct ca ce cp mychk chk lqi rssi"
fmtstr = '! H b b b H H H H H H h h h H H H H h h h h h h h h B H B B'
m = motetalk.motetalk(fmtstr, header, "/dev/ttyACM0", debug=False)
startup(m)
sys.stderr.write( "Sniffing...\n")
print "ts " + header
try:
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
except:
sys.stderr.write("oops")
pass
decimator = 0
while not done:
def pid():
global error_term, average_value, measurement, integral, const_integral, period, differential, const_differential, old_error_term, proportional, const_proportional, motor_output
error_term = reference - average_value
integral = integral + error_term * const_integral * period
differential = const_differential * (error_term - old_error_term) / period
proportional = error_term * const_proportional
old_error = error_term
motor_output = integral + differential + proportional
if motor_output>255:
motor_output = 255
elif motor_output<190:
motor_output = 190
#YK's PID controller ends
m = motetalk.motetalk(openwsnfmtstr, openwsnheader, "COM10")
startup(m)
#yk = open("yk.txt", "w")
#ser.write("p")
sys.stderr.write( "Sniffing...\n")
print "ts " + openwsnheader
# put the stuff here
#try:
#(arr, t, crc) = m.nextline()
#(arr, t, crc) = m.nextline()
#(arr, t, crc) = m.nextline()
#except:
#pass
oldvalue = (0, 0, 0) #stores the value to send if there is an error in transmission
tempvalue = (0, 0, 0) #stores the value to send to motor right after pid calculation
for port in availables:
print ' ', index, port
index += 1
try:
num = int(raw_input('Enter the number:'))
if num > 0 and num <= len(availables):
selected = availables[num-1]
except Exception as e:
print "invalid input!"
exit(1)
header = "addr len cmd type n sx sy z1 z3 ta lx ly lz ti gx gy gz mx my mz Address"
fmtstr = "! H b b b H H H H H H H H H H h h h h h h H"
m = motetalk.motetalk(fmtstr, header, selected, debug=False)
startup(m)
sys.stderr.write( "Starting up OSC...\n")
client = OSC.OSCClient()
client.connect( ('127.0.0.1', 8000) ) # note that the argument is a tupple and not two arguments
sys.stderr.write( "Sniffing...\n")
print "ts " + header
try:
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
(arr, t, crc) = m.nextline()
except: