package Ġf rc . robot . commands Ċ Ċ import Ġedu . wpi . first . wp ilib j . controller . PID Controller Ċ import Ġedu . wpi . first . wp ilib j 2 . command . PID Command Ċ import Ġf rc . robot . Constants Ċ import Ġf rc . robot . sub systems . Drive train Subsystem Ċ Ċ class ĠTurn To Angle ( val Ġdrive Subsystem : ĠDrive train Subsystem , Ġtarget Angle : Ġ() Ġ-> ĠDouble , Ġforward Speed : ĠDouble ) Ġ: ĠPID Command ( ĊĠĠĠ ĠPID Controller ( ĊĠĠĠĠĠĠĠ ĠConstants . constants [" Drive train PID _ P "] Ġ?: Ġ 0 . 0 3 5 , ĊĠĠĠĠĠĠĠ ĠConstants . constants [" Drive train PID _ I "] Ġ?: Ġ 0 . 0 , ĊĠĠĠĠĠĠĠ ĠConstants . constants [" Drive train PID _ D "] Ġ?: Ġ 0 . 0 0 5 ĊĠĠĠ Ġ), ĊĠĠĠ Ġdrive Subsystem :: get Angle , ĊĠĠĠ Ġtarget Angle , ĊĠĠĠ Ġ{ Ġoutput : ĠDouble Ġ-> Ġdrive Subsystem . drive Arc ade ( forward Speed , Ġoutput ) Ġ}, ĊĠĠĠ ĠarrayOf ( drive Subsystem )) Ġ{ ĊĊĠĠĠ Ġval Ġinitial Angle : ĠDouble ĊĊĠĠĠ Ġinit Ġ{ ĊĠĠĠĠĠĠĠ Ġinitial Angle Ġ= Ġdrive Subsystem . get Angle () ĊĠĠĠĠĠĠĠ Ġcontroller . enable Continuous Input (- 1 8 0 . 0 , Ġ 1 8 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġ/** Ġreload Ġpid Ġparameters Ġfrom Ġnetwork Ġtables Ġ*/ ĊĠĠĠĠĠĠĠ Ġset PID Params () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġset PID Params () Ġ{ ĊĠĠĠĠĠĠĠ Ġcontroller . set Tolerance ( ĊĠĠĠĠĠĠĠĠĠĠĠ ĠConstants . constants [" Drive train PID _ Angle Tolerance Deg "] Ġ?: Ġ 2 . 0 , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠConstants . constants [" Drive train PID _ Angle Rate Tolerance Deg Per S "] Ġ?: Ġ 1 . 0 ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠ Ġcontroller . set PID ( ĊĠĠĠĠĠĠĠĠĠĠĠ ĠConstants . constants [" Drive train PID _ P "] Ġ?: Ġ 0 . 0 3 5 , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠConstants . constants [" Drive train PID _ I "] Ġ?: Ġ 0 . 0 , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠConstants . constants [" Drive train PID _ D "] Ġ?: Ġ 0 . 0 0 5 ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} ĊĊĊĠĠĠ Ġoverride Ġfun Ġis Finished (): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġ/* Ġif Ġno Ġgyro , Ġfail Ġ*/ ĊĠĠĠĠĠĠĠ Ġif Ġ(! drive Subsystem . gyro Up ()) Ġreturn Ġtrue ĊĠĠĠĠĠĠĠ Ġ/* Ġcheck Ġif Ġwe Ġhit Ġse tpoint Ġyet Ġ*/ ĊĠĠĠĠĠĠĠ Ġreturn Ġcontroller . at Set point () ĊĠĠĠ Ġ} Ċ Ċ }