package Ġorg . team 5 4 1 9 . fault . trajectory . followers Ċ Ċ import Ġorg . team 5 4 1 9 . fault . trajectory . Trajectory Iterator Ċ import Ġorg . team 5 4 1 9 . fault . trajectory . types . Timed Entry Ċ import Ġorg . team 5 4 1 9 . fault . math . geometry . Pose 2 d With Cur v ature Ċ import Ġorg . team 5 4 1 9 . fault . math . geometry . Pose 2 d Ċ Ċ import Ġorg . team 5 4 1 9 . fault . math . epsilon Equals Ċ import Ġorg . team 5 4 1 9 . fault . math . units . SI Unit Ċ import Ġorg . team 5 4 1 9 . fault . math . units . Second Ċ import Ġorg . team 5 4 1 9 . fault . math . units . operations . times Ċ import Ġorg . team 5 4 1 9 . fault . math . units . operations . div Ċ Ċ import Ġkotlin . math . sqrt Ċ import Ġkotlin . math . sin Ċ Ċ class ĠRam set e Follower ( ĊĠĠĠ Ġprivate Ġval ĠkB eta : ĠDouble , ĊĠĠĠ Ġprivate Ġval Ġk Z eta : ĠDouble Ċ ) Ġ: ĠTra jectory Follower () Ġ{ ĊĊĠĠĠ Ġoverride Ġfun Ġcalculate State ( ĊĠĠĠĠĠĠĠ Ġiterator : ĠTra jectory Iterator < SI Unit < Second >, ĠTime d Entry < Pose 2 d With Cur v ature >>, ĊĠĠĠĠĠĠĠ Ġrobot Pose : ĠP ose 2 d ĊĠĠĠ Ġ): ĠTra jectory Follower . Trajectory Follower Velocity Output Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġreference State Ġ= Ġiterator . currentState . state ĊĊĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġgoal Ġin Ġrobot 's Ġcoordinates ĊĠĠĠĠĠĠĠ Ġval Ġerror Ġ= Ġreference State . state . pose Ġin Frame Of Reference Of Ġrobot Pose ĊĊĠĠĠĠĠĠĠ Ġ// ĠGet Ġreference Ġlinear Ġand Ġangular Ġve locities ĊĠĠĠĠĠĠĠ Ġval Ġvd Ġ= Ġreference State . velocity . value ĊĠĠĠĠĠĠĠ Ġval Ġwd Ġ= Ġvd Ġ* Ġreference State . state . cur v ature ĊĊĠĠĠĠĠĠĠ Ġ// ĠCompute Ġgain ĊĠĠĠĠĠĠĠ Ġval Ġk 1 Ġ= Ġ 2 Ġ* Ġk Z eta Ġ* Ġsqrt ( wd Ġ* Ġwd Ġ+ ĠkB eta Ġ* Ġvd Ġ* Ġvd ) ĊĊĠĠĠĠĠĠĠ Ġ// ĠGet Ġangular Ġerror Ġin Ġbounded Ġradians ĊĠĠĠĠĠĠĠ Ġval Ġangle Error Ġ= Ġerror . rotation . rad ian ĊĊĠĠĠĠĠĠĠ Ġreturn ĠTra jectory Follower . Trajectory Follower Velocity Output ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Velocity Ġ= ĠSI Unit ( vd Ġ* Ġerror . rotation . cos Ġ+ Ġk 1 Ġ* Ġerror . translation . x . value ), ĊĠĠĠĠĠĠĠĠĠĠĠ Ġangular Velocity Ġ= ĠSI Unit ( wd Ġ+ ĠkB eta Ġ* Ġvd Ġ* Ġs inc ( angle Error . value ) Ġ* ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġerror . translation . y . value Ġ+ Ġk 1 Ġ* Ġangle Error . value ) ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġcompanion Ġobject Ġ{ ĊĠĠĠĠĠĠĠ Ġprivate Ġfun Ġs inc ( theta : ĠDouble ) Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( theta Ġepsilon Equals Ġ 0 . 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ 1 . 0 Ġ- Ġ 1 . 0 Ġ/ Ġ 6 . 0 Ġ* Ġtheta Ġ* Ġtheta ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġsin ( theta ) Ġ/ Ġtheta ĊĠĠĠ Ġ} Ċ } Ċ