script/calibration/BinarySearchXZ.py

@@ -0,0 +1,29 @@
+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_x = robot_x.getPosition()
+current_z = robot_z.getPosition()
+
+
+r = bsearch([robot_x, 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 XZ")
+
+
+print r.print()
+opt_x, opt_z= r.getOptimalPosition()
+offset_x, offset_z = opt_x - current_x, opt_z - current_z
+
+print "offset_x: ", offset_x, " offset_z: ", offset_z

script/calibration/ScanXZ.py

@@ -9,13 +9,17 @@ else:
 STEP_Z_FINAL = 0.1
     
 RANGE = [-5.0, 5.0]
-LATENCY = 0.005
+LATENCY = 0.05
 
 Z_FINAL_OFFSET = 0.0
 
 
+SINGLE_PASS = True
+STEP_SIZE = 0.1
+
+
 robot.enable()
-#move_to_laser()
+move_to_laser()
 
 step_x = STEP_SIZE
 step_z = STEP_SIZE
@@ -27,7 +31,7 @@ current_x = robot_x.getPosition()
 current_z = robot_z.getPosition()
 
 print "Current pos x,z" , current_x, ",", current_z
-ret = ascan([robot_x, robot_z], ue.readable, [range_x[0], range_z[0]], [range_x[1], range_z[1]], [step_x,step_z], latency = LATENCY, relative = True , zigzag=False, title = "2d Scan")
+ret = ascan([robot_x, robot_z], ue.readable, [range_x[0], range_z[0]], [range_x[1], range_z[1]], [step_x,step_z], latency = LATENCY, relative = True , zigzag=False, title = "Scan XY")
 data = ret.getData(0)[0]
 #plot(Convert.transpose(data), title="Data")