advertisement

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 ﬁeld, and avoids the need for the robot to be in a speciﬁc 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 speciﬁc 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 ﬁrst equa on comes from using the ﬁrst kinema c equa on in the ver cal component of mo on, as the ﬁnal 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 ﬂight 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 ﬁrst 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 ﬁnd 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 ﬂywheel 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 ﬂywheel 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 ﬂywheel 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 conﬁrm that they would indeed work, we used a desmos graph, originally created by Jose and later modiﬁed 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 ﬂywheel 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 ﬁgure 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 ﬁne 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