def calibrateCompass():
x_out_raw_min = float("infinity")
x_out_raw_max = float("-infinity")
y_out_raw_min = float("infinity")
y_out_raw_max = float("-infinity")
# Turn around a few times to make sure all angles are measured.
for i in range(300):
(x_out_raw, y_out_raw, z_out_raw, x_out, y_out, raw_degrees, degrees) = readCompass(True)
own_util.move('right', 1, 0.2, True)
if x_out_raw_min > x_out_raw:
x_out_raw_min = x_out_raw
if x_out_raw_max < x_out_raw:
x_out_raw_max = x_out_raw
if y_out_raw_min > y_out_raw:
y_out_raw_min = y_out_raw
if y_out_raw_max < y_out_raw:
y_out_raw_max = y_out_raw
print str(i) + ':', 'x_out_raw y_out_raw z_out_raw =', x_out_raw, y_out_raw, z_out_raw
x_factor = 400.0 / (x_out_raw_max - x_out_raw_min)
x_offset = 200.0 - x_out_raw_max * x_factor
y_factor = 400.0 / (y_out_raw_max - y_out_raw_min)
y_offset = 200.0 - y_out_raw_max * y_factor
print 'x_out_raw_min x_out_raw_max =', round(x_out_raw_min,2), round(x_out_raw_max,2)
print 'y_out_raw_min y_out_raw_max =', round(y_out_raw_min,2), round(y_out_raw_max,2)
print 'x_factor =', round(x_factor,2)
print 'x_offset =', round(x_offset,2)
print 'y_factor =', round(y_factor,2)
print 'y_offset =', round(y_offset,2)
def calibrateCompass():
x_out_raw_min = float("infinity")
x_out_raw_max = float("-infinity")
y_out_raw_min = float("infinity")
y_out_raw_max = float("-infinity")
# Turn around a few times to make sure all angles are measured.
for i in range(300):
(x_out_raw, y_out_raw, z_out_raw, x_out, y_out, raw_degrees, degrees) = readCompass(True)
own_util.move('right', 128, 0.2, True)
if x_out_raw_min > x_out_raw:
x_out_raw_min = x_out_raw
if x_out_raw_max < x_out_raw:
x_out_raw_max = x_out_raw
if y_out_raw_min > y_out_raw:
y_out_raw_min = y_out_raw
if y_out_raw_max < y_out_raw:
y_out_raw_max = y_out_raw
print str(i) + ':', 'x_out_raw y_out_raw z_out_raw =', x_out_raw, y_out_raw, z_out_raw
x_factor = 400.0 / (x_out_raw_max - x_out_raw_min)
x_offset = 200.0 - x_out_raw_max * x_factor
y_factor = 400.0 / (y_out_raw_max - y_out_raw_min)
y_offset = 200.0 - y_out_raw_max * y_factor
print 'x_out_raw_min x_out_raw_max =', round(x_out_raw_min,2), round(x_out_raw_max,2)
print 'y_out_raw_min y_out_raw_max =', round(y_out_raw_min,2), round(y_out_raw_max,2)
print 'x_factor =', round(x_factor,2)
print 'x_offset =', round(x_offset,2)
print 'y_factor =', round(y_factor,2)
print 'y_offset =', round(y_offset,2)
def gotoDegreeAbs(targetDegree, doMove):
currentDegree = readCompass()
diffAngle = targetDegree - currentDegree
if diffAngle > 180:
diffAngle = diffAngle - 360
elif diffAngle < -180:
diffAngle = diffAngle + 360
if doMove == False:
return diffAngle
while abs(diffAngle) > 2.0:
if diffAngle > 0:
if abs(diffAngle) < 20:
own_util.move('right', 1, 1.0, True)
elif abs(diffAngle) < 50:
own_util.move('right', 12, 1.0, True)
else:
own_util.move('right', 32, 1.0, True)
else:
if abs(diffAngle) < 20:
own_util.move('left', 1, 1.0, True)
elif abs(diffAngle) < 50:
own_util.move('left', 12, 1.0, True)
else:
own_util.move('left', 32, 1.0, True)
currentDegree = readCompass()
diffAngle = targetDegree - currentDegree
if diffAngle > 180:
diffAngle = diffAngle - 360
elif diffAngle < -180:
diffAngle = diffAngle + 360
return diffAngle
def gotoDegreeAbs(targetDegree, doMove):
currentDegree = readCompass()
diffAngle = targetDegree - currentDegree
if diffAngle > 180:
diffAngle = diffAngle - 360
elif diffAngle < -180:
diffAngle = diffAngle + 360
if doMove == False:
return diffAngle
while abs(diffAngle) > 1.0:
if diffAngle > 0:
if abs(diffAngle) < 20:
own_util.move('right', 128, 1.0, True)
elif abs(diffAngle) < 50:
own_util.move('right', 140, 1.0, True)
else:
own_util.move('right', 160, 1.0, True)
else:
if abs(diffAngle) < 20:
own_util.move('left', 128, 1.0, True)
elif abs(diffAngle) < 50:
own_util.move('left', 140, 1.0, True)
else:
own_util.move('left', 160, 1.0, True)
currentDegree = readCompass()
diffAngle = targetDegree - currentDegree
if diffAngle > 180:
diffAngle = diffAngle - 360
elif diffAngle < -180:
diffAngle = diffAngle + 360
return diffAngle
