$$ \definecolor{color1}{RGB}{0,0,255} \definecolor{color2}{RGB}{255,128,0} \definecolor{color3}{RGB}{102,0,102} \definecolor{delete}{RGB}{255,0,0} \definecolor{answer}{RGB}{0,150,0} $$

My blog page : BaseketBot

NXT Yaw Angle Laws Cosines

In this web page, I show the calculations for determining the yaw (horizontal) angle using laws of cosines.
You may want to check-out how I get the same result using Pythagorean theorem.

In the following picture I want to find the angle \(\alpha\) (alpha). The way you calculate this angle depends on the target's location.

NXT target located to the left of the cannon

In above picture, f is a fixed distance between the tip of the canon and the left ultrasonic sensor.

The distance between sensors is also fixed and is the base of the triangle, which is labelled as b.

Calculating \(\theta\) & (theta)

I'll start by finding the constant angle \(\beta\). First of all, my robot canon is located between the two sensors; therefore, the mid-point is \(\frac{b}{2}\). Using trigonometry I can say that:

$$ \begin{align*} \cos \beta &= \frac{\frac{b}{2}}{f} \\ \\ &= \frac{b}{2} \frac{f}{1} = \frac{b}{2f} \end{align*} $$

I want to find the angle \(\beta\) (beta) so I need to take arc cosine of both sides to find the angle beta.

$$ \beta = \arccos(\frac{b}{2f}) $$

Next, I need to find the angle \(\theta\) (theta). Since the triangle \(\overline{abc}\) is not a right triangle, I must use laws of cosines to find \(\theta\):

$$ c^2 = a^2 + b^2 - 2ab \cos \theta $$

Now I solve for \(\theta\) using algebra:

$$ c^2 - a^2 - b^2 = -2ab \cos \theta $$ $$ \frac{c^2 - a^2 - b^2} {- 2 a b} = \cos \theta $$ $$ \theta = \arccos \frac{c^2 - a^2 - b^2} {- 2 a b} $$

Calculating distance d

Now I need to find the distance from the target to the tip of the cannon. In the above picture, the distance d is calculated using the \(\overline{adf}\) triangle via the following equation:

$$ d^2 = a^2 + f^2 - 2af \cos (\theta + \beta) $$

I must take square root of both sides to get the distance d:

$$ d = \sqrt{a^2 + f^2 - 2af \cos (\theta + \beta)} $$

Calculating angles \(\epsilon\) & \(\alpha\)

Finally, I have enough information to calculate \(\epsilon\), which is one of the required values for calculating the yaw angle \(\alpha\).

$$ a^2=f^2+d^2 -2fd \cos \epsilon $$

The following equations simplify the above equation.

$$ a^2 - f^2 - d^2 = -2fd \cos \epsilon $$ $$ \frac{a^2 - f^2 - d^2} {-2fd} = \cos \epsilon $$ $$ \epsilon = \arccos \frac{a^2 - f^2 - d^2} {- 2 f d} $$

Now, I have all the required values to calculate \(\alpha\).

$$ \alpha = \epsilon + \beta $$

Special cases

As you move the target to the right, all the calculations stay the same.

Calculation remain the same as target is move to the right

However, there is another special case that I must consider. If the target is moved to the far left, then the line for distance d falls to the left side of the \(\overline{abc}\) triangle (see below image):

Special case image when target is moved to extreme left

In above image, \(\beta + \theta \) is greater than 180o; therefore, the new angle \(\delta\) is equal to 360o-(\(\beta + \theta\)), which is needed to calculate distance d. But when \(\beta + \theta \) is less than 180o, \(\delta\) is equal to \(\beta + \theta\). In another words:

$$ \delta = \begin{cases} \beta + \theta & \text{if } \beta + \theta <=180 \\ 360 - (\beta + \theta) & \text{if } \beta + \theta > 180 \\ \end{cases} $$

However, cosine of the obtuse angle \(\delta\) is same as 360o-\(\delta\); that is, \(\cos \delta \equiv cos (360-\delta)\)
Therefore, I don't need to consider the above special case.

Below is the conclusion to my calculations:

$$ \textcolor{answer}{ \begin{align*} & \beta = \arccos(\frac{b}{2f}) \\ & \theta = \arccos \frac{c^2 - a^2 - b^2} {- 2 a b} \\ & d = \sqrt{a^2 + f^2 - 2af \cos (\theta + \beta)} \\ & \epsilon = \arccos \frac{a^2 - f^2 - d^2} {- 2 f d} \\ & \alpha = \epsilon + \beta \end{align*} } $$