Rotating robot towards direction of the ball
https://stackoverflow.com/questions/12603964
italiano
english
français
española
中国
日本の
العربية
Deutsch
한국어
Português
RussianQuestion
I would like to know is this method the right way to go about this?.I want to rotate my robot which is facing north to the direction where an object is in this case the red ball.
Below shows the grid and the position of the ball
And here is my code to this:
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)
i just want to know is this the right way to solve this? or is there an alternative because i would be passing in values based on the question i posted previously. Thanks
Regards Edward
Solution
Way too much math. Use math.atan2() to get the desired angle and subtract it from π/2 to get the appropriate rotation.
Licensed under: CC-BY-SA with attribution
Not affiliated with StackOverflow
