29 lines
746 B
Python
29 lines
746 B
Python
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")
|
|
|
|
|