timeold = time.time()

while not endflag.isSet():

global rsci

global lsci

global R

global R2

global histzR

global histzL

global Rhsp

global Lhsp

global Rtop

global Ltop

global RCOUNT

global LCOUNT

global catchflag

global steplengthL

global steplengthR

global Rgorb

global Lgorb

global stridecounter

global psudoL

global psudoR

global RHS

global LHS

#get some data

root = q.get()

#find the data we need from the frame packet

lp1 = root.find(".//Forceplate_0/Subframe_0/F_z")#Left Treadmill

rp1 = root.find(".//Forceplate_1/Subframe_0/F_z")#Right Treadmill

s0rhipy = root.find(".//Subject0/RHIP/Y")

s0lhipy = root.find(".//Subject0/LHIP/Y")

s0ranky = root.find(".//Subject0/RANK/Y")

s0lanky = root.find(".//Subject0/LANK/Y")

temp = rp1.attrib.values()

Rz = float(temp[0])#cast forceplate data as float

temp1 = lp1.attrib.values()

Lz = float(temp1[0])

temp2 = s0rhipy.attrib.values()

RHIPY = float(temp2[0])/1000 #convert to meters

temp3 = s0lhipy.attrib.values()

LHIPY = float(temp3[0])/1000

temp4 = s0ranky.attrib.values()

RANKY = float(temp4[0])/1000

temp5 = s0lanky.attrib.values()

LANKY = float(temp5[0])/1000

cursorR.setScale(0.1,rsci*(RHIPY-RANKY),0.01250)

cursorL.setScale(-0.1,lsci*(LHIPY-LANKY),0.01250)

#determine if we need to hide the cursor

if (RHIPY-RANKY < 0) | (Rz < -30):

cursorR.visible(0)

else:

cursorR.visible(1)

if (LHIPY-LANKY < 0) | (Lz < -30):

cursorL.visible(0)

else:

cursorL.visible(1)

#detect gait events

if (Rz < -30) & (histzR >-30): #RHS

HistBallR.setPosition([0.2,rsci*(RHIPY-RANKY), 0])

steplengthR = rsci*(RHIPY-RANKY)

stridecounter = stridecounter+1

RHS = 1

if (psudoR == 5):

Rhsp = RHIPY-RANKY#update the alpha value

rsci = 0.5*(1/(Rhsp+1/(1+R)*(abs(Rtop)-R*Rhsp)))

# print("rsci is: ")

# print(rsci)

if (abs(steplengthR-targetR) <= targettol):

RCOUNT = RCOUNT+1

rightcounter.message(str(RCOUNT))

Rgorb = 1 #flag this step as good or bad

else:

Rgorb = 0

if (catchflag == 1) & (stridecounter > 12):

boxR.setPosition([0.2,targetR,0])

elif (catchflag == 1) & (stridecounter <= 12):

rsci = 2

else:

boxR.setPosition([0.2,targetR,0])

elif (Rz > -30) & (histzR < -30): #RTO

#calculate Toe-Off position

if (psudoR == 5):

Rtop = RHIPY-RANKY#update beta value

psudoR = 1

psudoR = psudoR+1

RHS = 0

else:

RHS = 0

if (abs(steplengthR-targetR) <= targettol):#highlight the target when the target is hit within tolerance

boxR.color( viz.WHITE )

else:

boxR.color( viz.BLUE )

if (Lz < -30) & (histzL >-30): #LHS

HistBallL.setPosition([-0.2,lsci*(LHIPY-LANKY), 0])

steplengthL = lsci*(LHIPY-LANKY)

stridecounter = stridecounter+1

LHS = 1

if (psudoL == 5):

Lhsp = LHIPY-LANKY#update the alpha value

lsci = 0.5*(1/(Lhsp+1/(1+R2)*(abs(Ltop)-R2*Lhsp)))

# print("lsci is: ")

# print(lsci)

if (abs(steplengthL-targetL) <= targettol):

LCOUNT = LCOUNT+1

leftcounter.message(str(LCOUNT))

Lgorb = 1 #flag this step as good or bad

else:

Lgorb = 0

if (catchflag == 1) & (stridecounter > 12):

boxL.setPosition([-0.2,targetL,0])

elif (catchflag == 1) & (stridecounter <= 12):

lsci = 2

else:

boxL.setPosition([-0.2,targetL,0])

elif (Lz > -30) & (histzL < -30): #LTO

#calculate Toe-Off position

if (psudoL == 5):

Ltop = LHIPY-LANKY#update beta value

psudoL = 1

psudoL = psudoL+1

LHS = 0

else:

LHS = 0

if (abs(steplengthL-targetL) <= targettol):#highlight the target when the target is hit within tolerance

boxL.color( viz.WHITE )

else:

boxL.color( viz.BLUE )

histzR = Rz

histzL = Lz

#send some data to be saved

fn = root.find(".//FrameNumber")#find the frame number

fnn = fn.attrib.values()

# print(fnn[0])

savestring = [int(fnn[0]),Rz,Lz,RHS,LHS,Rgorb,Lgorb,RHIPY-RANKY,LHIPY-LANKY,rsci,lsci,RHIPY,LHIPY,RANKY,LANKY]#organize the data to be written to file

# print(sys.getsizeof(savestring))

q3.put(savestring)

# q3.join()

#illegal characters to remove from string later before going to xml

RE_XML_ILLEGAL = u'([\u0000-\u0008\u000b-\u000c\u000e-\u001f\ufffe-\uffff])' + \

u'|' + \

u'([%s-%s][^%s-%s])|([^%s-%s][%s-%s])|([%s-%s]$)|(^[%s-%s])' % \

(unichr(0xd800),unichr(0xdbff),unichr(0xdc00),unichr(0xdfff),

unichr(0xd800),unichr(0xdbff),unichr(0xdc00),unichr(0xdfff),

unichr(0xd800),unichr(0xdbff),unichr(0xdc00),unichr(0xdfff))

HOST = 'localhost'#IP address of CPP server

PORT = 50008

s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

print 'Socket created'

print 'Socket now connecting'

s.connect((HOST,PORT))

s.send('1')#send initial request for data

while not endflag.isSet():

data = s.recv(50)#receive the initial message

data3 = data[:3]#get first 3 letters

if (data3 == "New"):

nextsizestring = data[3:]#get the integer after "New"

nextsizestring2 = nextsizestring.rstrip('\0')#format

nextsize = int(nextsizestring2,10)#cast as type int

# print("Next Packet is size: ")

# print(nextsize)

s.send('b')#tell cpp server we are ready for the packet

databuf = ''#initialize a buffer

while (sys.getsizeof(databuf) < nextsize+21):

data = s.recv(nextsize)#data buffer as a python string

databuf = databuf + data#collect data into buffer until size is matched

# print(sys.getsizeof(databuf))

root = ElementTree.ElementTree(ElementTree.fromstring(databuf))#create the element tree

q.put(root)#place the etree into the threading queue

elif (data3 != "New"):

print("WARNING! TCP SYNCH HAS FAILED")

break

if not data: break

s.send('b')

#initialize the file

mst = time.time()

mst2 = int(round(mst))

mststring = str(mst2)+'STATTARGsteplength.txt'

print("Data file created named:")

print(mststring)

file = open(mststring,'w+')

json.dump(['FrameNumber','Rfz','Lfz','RHS','LHS','RGORB','LGORB','Ralpha','Lalpha','Rscale','Lscale','RHIPY','LHIPY','RANKY','LANKY'],file)

file.close()

file = open(mststring,'a')#reopen for appending only

while not endflag.isSet():

savestring = q3.get()#look in the queue for data to write

if savestring is None:

continue

else:

json.dump(savestring, file)

print("savedata stop flag raised, finishing...")

while 1:

try:

savestring = q3.get(False,2)

except:

savestring = 'g'

# print(savestring)

if savestring == 'g':

break

print("data finished write to file")

else:

json.dump(savestring, file)

print("data still writing to file")

print("savedata finished writing")

#the sign passed in doesn't do anything, I just didn't know how to make this work without passing something in...

print("stop flag raised")

endflag.set()

t1.join(5)

t2.join(5)

t4.join(5)