29 Jan 2021
Derive And Translate Trajectory Calculations Into Code
By Mahesh, Cooper, Shawn, Ben, Bhanaviya, and Jose
Task: Derive And Translate Trajectory Calculations Into Code
To ease the work put on the drivers, we wanted to have the robot
automatically shoot into the goal. This would improve cycle times by
allowing the drivers, theoretically, to shoot from any location on the
field, and avoids the need for the robot to be in a specific location each
time it shoots, eliminating the time needed to drive and align to a
location after intaking disks.
To be able to have the robot automatically shoot, we needed to derive the
equations necessary to get the desired \(\theta\) (angle of launch) and
\(v_0\) (initial velocity) values. This would be done given a few
constants, the height of travel (the height of the goal - the height of
the robot, which we will call \(h\)), the distance between the robot and
the goal (which we will call \(d\)), and of course acceleration due to
gravity, \(g\) (approximately \(9.8 \frac{m}{s^2}\)). \(d\) would be given
through either a distance sensor, or using vuforia. Either way, we can
assume this value is constant for a specific trajectory. Given these
values, we can calculate \(\theta\) and \(v_0 \).
However, without any constraints multiple solutions exist, which is why we
created a constraint to both limit the number of solutions and reduce the
margin of error of the disk's trajectory near the goal. We added the
constraint the disk should reach the summit (or apex) of its trajectory at
the point at which it enters the goal, or that it's vertical velocity
component is \(0\) when it enters the goal. This way there exists only one
\(\theta\) and \(v_0\) that can launch the disk into the goal.
We start by outlining the basic kinematic equations which model any
object's trajectory under constant acceleration:
\[\displaylines{v = v_0 + at \\ v^2 = v_0^2 + 2a\Delta x \\ \Delta x =
x_0 + v_0t + \frac{1}{2}at^2}\]
When plugging in the constants, considering the constraints mentioned
before into these equations, accounting for both the horizontal and
vertical components of motion, we get the following equations:
\[\displaylines{0 = v_0sin(\theta) - gt \\ 0^2 = (v_0sin(\theta))^2 -
2gh \\ d = v_0cos(\theta)t}\]
The first equation comes from using the first kinematic equation in the
vertical component of motion, as the final velocity of the disk should be
\(0\) according to the constraints, \(-g\) acts as the acceleration, and
\(v_0sin(\theta)\) represents the vertical component of the launch
velocity. The second equation comes from using the second kinematics
equation again in the vertical component of motion, and again according to
the constraints, \(-g\) acts as the acceleration, \(h\) represents the
distance travelled vertically by the disk, and \(v_0sin(\theta)\)
represents the vertical component of the launch velocity. The last
equation comes from applying the third kinematics equation in the
horizontal component of motion, with \(d\) being the distance travelled by
the disk horizontally, \(v_0cos(\theta)\) representing the horizontal
component of the launch velocity, and \(t\) representing the flight time
of the disk.
Solving for \(v_0sin(\theta)\) in the first equation and substituting this
for \(v_0sin(\theta)\) in the second equation gives:
\[\displaylines{v_0sin(\theta) = gt \\ 0^2 = (gt)^2 - 2gh, t =
\sqrt{\frac{2h}{g}}}\]
Now that an equation is derived for \(t\) in terms of known values, we can
treat t as a constant/known value and continue.
Using pythagorean theorem, we can find the initial velocity of launch
\(v_0\). \(v_0cos(\theta)\) and \(v_0sin(\theta)\) can be treated as two
legs of a right triangle, with \(v_0\) being the hypotenuse. Therefore
\(v_0 = \sqrt{(v_0cos(\theta))^2 + (v_0sin(\theta))^2}\), so:
\[\displaylines{v_0sin(\theta) = gt \\ v_0cos(\theta) = \frac{d}{t} \\
v_0 = \sqrt{(gt)^2 + {\left( \frac{d}{t}\right) ^2}}}\]
Now that \(v_0\) has been solved for, \(\theta\) can be solved for using
\(sin^{-1}\) like so:
\[\displaylines{\theta = sin^{-1}\left( \frac{v_0sin(\theta)}{v_0}
\right) = sin^{-1}\left( \frac{gt}{v}\right) }\]
In order to be practically useful, the \(v_0\) previously found must be
converted into a ticks per second value for the flywheel motor to maintain
in order to have a tangential velocity equal to \(v_0\). In other words, a
linear velocity must be converted into an angular velocity. This can be
done using the following equation, where \(v\) = tangential velocity,
\(\omega\) = angular velocity, and \(r\) = radius.
\[\displaylines{v = \omega r, \omega = \frac{v}{r} \\ }\]
The radius of the flywheel \(r\) can be considered a constant, and \(v\)
is substituted for the \(v_0\) solved for previously.
However, this value for \(\omega\) is in \(\frac{radians}{second}\), but
has to be converted to \(\frac{encoder \space ticks}{second}\) to be
usable. This can be done easily with the following expression:
\[\displaylines{\frac{\omega \space radians}{1 \space second} \cdot
\frac{1 \space revolution}{2\pi \space radians} \cdot \frac{20 \space
encoder \space ticks \space per \space revolution}{1 \space revolution}
\cdot \frac{3 \space encoder \space ticks}{1 \space encoder \space
tick}}\]
The last \(\frac{3 \space encoder \space ticks}{1 \space encoder \space
tick}\) comes from a \(3:1\) gearbox placed on top of the flywheel motor.
To sanity check these calculations and confirm that they would indeed
work, we used a desmos graph, originally created by Jose and later
modified with the updated calculations, to take in the constants used
previously and graph out the parabola of a disk's trajectory. The link to
the desmos is
https://www.desmos.com/calculator/zuoa50ilmz, and the image below shows an example of a disk's trajectory.
To translate these calculations into code, we created a class named
TrajectoryCalculator, originally created by Shawn and later
refactored to include the updated calculations. To hold both an angle and
velocity solution, we created a simple class, or struct, named
TrajectorySolution. Both classes are shown below.
public class TrajectoryCalculator {
private double distance;
public TrajectoryCalculator(double distance) {
this.distance = distance;
}
public TrajectorySolution getTrajectorySolution() {
// vertical distance in meters the disk has to travel
double travelHeight = Constants.GOAL_HEIGHT - Constants.LAUNCH_HEIGHT;
// time the disk is in air in seconds
double flightTime = Math.sqrt((2 * travelHeight) / Constants.GRAVITY);
// using pythagorean theorem to find magnitude of muzzle velocity (in m/s)
double horizontalVelocity = distance / flightTime;
double verticalVelocity = Constants.GRAVITY * flightTime;
double velocity = Math.sqrt(Math.pow(horizontalVelocity, 2) + Math.pow(verticalVelocity, 2));
// converting tangential velocity in m/s into angular velocity in ticks/s
double angularVelocity = velocity / Constants.FLYWHEEL_RADIUS; // angular velocity in radians/s
angularVelocity *= (Constants.ENCODER_TICKS_PER_REVOLUTION * Constants.TURRET_GEAR_RATIO) / (2 * Math.PI); // angular velocity in ticks/s
double theta = Math.asin((Constants.GRAVITY * flightTime) / velocity);
return new TrajectorySolution(angularVelocity, theta);
}
}
public class TrajectorySolution {
private double angularVelocity;
private double theta;
public TrajectorySolution(double angularVelocity, double theta) {
this.angularVelocity = angularVelocity;
this.theta = theta;
}
public double getAngularVelocity() {
return angularVelocity;
}
public double getTheta() {
return theta;
}
}
Next Steps:
The next step is to use PID control to maintain target velocities and
angles. The calculated angular velocity \(\omega\) can be set as the
target value of a PID controller in order to accurately have the flywheel
motor hold the required \(\omega\). The target angle of launch above the
horizontal \(\theta\) can easily be converted into encoder ticks, which
can be used again in conjunction with a PID controller to have the elbow
motor maintain a position.
Another important step is to of course figure out how \(d\) would be
measured. Experimentation with vuforia and/or distance sensors is
necessary to have a required input to the trajectory calculations. From
there, it's a matter of fine tuning values and correcting any errors in
the system.
Match 7(Q121)
Finals 2
Finals 2
