Basic Trig


Note: some of the variable names will have changed from the video, specifically x, y, and h.

θ = theta = angle
x = horizontal distance

y = vertical distance

Localizing position

A right triangle is formed with h, x, and y. Thus, we can establish the following trigonometric identities:

sin(θ) = y / h
cos(θ) = x / h

Here, we are assuming that we know the angle θ (for example, from a gyro sensor or IMU) and the distance h as measured by a sensor. We can use this information to calculate the distances x and y by using the trigonometric identities listed above:

y = h sin(θ)
x = h cos(θ)

Localizing orientation

Let us define h=h1+h2. h is therefore the total length of the hypotenuse in the right triangle. Since we know the horizontal distance x, which is the width of the field, we can therefore find the angle θ.

Let us first establish the following trigonometric identities by simply taking the reverse of the previous identities discussed (note that arcsin is the inverse function of sin):

θ = arcsin(y / h)
θ = arccos(x / h)

By these identities, we can simply plug in h we measure with our sensors to get θ = arccos((4 feet)/h), and thus we have the angle the robot is facing. Note that the angle you get won’t be able to distinguish whether the robot is pointing towards h1 or h2. Note the following image will yield the same angle theta, but the robot will be oriented differently.


Note first the difference between radians and degrees. They are both ways of measuring angles. There there are 360 degrees in a circle, whereas there are 2π radians in a circle. Therefore, 1 degree =  π/180 radians.

In RobotC, these are the function calls for trigonometric functions:

asin(y/h) returns the arcsine in radians (arcsin(y / h)).
acos(y/h) returns the arccosine in radians (arcsin(x / h)).
sin(θ) returns the value of y/h for an angle θ in radians.
cos(θ) returns the value of x/h for an angle θ in radians.
sinDegrees(θ) returns the value of y/h for an angle θ in degrees.
cosDegrees(θ) returns the value of x/h for an angle θ in degrees.
radiansToDegrees(θ) returns the angle in degrees for a given angle θ in radians.
degreesToRadians(θ) returns the angle in radians for a given angle θ in degrees.

Sensor Inaccuracies

Note that the values you get back from the sensor may not be exactly accurate. First, the value an ultrasonic or infrared sensor returns can vary from the true result, especially at long distances, since practically its is impossible to make a perfectly accurate sensor. In particular, both the infrared and ultrasonic sensors have a maximum distance it can sense up to, and the ultrasonic sensor will give junk values if it is placed directly up to something (when the distance between the object and the sensor is very low). In other cases, they should be fairly accurate.

Another thing to note is that ultrasonic sensors don’t see in a straight line, but instead a cone. That means the value you get back will be approximately the closest distance to any obstacle inside a conical region in front of it, as demonstrated by the image below.

In the diagram below, even though the obstacle (denoted by the black circle) is not directly in front of the sensor, since it is in the conical field of view the sensor will not return the distance to the wall but instead something closer to the distance to the obstacle.

Finally, some infrared sensors (such as the LEGO EV3 infrared sensor) don’t provide an absolute distance measurement; they must first be converted to distances, and their values may change depending on the color of the surface.

Motor Encoders

What are motor encoders?

Motor encoders measure how many degrees a motor has rotated. So if the motor encoder value reported is 720, the motor has gone 2 full rotations. Therefore, the formula to calculate the number of rotations is (# of rotations) = (# of degrees)/360°. Let (# of rotations) = r and (# of degrees) = n. Thus, r = n/360°.

Note: in the stock graphical LEGO software, encoder values can be provided as either rotations or degrees.

Moving in a straight line

First, we can easily find the circumference of a wheel by finding the radius and applying the equation c = 2πr. Note that the circumference c is how far the robot moves with one rotations of the motor (and thus the wheel).

The equation d = c*r is true to find the distance d the robot moves with r rotations. Since r = n/360°, d = c*n/360°. By simply rearranging the equation, we get n = d*360/c°. Therefore, given the distance we want to travel, we know how many degrees the motor needs to spin. This can be used in conjunction with a loop or similar to implement a moving function.

Point Turns

To turn a certain angle in place, we can execute a point turn where the robot’s two motors turn in opposite directions. Thus, when the robot turns 360°, each motor traces the circumference of a circle with a diameter of the distance between the robot’s wheels.

If we define the wheelbase (the distance between the wheels) to be w, we can calculate the circumference of this circle with πw. To turn an arbitrary amount, we multiply this circumference by the ratio of the angle desired a to one full revolution: a/360°. Now that we know the distance that each wheel needs to travel, we can apply the equations from moving in straight lines; the only difference is that one wheel is rotating in the opposite direction. Therefore the equation for the degrees a motor needs to turn is n = aπw/(360c) where c is the circumference of the wheel.

Full video