Raised pendulum state space control

[davidmuchow]

Raised Pendulum State Space

The team has decided to use a raised pendulum of fixed length, which means that calculating positions is extremely easy. There are a couple of factors involved with determining the correct positioning for a piece, based on an end effector:

• The robot's position in X and Y and theta
• The angle of the pendulum.

These can, once again, be expressed in a matrix:

$$\Large \begin{bmatrix} R{x} \ R{y} \ R{\theta} \ A{\theta} \ \end{bmatrix} $$

First, we want to calculate the distance the robot has to be away from the end effector, or the X value. Let's say that $\Large E$ is the end effector.

$$\Large E = \begin{bmatrix} E{x} \ E{y} \ E_z\ \end{bmatrix} $$

With knowing $E$ and the fixed length of the arm, we can determine the distance the robot has to be for the arm to be in the right position when it as at the correct angle. To do that, we just derive pythagorean to find $\Large b$ instead of $\Large c$, which in this case represents the distance of the robot from the end affector. The following picture expresses what I mean.

$$\Large b = \sqrt{c^2 - a^2}$$

With knowing that, we can add $\Large b$ to the end effector's $\Large x$ to get $R_x$. $R_y$ can also be determined simply as the end effector's y, because that can represent the side-to-side alignment, which has to be the same to accurately place.

$$\Large R_{x} = b + E_x$$

$$\Large R_{y} = E_y$$

There are two more values: The $\Large R{\theta}$ and the $\Large A{\theta}$. $\Large R{\theta}$ is actually very simple. No matter what, if X+ is defined as the robot facing down the field, then the robot facing forward ALWAYS is the correct orientation. That orientation, expressed in radians is $\Large 0$. $\Large A{\theta}$ is simply equal to $\Large sin^{-1}(E_{z} - P_h / Al)$ with $\Large E{z}$ being the end effector z, $\Large P_h$ as the pendulum's height from the ground to the center of the hex shaft, and $\Large A_l$ as the length of the pendulum. Finally, the outcome is clamped between $\Large -\pi\over{2}$ radian as the lower limit and $\Large 2\pi\over{9}$ radians as the upper limit. The upper limit is found with this equation, all units expressed in inches: $$\Large sin^{-1}({{78 - P_h}\over{P_l}})$$

---

Recap

To recap, this is essentially all you have to calculate to figure out all the portions of this problem:

$$\Large \begin{bmatrix} R{x} \ R{y} \ R{\theta} \ A{\theta} \ \end{bmatrix} = \begin{bmatrix} E{x} + \sqrt{c^2 - a^2} \ E{y} \ 0 \ sin^{-1}({{-Ph + E{z}}\over{A_l}}) \ \end{bmatrix} $$

[davidmuchow]

done in code, just needs to be tested :)