Python 向球的方向旋转机器人

我想知道这个方法是正确的吗？我想旋转我的机器人，它朝着一个物体的方向，在这个例子中，红色的球

下面显示了网格和球的位置

这是我的代码：

def turn_to(brick, new_x, new_y, old_x, old_y, angle):
    delta_x = new_x - old_x
    print delta_x
    delta_y = new_y - old_y
    print delta_y

    roboCturn = base * pi
    Circumference = diameter * pi

    if(angle > 315 or angle < 45):
        #tested
        if (delta_x < 0 and delta_y == 0):
            #180 degrees tested
            angle_rad = math.radians(180)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho      

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x == 0 and delta_y > 0):
            #90 degrees right tested
            angle_rad = math.radians(90)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x == 0 and delta_y < 0): 
            #90 degrees left tested
            angle_rad = math.radians(90)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y > 0):
            # degrees in 1st quadrant tested
            angle_rad = math.atan2(delta_y, delta_x)
            angle_deg = 90 - ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y < 0):
            #degrees in 4th quadrant tested
            angle_rad = math.atan2((math.fabs(delta_y)), delta_x)
            angle_deg = 90 - ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x < 0 and delta_y > 0):
            #degrees in 2nd quadrant tested
            angle_rad = math.atan2(delta_y, (math.fabs(delta_x)))
            angle_deg = 90 + ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x < 0 and delta_y < 0):
            #degrees in 3rd quadrant tested
            angle_rad = math.atan2((math.fabs(delta_y)), (math.fabs(delta_x)))
            angle_deg = 90 + ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        else:
            left.weak_turn(power=0, tacho_units=0)
            right.weak_turn(power = 0, tacho_units=0)

    elif(angle > 45 and angle < 135):
        #tested
        if (delta_x == 0 and delta_y < 0):
            #180 degrees tested
            angle_rad = math.radians(180)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho      

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x < 0 and delta_y == 0):
            #90 degrees right tested
            angle_rad = math.radians(90)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y == 0): 
            #90 degrees left tested
            angle_rad = math.radians(90)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y > 0):
            # degrees in 1st quadrant tested
            angle_rad = math.atan2(delta_y, delta_x)
            angle_deg = 90 - ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y < 0):
            #degrees in 4th quadrant tested
            angle_rad = math.atan2((math.fabs(delta_y)), delta_x)
            angle_deg = 90 + ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x < 0 and delta_y > 0):
            #degrees in 2nd quadrant tested
            angle_rad = math.atan2(delta_y, (math.fabs(delta_x)))
            angle_deg = 90 - ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x < 0 and delta_y < 0):
            #degrees in 3rd quadrant tested
            angle_rad = math.atan2((math.fabs(delta_y)), (math.fabs(delta_x)))
            angle_deg = 90 + ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        else:
            left.weak_turn(power=0, tacho_units=0)
            right.weak_turn(power = 0, tacho_units=0)

    elif(angle > 135 and angle < 225):
        #tested
        if (delta_x > 0 and delta_y == 0):
            #180 degrees tested
            angle_rad = math.radians(180)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho      

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x == 0 and delta_y < 0):
            #90 degrees right tested
            angle_rad = math.radians(90)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x == 0 and delta_y > 0): 
            #90 degrees left tested
            angle_rad = math.radians(90)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y > 0):
            # degrees in 1st quadrant tested
            angle_rad = math.atan2(delta_y, delta_x)
            angle_deg = 90 + ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y < 0):
            #degrees in 4th quadrant tested
            angle_rad = math.atan2((math.fabs(delta_y)), delta_x)
            angle_deg = 90 + ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x < 0 and delta_y > 0):
            #degrees in 2nd quadrant tested
            angle_rad = math.atan2(delta_y, (math.fabs(delta_x)))
            angle_deg = 90 - ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x < 0 and delta_y < 0):
            #degrees in 3rd quadrant tested
            angle_rad = math.atan2((math.fabs(delta_y)), (math.fabs(delta_x)))
            angle_deg = 90 - ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        else:
            left.weak_turn(power=0, tacho_units=0)
            right.weak_turn(power = 0, tacho_units=0)

    elif(angle > 225 and angle < 315):
        #tested
        if (delta_x == 0 and delta_y > 0):
            #180 degrees tested
            angle_rad = math.radians(180)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho      

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y == 0):
            #90 degrees right tested
            angle_rad = math.radians(90)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x < 0 and delta_y == 0): 
            #90 degrees left tested
            angle_rad = math.radians(90)
            angle_deg = (angle_rad) * (180/pi)
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif (delta_x > 0 and delta_y > 0):
            # degrees in 1st quadrant tested
            angle_rad = math.atan2(delta_y, delta_x)
            angle_deg = 90 + ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif(delta_x > 0 and delta_y < 0):
            #degrees in 4th quadrant tested
            angle_rad = math.atan2((math.fabs(delta_y)), delta_x)
            angle_deg = 90 - ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            left.weak_turn(power=power, tacho_units=tachoUnits)
            right.weak_turn(power=-power, tacho_units=tachoUnits)

        elif(delta_x < 0 and delta_y > 0):
            #degrees in 2nd quadrant tested
            angle_rad = math.atan2(delta_y, (math.fabs(delta_x)))
            angle_deg = 90 + ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        elif(delta_x < 0 and delta_y < 0):
            #degrees in 3rd quadrant tested
            angle_rad = math.atan2((math.fabs(delta_y)), (math.fabs(delta_x)))
            angle_deg = 90 - ((angle_rad) * (180/pi))
            print angle_deg
            distanceTurned = (angle_deg/360) * (roboCturn)
            numberRotations = (distanceTurned) / (Circumference)
            tachoUnits = (numberRotations) * tacho

            right.weak_turn(power=power, tacho_units=tachoUnits)
            left.weak_turn(power=-power, tacho_units=tachoUnits)

        else:
            left.weak_turn(power=0, tacho_units=0)
            right.weak_turn(power = 0, tacho_units=0)

def turn_to（砖、新砖、新砖、旧砖、旧砖、角度）：
delta_x=新_x-旧_x
打印delta_x
delta_y=新_y-旧_y
打印增量y
roboCturn=基准*pi
周长=直径*pi
如果（角度>315或角度<45）：
#测试
如果（δx<0，δy==0）：
#180度测试
角度弧度=数学弧度（180）
角度角度=（角度弧度）*（180/pi）
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
左。弱转弯（功率=功率，转速计单位=转速计）
右。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（delta_x==0且delta_y>0）：
#右90度测试
角度弧度=数学弧度（90）
角度角度=（角度弧度）*（180/pi）
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
左。弱转弯（功率=功率，转速计单位=转速计）
右。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（delta_x==0，delta_y<0）：
#左90度测试
角度弧度=数学弧度（90）
角度角度=（角度弧度）*（180/pi）
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
右。弱转弯（功率=功率，测速仪单位=测速仪）
左。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（增量x>0和增量y>0）：
#测试第一象限的度数
角度弧度=数学常数2（δy，δx）
角度度=90-（（角度度）*（180/pi））
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
左。弱转弯（功率=功率，转速计单位=转速计）
右。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（δx>0和δy<0）：
#测试第四象限的度数
角度=math.atan2（（math.fabs（delta_y）），delta_x）
角度度=90-（（角度度）*（180/pi））
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
右。弱转弯（功率=功率，测速仪单位=测速仪）
左。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（δx<0和δy>0）：
#测试第二象限的度数
角度rad=math.atan2（delta_y，（math.fabs（delta_x）））
角度度=90+（（角度度）*（180/pi））
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
左。弱转弯（功率=功率，转速计单位=转速计）
右。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（δx<0和δy<0）：
#测试第三象限的度数
angle_rad=math.atan2（（math.fabs（delta_y）），（math.fabs（delta_x）））
角度度=90+（（角度度）*（180/pi））
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
右。弱转弯（功率=功率，测速仪单位=测速仪）
左。弱转弯（功率=-功率，测速仪单位=测速仪）
其他：
向左。弱转弯（功率=0，测速单位=0）
右。弱转弯（功率=0，测速单位=0）
elif（角度>45且角度<135）：
#测试
如果（δx==0且δy<0）：
#180度测试
角度弧度=数学弧度（180）
角度角度=（角度弧度）*（180/pi）
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
左。弱转弯（功率=功率，转速计单位=转速计）
右。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（delta_x<0，delta_y==0）：
#右90度测试
角度弧度=数学弧度（90）
角度角度=（角度弧度）*（180/pi）
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
左。弱转弯（功率=功率，转速计单位=转速计）
右。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（delta_x>0，delta_y==0）：
#左90度测试
角度弧度=数学弧度（90）
角度角度=（角度弧度）*（180/pi）
打印角度
旋转距离=（角度360）*（机械臂旋转）
numberRotations=（旋转距离）/（周长）
tachoUnits=（数字箭头）*测速仪
右。弱转弯（功率=功率，测速仪单位=测速仪）
左。弱转弯（功率=-功率，测速仪单位=测速仪）
elif（增量x>0和增量y>0）：