$$\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 Pitch Angle

I made a mistake at time slot 19:45 where I squared distance $$d$$, that is $$tan\theta=\frac{v^2}{g\ d^\cancel{\textcolor{delete}{2}}} \pm \ \sqrt{...}$$

In order to calculate my pitch angle $$\theta$$, I need to use the projectile motion formula of:

$$y - y_o = v_o \times t + \frac{1}{2} a t^2$$

$$y-y_o$$ is the difference between the target height and the tip of the cannon barrel. The tip of the barrel is $$\textcolor{answer}{\delta}$$ the height of the robot ($$h_r$$) plus the length $$l$$ of the barrel for the given angle theta; in another words, this is: $$l \ sin\theta + h_r$$.

I already calculated my Lego Mindstorm cannon velocity $$v_o$$, which is constant.

The projectile flight time is $$t$$, and the vertical acceleration is $$g$$ for gravity, which is $$9.81 \frac{m}{s^2}$$. The vertical velocity for the angle theta is $$v \ \sin \theta$$.
With some substitutions we drive to the following equation.

$$h_t - l \ \sin \theta - h_r = v \ \sin \theta \ \times \ \textcolor{color1}{t} - \frac{1}{2} g \textcolor{color1}{t}^2 \tag{1}$$

The above equation is for the vertical motion. We can now do the same for the horizontal motion. Again, we start with the same projectile equation:

$$x - x_o = v_o \times t + \frac{1}{2} a t^2$$

$$x-x_o$$ is the horizontal distance that the projectile must travel. $$d_o$$ is the distance from the base of the cannon to the middle of the target. Because the angle of the barrel changes, we must account for the change in the horizontal distance, which is : $$l \ cos \theta$$. Therefore, the total horizontal distance travelled is $$d_o - l \ cos \theta$$. Also, the horizontal acceleration is zero because there isn't any force working with or against the ball.Lastly, the horizontal velocity is $$v\ cos\theta$$. We now have the horizontal projectile motion equation:

$$d_o \ - \ l \ cos\theta = v\ cos\theta\ t + 0 \tag{2}$$

We now solve for time $$t$$ in our horizontal equation (2) and then and then plug it in to our vertical equation (1).

$$\frac{d_o \ - \ l \ cos\theta}{v\ cos\theta} = \frac{v\ cos\theta \ t}{v\ cos\theta} \\ t = \frac{d_o \ - \ l \ cos\theta}{v\ cos\theta}$$

Now that I have solved for time $$t$$, I'll substitute time into the equation (1), which yields to the following:

$$h_t - l \ \textcolor{delete}{\sin \theta}- h_r = v \ \sin \theta \ \bigg( \textcolor{color1}{ \frac {d \textcolor{delete}{- l \ cos \theta} } {v \ cos \theta}\bigg) } - \frac{1}{2} g \bigg( \textcolor{color1}{ \frac {d \textcolor{delete}{- l \ cos \theta}} {v \ cos \theta}\bigg) } ^2 \tag{3}$$

If I continue solving this problem, I'll get to a 4th degree polynomial equation which is very hard to solve; so, I can try and do some estimates. I'm shooting at above a 45$$^{\circ}$$ degree angle because I want the ball to go into the basket, not hit it. As I increase the angle, $$\textcolor{delete}{l \ sin \theta}$$ quickly approaches $$l$$ . Similarly, $$\textcolor{delete}{l \ cos \theta }$$ approaches zero as I get closer to 90$$^{\circ}$$ degrees. Therefore, I'll change those values to $$l$$ and zero, respectively. Later, I'll show the program pseudocode for a more accurate estimate.

$$h_t - l- h_r \ = v \ \sin \theta \ \bigg(\frac {d} {v \ cos \theta}\bigg) - \frac{g}{2} \bigg(\frac {d} {v \ cos \theta}\bigg)^2 \tag{4}$$

Now I will simplify the above equation.

$$h_t - l- h_r \ = \ \textcolor{delete}{\frac {d\ v \ \sin\theta} {v \ cos \theta}}\ - \frac {g\ d^2} {2\ v^2}\ \textcolor{color1}{\frac{1}{cos^2 \theta} } \tag{5}$$

We know the trigonometry identities $$\frac{ sin \theta }{cos \theta} = \tan \theta$$, $$\frac{1}{cos^2 \theta} = \sec^2 \theta$$, and $$\sec^2 \theta = ( 1 + tan^2 \theta )$$, which leads to the following equations:

$$h_t - l- h_r \ = \ d\ \tan\theta\ - \frac {g\ d^2} {2\ v^2}\ \textcolor{color1}{ sec^2 \theta } \tag{6}$$ $$h_t - l - h_r = \ d\ \tan\theta\ - \frac {g\ d^2} {2\ v^2}\ \textcolor{color1}{(1+tan^2 \theta)} \tag{7}$$ $$h_t - l- h_r \ = \ d\ \tan\theta\ - \frac {g\ d^2} {2\ v^2}-\ \frac {g\ d^2} {2\ v^2}tan^2\theta \tag{8}$$

I'm going to reorder this equation so that I can use quadratic equation:

$$\textcolor{color1}{ \frac {-g\ d^2} {2\ v^2} } \ tan^2\theta+ \ \textcolor{color2}{d} \ \tan\theta + \textcolor{color3}{ \bigg(\frac {-g\ d^2} {2\ v^2}-h_t + l+ h_r \bigg) } =0 \tag{9}$$

Solving for $$tan \theta$$ using quadratic equation we get:

$$tan \theta = \frac {\textcolor{color2}{-d} \pm \sqrt{\textcolor{color2}{d^2}- 4 \textcolor{color1}{ \frac {-g\ d^2} {2v^2} } \textcolor{color3}{ \bigg( \frac {-g\ d^2} {2\ v^2} -h_t + l+ h_r \bigg) } } } {2 \textcolor{color1}{ \frac {-g\ d^2} {2v^2} } } \tag{10}$$

We can simplify the above equation with some algebra:

$$tan \theta = \Bigg(-d \pm \sqrt{d^2+ \frac {2\ g\ d^2} {v^2} \bigg( \frac {-g\ d^2} {2\ v^2} -h_t + l+ h_r \bigg) }\ \ \Bigg)\Big( \frac {v^2} {-g\ d^2} \Big) \tag{11}$$

The next part of the equation being simplified is in blue.

$$tan \theta = \frac {v^2} {g\ d} \pm \sqrt{\frac {v^4} {g^2\ d^2}+ \Bigg( \textcolor{color1}{ \Big(\frac {v^2} {-g\ d^2} \Big)^2 \times \frac {2\ g\ d^2} {v^2} \bigg( \frac {-g\ d^2} {2\ v^2} -h_t + l+ h_r \bigg) } }\ \ \Bigg) \tag{12}$$ $$tan \theta = \frac {v^2} {g\ d} \pm \sqrt{\frac {v^4} {g^2\ d^2}+\Bigg( \textcolor{color1}{ \frac {2\ v^2} {g\ d^2} \bigg( \frac {-g\ d^2} {2\ v^2} -h_t + l+ h_r \bigg) } }\ \ \Bigg) \tag{13}$$ $$tan \theta = \frac {v^2} {g\ d} \pm \sqrt{ \frac {v^4} {g^2\ d^2} + \textcolor{color1}{ \frac {2\ v^2} {g\ d^2} \bigg( -h_t + l+ h_r\bigg) -1 } }\ \ \tag{14}$$ $$tan \theta = \frac {v^2} {g\ d} \pm \sqrt{ \frac {v^2} {g\ d^2} \bigg( \frac{v^2}{g} - 2 \ h_t + 2 \ l+ h_r \bigg) -1 }\ \ \tag{15}$$

Now that this problem is simplified, I must take $$arctan$$ of both sides and I have my answer in simplest form.

$$\enclose{box}{ \textcolor{answer} { \theta = arctan \Bigg( \frac {v^2} {g\ d} \pm \sqrt{ \frac {v^2} {g\ d^2} \bigg( \frac{v^2}{g} + 2 \ h_t - 2 \ l+ h_r \bigg) -1 }\ \ \Bigg) } } \tag{16}$$

This is the calculation for the vertical angle theta. Recall that equation $$(3)$$ led to fourth degree polynomial and I used some estimates to get to equation $$(4)$$. We now use algorithm to get closer to our angle.

After the first calculation of theta, I can determine the new vertical offset of $$h_t - l \ \sin \theta - h_r$$ and the horizontal offset $$d_o - l \ cos \theta$$ and then recalculate theta. We repeat this process until the change between each iteration of theta yields to a small difference. In below pseudocode we look for a 0.5 degree difference.

	theta = 90
do {
h = L * sin( theta )                // ReCalc height
d = initial_d - L * cos( theta )    // ReCalc distance
lastTheta = theta
theta = ReCalc( d, h )
} while ( AbsoluteValue(theta - lastTheta) >0.5 )