$$\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{grey}{RGB}{120,120,120} \definecolor{answer}{RGB}{0,150,0}$$

# My blog page : BaseketBot

## NXT Yaw Angle Pythagorean theorem

In this web page, I'll show the calculations for finding the yaw (horizontal) angle of my robot using right triangles and Pythagorean theorem.
You may want to check-out how I get the same result using laws of cosines.

### Calculating the sides of $$\overline{ahw}$$ triangle.

In the above picture, I am trying to calculate angle $$\alpha$$ and distance d.
First, I will need to determine the height of the $$\overline{ahw}$$ triangle. I'll start by calculating the height of the $$\overline{abc}$$ triangle since I know that its height is equivalent to the height of $$\overline{ahw}$$ triangle. I know this to be true because of parallel geometry and rules of parallelograms.

I know the area of a triangle is equal to $$A_1=\frac{1}{2} b h$$. I also know that the area of a triangle is $$A_2= \sqrt{ p ( p - a ) ( p - b ) ( p - c ) }$$, so it is a valid statement to say:

$$A_1=A_2 \\ \frac {1} {2} b h = \sqrt { p ( p - a ) ( p - b ) ( p - c ) } \\ \text{where } p = \frac{a+b+c}{2} \tag{1}$$

I know the base of the $$\overline{abc}$$ triangle is b, and its height is h. I solve for height $$h$$:

$$b h = 2 \sqrt { p ( p - a ) ( p - b ) ( p - c ) } \\ h = \frac {2 \sqrt { p ( p - a ) ( p - b ) ( p - c ) } } { b } \tag{2}$$

Now, I need to calculate the length of $$w$$ in the $$\overline{awh}$$ triangle. Pythagorean theorem states that the hypotenuse squared is equal to the other sides squared. Therefore, I can say:

$$a ^ 2 = h ^ 2 + w ^ 2 \\ w ^ 2 = a ^ 2 - h ^ 2 \\ w = \sqrt { a ^ 2 - h ^ 2 } \tag{3}$$

Now I have enough information to calculate angle $$\alpha$$ and distance $$d$$.

### Calculating $$\alpha$$ & $$d$$

To calculate distance $$d$$, I am going to use the Pythagorean theorem again:

$$d ^ 2 = (\frac{b}{2} + w) ^ 2 + ( h + f ) ^ 2 \\ d = \sqrt{(\frac{b}{2} + w) ^ 2 + ( h + f ) ^ 2 } \tag{4}$$

To calculate $$\alpha$$, I am going to use trigonometry and solve for $$\alpha$$

$$sin (\alpha) = \frac { ( h + f ) } {d} \\ \alpha = arcsin \frac { ( h + f ) } {d} \tag{5}$$

### Checking for special conditions

In the following picture, the target is moved to the left so that it's aligned under the left ultrasonic sensor. As a result, the width $$w$$ becomes zero. Despite this change, my earlier equations for distance $$d$$ and angle $$\alpha$$ remain the same.

However, when the target is between the left ultrasonic sensor and the mid-point of the two sensors, as shown in the following picture, the formula changes for distance $$d$$.

As you can see in the above picture, the length $$w$$ takes overlaps $$\frac{b}{2}$$. Therefore, I am going have to change the calculations for distance from the equation (4) to $$d = \sqrt{(\frac{b}{2} \textcolor{delete}{-} w) ^ 2 + ( h + f ) ^ 2 }$$ when the target is on the right side of the left ultrasonic sensor and to the left of mid-point of the two sensors. Notice that the sign of $$w$$ is now negative in comparison to equation (4).

Instead of using an if condition to determine if I should use $$(\frac{b}{2} \textcolor{delete}{-} w)$$ or $$(\frac{b}{2} \textcolor{delete}{+} w)$$ in the equation for distance, I can create two new variables for length $$l$$ and sign $$s$$, and substitute them into the equation for distance $$d$$. Consider $$l$$ to be the following:

$$l = \frac{b}{2} + \textcolor{answer}{s}w \tag{6}$$

If the target is on the left side of the left ultrasonic sensor, then the value of $$\textcolor{answer}{s}$$ will be $$1$$ (a positive value); if it is to the right, then its value will be $$-1$$ (a negative value).

We can calculate the sign $$\textcolor{answer}{s}$$ using Pythagorean theorem, because of the following rules:

 $$c^2 < a^2 + b^2$$ $$\angle \beta$$ is an acute angle $$c^2 > a^2 + b^2$$ $$\angle \beta$$ is an obtuse angle $$c^2 = a^2 + b^2$$ $$\angle \beta$$ is a right angle
$$\textcolor{answer}{s} = \frac{ c ^ 2 - a ^ 2 - b^ 2 } { | c ^ 2 - a ^ 2 - b^ 2 | } \tag{7}$$

But note that $$\textcolor{answer}{s}$$ is undefined when we have a right angle because the denominator is zero. Though, it is very unlikely that the target will meet such a condition, we still need to consider this situation. Notice that $$c^2 = a^2 + b^2$$ when $$a=h$$ for the right triangle. So, we can summarize our equation to the following:

$$l = \frac{b}{2} + \textcolor{answer}{s}w \\ \\ \text {where } \textcolor{answer}{s} = \cases { 0 & \text {if } a=h \\ \frac{ c ^ 2 - a ^ 2 - b^ 2 } { | c ^ 2 - a ^ 2 - b^ 2 | } & \text {if } a \ne h } \\ \\ d = \sqrt{ l ^ 2 + ( h + f ) ^ 2 } \tag{8}$$

In the below image, I continue to move the ultrasonic sensor to the right so that the target is in the middle of the two ultrasonic sensors and directly in front of the cannon, forming a right triangle. However, no changes occur in calculating angle $$\alpha$$ from equation (5) and distance $$d$$ from equation (8).

As the target continues to move to the right such that it's located between the midpoint and the right ultrasonic sensor, there needs to be a change in the equation.

Using the existing equations for the above picture, I am calculating the angle $$\theta$$ (theta) instead of $$\alpha$$, so I must take the supplement of $$\theta$$ to get $$alpha$$, that is:

$$\theta = arcsin \frac { ( h + f ) } {d}\\ \alpha = 180 - \theta \tag{9}$$

I will work backwards to build a generic equation for $$\alpha$$. First, I need something to cancel itself out and the sum of its absolute value is equal to $$180$$, that is:

$$x-x=0 \\ |x| + |x| = 180 \\ 2|x|=180 \\ |x|=90 \tag{10}$$

Second, $$\alpha$$ = $$180-\theta$$ when the target is to the right side of the mid-point; that is, when length $$l$$ is negative. However, I don't take the supplement of the angle when the target is to the left of the mid-point; that is $$l$$ is positive. Thus, the following equation is realized:

\begin{align*} \alpha & = \frac{-l}{|l|}90 + 90 + \frac{l}{|l|}\theta \\ & = \frac{-l}{|l|}90 + \frac{l}{|l|}\theta + 90 \\ & = \frac{l}{|l|}(\theta -90) + 90 \end{align*} \tag{11}

But note that $$\alpha$$ is undefined when length $$l$$ is zero. So, we generalize it into the following:

$$\alpha = \cases{ 90 & \text{if } l = 0\cr \frac{l}{|l|}(\theta - 90)+90 & \text{if } l \ne 0 } \tag{12}$$

As the target moves to the right, the calculations remain the same.

The final answer is shown below:
Notice that I must check for multiple condition to avoid division by zero, which means I must have multiple if conditions in my program. This is why, I prefer to use laws of cosines to calculate the yaw angle for this problem.

$$\textcolor{grey} { h = \frac {2 \sqrt { p ( p - a ) ( p - b ) ( p - c ) } } { b } \\ \text{ where } p = \frac{a+b+c}{2} \\ l = \cases { b \div 2 & \text {if } a=h \\ \frac{b}{2} + w\bigg( \frac{ c ^ 2 - a ^ 2 - b^ 2 } { | c ^ 2 - a ^ 2 - b^ 2 | }\bigg) & \text {if } a \ne h } \\ \textcolor{answer} { d = \sqrt{ l ^ 2 + ( h + f ) ^ 2 } \\ \alpha = \cases{ 90 & \text{if } l = 0\cr \frac{l}{|l|}(arcsin \frac { ( h + f ) } {d} - 90)+90 & \text{if } l \ne 0 } } }$$