package Ġorg . first insp ires . ft c . team code . op modes Ċ Ċ import Ġcom . qual comm . robot core . event loop . op mode . Tele Op Ċ import Ġorg . first insp ires . ft c . team code . library . functions . Math Operations . range Buffer Ċ import Ġorg . first insp ires . ft c . team code . library . functions . Math Operations . range Clip Ċ import Ġorg . first insp ires . ft c . team code . library . functions . to Degrees Ċ import Ġorg . first insp ires . ft c . team code . library . functions . to Radians Ċ import Ġorg . first insp ires . ft c . team code . library . functions . truncate Ċ import Ġorg . first insp ires . ft c . team code . library . robot . systems . IM UD rive train Controller Kotlin Ċ Ċ @ Tele Op ( name Ġ= Ġ" World ĠTele op Ġ+ ĠI MU ", Ġgroup Ġ= Ġ" World ") Ċ class ĠWorld Tele op IM U Ġ: ĠWorld Tele op () Ġ{ ĊĊĠĠĠ Ġvar Ġuse IM U Ġ= Ġtrue ĊĠĠĠ Ġlateinit Ġvar Ġim u Controller : ĠIM UD rive train Controller Kotlin ĊĠĠĠ Ġvar Ġzero Angle Ġ= Ġ 0 . 0 ĊĊĠĠĠ Ġoverride Ġfun Ġinit () Ġ{ ĊĠĠĠĠĠĠĠ Ġsuper . init () ĊĠĠĠĠĠĠĠ Ġim u Controller Ġ= ĠIM UD rive train Controller Kotlin ( robot . imu A , Ġrobot . imu B , Ġrobot . hol onom ic , Ġtelemetry ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġcontrol Drive train () Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ( gamepad 1 . a Ġor Ġgame pad 1 . b ) Ġuse IM U Ġ= Ġfalse ĊĠĠĠĠĠĠĠ Ġelse Ġif Ġ( gamepad 1 . x ) Ġuse IM U Ġ= Ġtrue ĊĊĠĠĠĠĠĠĠ Ġif Ġ( use IM U ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . addData (" D riving ĠMode ", Ġ" IM U ") ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( gamepad 1 . x ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġzero Angle Ġ= Ġim u Controller . heading A . toDouble (). to Radians () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġx Target Ġ= Ġ Ġgame pad 1 . left _ stick _ x . toDouble (). process Speed (). process Direction (). run Ġ{ Ġif Ġ( this Ġ== Ġ 0 . 0 ) Ġ 0 . 0 0 0 0 0 1 Ġelse Ġthis Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġy Target Ġ= Ġ- gamepad 1 . left _ stick _ y . toDouble (). process Speed (). process Direction () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġr : ĠDouble ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġtheta : ĠDouble ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġaxis Conversion Angle Ġ= ĠMath . PI Ġ/ Ġ 4 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġx Prime : ĠDouble ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġy Prime : ĠDouble ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcalculate Ġr ĊĠĠĠĠĠĠĠĠĠĠĠ Ġr Ġ= ĠMath . sqrt ( Math . pow ( x Target , Ġ 2 . 0 ) Ġ+ ĠMath . pow ( y Target , Ġ 2 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcalculate Ġtheta ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtheta Ġ= ĠMath . atan ( y Target Ġ/ Ġx Target ) Ġ- ĠMath . PI Ġ/ Ġ 2 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( x Target Ġ< Ġ 0 ) Ġtheta Ġ+= ĠMath . PI ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtheta Ġ+= Ġ( zero Angle Ġ- Ġim u Controller . heading A . toDouble (). to Radians ()) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcalculate Ġx Ġand Ġy Ġprime ĊĠĠĠĠĠĠĠĠĠĠĠ Ġx Prime Ġ= Ġr Ġ* ĠMath . cos ( theta Ġ- Ġaxis Conversion Angle ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġy Prime Ġ= Ġr Ġ* ĠMath . sin ( theta Ġ- Ġaxis Conversion Angle ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġtelemetry ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . add Line (). addData (" heading ", Ġim u Controller . heading A . toDouble (). truncate ( 2 )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . add Line (). addData ("\" zero \" Ġangle ", Ġzero Angle . to Degrees (). truncate ( 2 )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . add Line (). addData (" x Ġinput ", Ġx Target . truncate ( 2 )). addData (" y Ġinput ", Ġy Target . truncate ( 2 )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . add Line (). addData (" r ", Ġr . truncate ( 2 )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . add Line (). addData (" theta ", Ġtheta . to Degrees (). truncate ( 2 )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . add Line (). addData (" x Ġprime ", Ġx Prime . truncate ( 2 )). addData (" y Ġprime ", Ġy Prime . truncate ( 2 )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrobot . hol onom ic . run Without Encoder Prime ( x Prime , Ġy Prime , Ġgame pad 1 . right _ stick _ x . toDouble (). process Speed ()) ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġsuper . control Drive train () ĊĠĠĠ Ġ} Ċ }