Beispiel #1
0
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)
Beispiel #2
0
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)
Beispiel #3
0
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
Beispiel #4
0
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