import plotutils
from mathutils import fit_gaussian, Gaussian

robot.enable()
move_to_laser()

#robot.reset_motion("jLaser")

#import ch.psi.pshell.device.Timestamp as Timestamp
#sensor = Timestamp()

#ret = lscan(robot_z, sensor, -50.0, 0.0, 0.5, latency = 0, relative = True)
#ret = lscan(robot_x, sensor, -50.0, 0.0, 0.5, latency = 0, relative = True)

robot.set_motors_enabled(True)
ret = lscan(robot_x, ue.readable, -1.5, 1.5, 0.1, latency = 0.01, relative = True)

x,y = ret.getPositions(0), ret.getReadable(0)
y = [100.0-v for v in y]
(norm, mea, sigma) = fit(y, xdata=x)
print "Maximum at " + str(mea)



robot.set_motors_enabled(True)
ret = lscan(robot_rz, ue.readable, 0.0, 40.0, 1.0, latency = 0.01, relative = True, range = "auto")