37 lines
821 B
Python
37 lines
821 B
Python
import plotutils
|
|
from mathutils import fit_gaussian, Gaussian
|
|
|
|
robot.enable()
|
|
move_to_laser()
|
|
|
|
|
|
RANGE = [-1.5, 1.5]
|
|
STEP = 0.025
|
|
Z_OFFSET = -1.0
|
|
LATENCY = 0.005
|
|
|
|
robot.set_motors_enabled(True)
|
|
current_positon = robot_x.getPosition()
|
|
robot_z.moveRel(Z_OFFSET)
|
|
ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
|
|
|
|
d = ret.getReadable(0)
|
|
|
|
first_index = -1
|
|
last_index = -1
|
|
for i in range(len(d)):
|
|
if not math.isnan(d[i]):
|
|
if first_index<0:
|
|
first_index = i
|
|
last_index = i
|
|
|
|
if first_index == -1 or last_index < first_index:
|
|
raise Exception("Invalid X detection")
|
|
|
|
|
|
center_index = (first_index + last_index)/2
|
|
center_positon = ret.getPositions(0)[center_index]
|
|
center_offset = center_positon-current_positon
|
|
|
|
|
|
print "X offset = ", center_offset |