29 lines
676 B
Python
29 lines
676 B
Python
import plotutils
|
|
import math
|
|
|
|
|
|
#STRATEGY = "Normal"
|
|
#STRATEGY = "Boundary"
|
|
#STRATEGY = "FullNeighborhood"
|
|
RANGE = [-5.0, 5.0]
|
|
STEP_SIZE = 0.1
|
|
LATENCY = 0.05
|
|
|
|
|
|
robot.enable()
|
|
move_to_laser()
|
|
|
|
|
|
robot.set_motors_enabled(True)
|
|
current_y = robot_y.getPosition()
|
|
current_z = robot_z.getPosition()
|
|
|
|
|
|
r = bsearch([robot_y, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search YZ")
|
|
|
|
|
|
print r.print()
|
|
opt_y, opt_z= r.getOptimalPosition()
|
|
offset_y, offset_z = opt_y - current_y, opt_z - current_z
|
|
|
|
print "offset_y: ", offset_y, " offset_z: ", offset_z |