# Cross-notebook include shim
with open("nbinclude.ipynb") as nbinclude_f: # don't rename nbinclude_f
    import IPython.nbformat.current
    get_ipython().run_cell(IPython.nbformat.current.read(nbinclude_f, 'json').worksheets[0].cells[0].input)

import sys, IPython, time

nbinclude('robot')

r=Robot()

r.enable()

r.cmd(.2,.2)

r.cmd(0,0)

mbed=SerialRPC('/dev/ttyACM0', 115200)

ains=[]
for i in (p20,p19,p18,p17,p16,p15):
#     AnalogIn(mbed, 'p{0}'.format(i))
    a=AnalogIn(mbed, i)
    print a.read()
    ains.append(AnalogIn(mbed, i))

for ain in ains:
    print ain.read()

def read(ain): return ain.read()

def line_pos(ains):
    vals=map(read, ains)
    wsum=0
    for i in range(len(vals)):
        wsum+= (i+1.0)*vals[i]
    return wsum/sum(vals)

line_pos(ains)

%timeit line_pos(ains)

1000/32.7 

while True:
    IPython.display.clear_output()
    pos= line_pos(ains)
    out=(3.0-pos)*.2
    
    print pos, out
    r.cmd(.2-out, .2+out)
    sys.stdout.flush()
    time.sleep(.01)