3/25/2021
Derive And Translate Trajectory Calculations Into Code – Iron Reign Robotics –
Iron Reign Robotics
Blog
About
Tags
Sponsorship
Stats
Sign Up
Archived Posts
Please help support our team! $25 buys a motor, $50 buys a new battery, $150 adds
controllers and sensors, $500 pays tournament fees, $750 upgrades our drivetrain
https://ironreignrobotics.org/2021-01-29-derive-and-translate-trajectory-calculations-into-code/
1/7
3/25/2021
Derive And Translate Trajectory Calculations Into Code – Iron Reign Robotics –
Derive And Translate Trajectory Calculations
Into Code
Tags: think and control
Personhours: 40
By Mahesh, Cooper, Shawn, Ben, Bhanaviya, and Jose
Task: Derive And Translate Trajectory Calculations Into Code
Control
To ease the work put on the drivers, we wanted to have the robot automa cally shoot into the
goal. This would improve cycle mes by allowing the drivers, theore cally, to shoot from any
loca on on the field, and avoids the need for the robot to be in a specific loca on each me it
shoots, elimina ng the me needed to drive and align to a loca on a er intaking disks.
To be able to have the robot automa cally shoot, we needed to derive the equa ons necessary to
get the desired θ (angle of launch) and v (ini al velocity) values. This would be done given a few
0
constants, the height of travel (the height of the goal - the height of the robot, which we will call h
https://ironreignrobotics.org/2021-01-29-derive-and-translate-trajectory-calculations-into-code/
2/7
3/25/2021
Derive And Translate Trajectory Calculations Into Code – Iron Reign Robotics –
), the distance between the robot and the goal (which we will call d), and of course accelera on
due to gravity, g (approximately 9.8
m
2
s
). 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 θ and v .
0
However, without any constraints mul ple solu ons exist, which is why we created a constraint to
both limit the number of solu ons 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 ver cal velocity component is 0 when it enters the
goal. This way there exists only one θ and v that can launch the disk into the goal.
0
We start by outlining the basic kinema c equa ons which model any object's trajectory under
constant accelera on:
v = v0 + at
2
v
2
= v0 + 2aΔx
1
Δx = x0 + v0 t +
2
at
2
Think
When plugging in the constants, considering the constraints men oned before into these
equa ons, accoun ng for both the horizontal and ver cal components of mo on, we get the
following equa ons:
0 = v0 sin(θ) − gt
0
2
2
= (v0 sin(θ))
− 2gh
d = v0 cos(θ)t
The first equa on comes from using the first kinema c equa on in the ver cal component of
mo on, as the final velocity of the disk should be 0 according to the constraints, −g acts as the
accelera on, and v
0
sin(θ)
represents the ver cal component of the launch velocity. The second
equa on comes from using the second kinema cs equa on again in the ver cal component of
mo on, and again according to the constraints, −g acts as the accelera on, h represents the
distance travelled ver cally by the disk, and v
0
sin(θ)
represents the ver cal component of the
launch velocity. The last equa on comes from applying the third kinema cs equa on in the
horizontal component of mo on, with d being the distance travelled by the disk horizontally,
v0 cos(θ)
represen ng the horizontal component of the launch velocity, and t represen ng the
flight me of the disk.
https://ironreignrobotics.org/2021-01-29-derive-and-translate-trajectory-calculations-into-code/
3/7
3/25/2021
Derive And Translate Trajectory Calculations Into Code – Iron Reign Robotics –
Solving for v
0
sin(θ)
in the first equa on and subs tu ng this for v
0
sin(θ)
in the second equa on
gives:
v0 sin(θ) = gt
0
2
2
= (gt)
2h
− 2gh, t = √
g
Now that an equa on is derived for t in terms of known values, we can treat t as a
constant/known value and con nue.
Using pythagorean theorem, we can find the ini al velocity of launch v . v
0
0 cos(θ)
and v
0 sin(θ)
can be treated as two legs of a right triangle, with v being the hypotenuse. Therefore
0
2
2
v0 = √ (v0 cos(θ)) + (v0 sin(θ))
, so:
v0 sin(θ) = gt
d
v0 cos(θ) =
v0
t
2
= √ (gt) + (
2
d
)
t
Now that v has been solved for, θ can be solved for using sin
−1
0
−1
θ = sin
(
v0 sin(θ)
−1
) = sin
like so:
gt
(
v0
)
v
In order to be prac cally useful, the v previously found must be converted into a cks per second
0
value for the flywheel motor to maintain in order to have a tangen al velocity equal to v . In other
0
words, a linear velocity must be converted into an angular velocity. This can be done using the
following equa on, where v = tangen al velocity, ω = angular velocity, and r = radius.
v
v = ωr, ω =
r
The radius of the flywheel r can be considered a constant, and v is subs tuted for the v solved
0
for previously.
However, this value for ω is in
radians
second
, but has to be converted to
encoder ticks
second
to be usable. This
can be done easily with the following expression:
ω radians
1 revolution
⋅
1 second
The last
3 encoder ticks
1 encoder tick
20 encoder ticks per revolution
⋅
2π radians
3 encoder ticks
⋅
1 revolution
1 encoder tick
comes from a 3 : 1 gearbox placed on top of the flywheel motor.
https://ironreignrobotics.org/2021-01-29-derive-and-translate-trajectory-calculations-into-code/
4/7
3/25/2021
Derive And Translate Trajectory Calculations Into Code – Iron Reign Robotics –
Think
To sanity check these calcula ons and confirm that they would indeed work, we used a desmos
graph, originally created by Jose and later modified with the updated calcula ons, to take in the
constants used previously and graph out the parabola of a disk's trajectory. The link to the desmos
is h ps://www.desmos.com/calculator/zuoa50ilmz, and the image below shows an example of a
disk's trajectory.
To translate these calcula ons into code, we created a class named TrajectoryCalculator ,
originally created by Shawn and later refactored to include the updated calcula ons. To hold both
an angle and velocity solu on, 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.LAUNC
// time the disk is in air in seconds
double flightTime = Math.sqrt((2 * travelHeight) / Constants.
// using pythagorean theorem to find magnitude of muzzle velo
double horizontalVelocity = distance / flightTime;
double verticalVelocity = Constants.GRAVITY * flightTime;
double velocity = Math.sqrt(Math.pow(horizontalVelocity, 2) +
https://ironreignrobotics.org/2021-01-29-derive-and-translate-trajectory-calculations-into-code/
5/7
3/25/2021
Derive And Translate Trajectory Calculations Into Code – Iron Reign Robotics –
// converting tangential velocity in m/s into angular velocity
double angularVelocity = velocity / Constants.FLYWHEEL_RADIUS
angularVelocity *= (Constants.ENCODER_TICKS_PER_REVOLUTION * C
double theta = Math.asin((Constants.GRAVITY * flightTime) / v
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:
Control
The next step is to use PID control to maintain target veloci es and angles. The calculated angular
velocity ω can be set as the target value of a PID controller in order to accurately have the
flywheel motor hold the required ω. The target angle of launch above the horizontal θ can easily
be converted into encoder cks, which can be used again in conjunc on with a PID controller to
have the elbow motor maintain a posi on.
Another important step is to of course figure out how d would be measured. Experimenta on with
vuforia and/or distance sensors is necessary to have a required input to the trajectory calcula ons.
From there, it's a ma er of fine tuning values and correc ng any errors in the system.
Date | January 29, 2021
https://ironreignrobotics.org/2021-01-29-derive-and-translate-trajectory-calculations-into-code/
6/7
3/25/2021
Derive And Translate Trajectory Calculations Into Code – Iron Reign Robotics –
https://ironreignrobotics.org/2021-01-29-derive-and-translate-trajectory-calculations-into-code/
7/7