package Ġcom . team 4 0 9 9 . robot 2 0 2 2 . sub systems . drive train Ċ Ċ import Ġcom . team 4 0 9 9 . lib . logging . T unable Number Ċ import Ġcom . team 4 0 9 9 . lib . units . Linear Acceleration Ċ import Ġcom . team 4 0 9 9 . lib . units . Linear Velocity Ċ import Ġcom . team 4 0 9 9 . lib . units . base . fe et Ċ import Ġcom . team 4 0 9 9 . lib . units . base . meters Ċ import Ġcom . team 4 0 9 9 . lib . units . derived . Angle Ċ import Ġcom . team 4 0 9 9 . lib . units . derived . degrees Ċ import Ġcom . team 4 0 9 9 . lib . units . derived . in Radians Ċ import Ġcom . team 4 0 9 9 . lib . units . derived . radians Ċ import Ġcom . team 4 0 9 9 . lib . units . in Radians PerSecond Ċ import Ġcom . team 4 0 9 9 . lib . units . in Radians PerSecond PerSecond Ċ import Ġcom . team 4 0 9 9 . lib . units . per Second Ċ import Ġcom . team 4 0 9 9 . robot 2 0 2 2 . config . constants . Drive train Constants Ċ import Ġorg . lit t leton robot ics . junction . Logger Ċ import Ġkotlin . math . IEEE rem Ċ import Ġkotlin . math . with Sign Ċ Ċ class ĠS wer ve Module ( val Ġio : ĠS wer ve Module IO ) Ġ{ ĊĠ Ġval Ġinputs Ġ= ĠS wer ve Module IO . Sw er ve Module IO Inputs () ĊĊĠ Ġprivate Ġvar Ġspeed Set Point : ĠLinear Velocity Ġ= Ġ 0 . fe et . per Second ĊĠ Ġprivate Ġvar Ġacceleration Set Point : ĠLinear Acceleration Ġ= Ġ 0 . fe et . per Second . per Second ĊĊĠ Ġprivate Ġvar Ġste ering Set Point : ĠAngle Ġ= Ġ 0 . degrees ĊĊĠ Ġprivate Ġvar Ġshould Invert Ġ= Ġfalse ĊĊĠ Ġprivate Ġval Ġste ering k P Ġ= ĊĠĠĠ ĠT unable Number (" Drive train / module Ste ering k P ", ĠDrive train Constants . PID . STE ER ING _ KP ) ĊĠ Ġprivate Ġval Ġste ering k I Ġ= ĊĠĠĠ ĠT unable Number (" Drive train / module Ste ering k I ", ĠDrive train Constants . PID . STE ER ING _ KI ) ĊĠ Ġprivate Ġval Ġste ering k D Ġ= ĊĠĠĠ ĠT unable Number (" Drive train / module Ste ering k D ", ĠDrive train Constants . PID . STE ER ING _ KD ) ĊĊĠ Ġprivate Ġval Ġste ering Max Vel Ġ= ĊĠĠĠ ĠT unable Number ( ĊĠĠĠĠĠ Ġ" Drive train / module Ste ering Max Vel Rad Per Sec ", ĊĠĠĠĠĠ ĠDrive train Constants . STE ER ING _ VEL _ MAX . in Radians PerSecond ĊĠĠĠ Ġ) ĊĠ Ġprivate Ġval Ġste ering Max Accel Ġ= ĊĠĠĠ ĠT unable Number ( ĊĠĠĠĠĠ Ġ" Drive train / module Ste ering Max Accel Rad Per Sec Sq ", ĊĠĠĠĠĠ ĠDrive train Constants . STE ER ING _ AC CEL _ MAX . in Radians PerSecond PerSecond ĊĠĠĠ Ġ) ĊĊĠ Ġprivate Ġval Ġdrive k P Ġ= ĠT unable Number (" Drive train / module Drive k P ", ĠDrive train Constants . PID . DRIVE _ KP ) ĊĠ Ġprivate Ġval Ġdrive k I Ġ= ĠT unable Number (" Drive train / module Drive k I ", ĠDrive train Constants . PID . DRIVE _ KI ) ĊĠ Ġprivate Ġval Ġdrive k D Ġ= ĠT unable Number (" Drive train / module Drive k D ", ĠDrive train Constants . PID . DRIVE _ KD ) ĊĊĠ Ġinit Ġ{} ĊĊĠ Ġfun Ġperiodic () Ġ{ ĊĠĠĠ Ġio . update Inputs ( inputs ) ĊĊĠĠĠ Ġif Ġ( ste ering k P . has Changed () Ġ|| Ġste ering k I . has Changed () Ġ|| Ġste ering k D . has Changed ()) Ġ{ ĊĠĠĠĠĠ Ġio . configure Ste ering PID ( ste ering k P . value , Ġste ering k I . value , Ġste ering k D . value ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġif Ġ( ste ering Max Vel . has Changed () Ġ|| Ġste ering Max Accel . has Changed ()) Ġ{ ĊĠĠĠĠĠ Ġio . configure Ste ering Motion Magic ( ĊĠĠĠĠĠĠĠ Ġste ering Max Vel . value . radians . per Second , ĊĠĠĠĠĠĠĠ Ġste ering Max Accel . value . radians . per Second . per Second ĊĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġif Ġ( drive k P . has Changed () Ġ|| Ġdrive k I . has Changed () Ġ|| Ġdrive k D . has Changed ()) Ġ{ ĊĠĠĠĠĠ Ġio . configure Drive PID ( drive k P . value , Ġdrive k I . value , Ġdrive k D . value ) ĊĠĠĠ Ġ} ĊĊĠĠĠ ĠLogger . getInstance (). process Inputs ( io . label , Ġinputs ) ĊĠĠĠ Ġ// ĠĠĠ ĠLogger . getInstance () ĊĠĠĠ Ġ// ĠĠĠĠĠ Ġ. record Output ( ĊĠĠĠ Ġ// ĠĠĠĠĠĠĠ Ġ"${ io . label }/ drive Speed Set point Met ers PerSecond ", ĊĠĠĠ Ġ// ĠĠĠĠĠĠĠ Ġif Ġ(! should Invert ) Ġspeed Set Point . in Met ers PerSecond ĊĠĠĠ Ġ// ĠĠĠĠĠĠĠ Ġelse Ġ- speed Set Point . in Met ers PerSecond ĊĠĠĠ Ġ// ĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ// ĠĠĠ ĠLogger . getInstance () ĊĠĠĠ Ġ// ĠĠĠĠĠ Ġ. record Output ( ĊĠĠĠ Ġ// ĠĠĠĠĠĠĠ Ġ"${ io . label }/ drive Accel Set point Met ers PerSecond Sq ", ĊĠĠĠ Ġ// ĠĠĠĠĠĠĠ Ġacceleration Set Point . in Met ers PerSecond PerSecond ĊĠĠĠ Ġ// ĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ// ĠĠĠ ĠLogger . getInstance () ĊĠĠĠ Ġ// ĠĠĠĠĠ Ġ. record Output ("${ io . label }/ ste ering Set point Degrees ", Ġste ering Set Point . in Degrees ) ĊĠĠĠ Ġ// ĠĠĠ ĠLogger . getInstance () ĊĠĠĠ Ġ// ĠĠĠĠĠ Ġ. record Output ( ĊĠĠĠ Ġ// ĠĠĠĠĠĠĠ Ġ"${ io . label }/ ste ering Value Degrees With Mod ", ĊĠĠĠ Ġ// ĠĠĠĠĠĠĠ Ġinputs . ste ering Position . in Degrees . IEEE rem ( 3 6 0 . 0 ) ĊĠĠĠ Ġ// ĠĠĠĠĠ Ġ) ĊĠ Ġ} ĊĊĠ Ġ/** ĊĠĠ Ġ* ĠSets Ġthe Ġsw er ve Ġmodule Ġto Ġthe Ġspecified Ġangular Ġand ĠX Ġ& ĠY Ġve locities Ġusing Ġfeed Ġforward . ĊĠĠ Ġ* ĊĠĠ Ġ* Ġ@ param Ġste ering ĠThe Ġangular Ġposition Ġdesired Ġfor Ġthe Ġsw er ve Ġmodule Ġto Ġbe Ġset Ġto ĊĠĠ Ġ* Ġ@ param Ġspeed ĠThe Ġspeed Ġdesired Ġfor Ġthe Ġsw er ve Ġmodule Ġto Ġreach ĊĠĠ Ġ* Ġ@ param Ġacceleration ĠThe Ġlinear Ġacceleration Ġused Ġto Ġcalculate Ġhow Ġto Ġreach Ġthe Ġdesired Ġspeed ĊĠĠ Ġ*/ ĊĠ Ġfun Ġset ( ĊĠĠĠ Ġste ering : ĠAngle , ĊĠĠĠ Ġspeed : ĠLinear Velocity , ĊĠĠĠ Ġacceleration : ĠLinear Acceleration Ġ= Ġ 0 . 0 . meters . per Second . per Second , ĊĠĠĠ Ġoptimize : ĠBoolean Ġ= Ġtrue ĊĠ Ġ) Ġ{ ĊĠĠĠ Ġif Ġ( speed Ġ== Ġ 0 . fe et . per Second ) Ġ{ ĊĠĠĠĠĠ Ġio . set Open Loop ( ste ering Set Point , Ġ 0 . 0 ) ĊĠĠĠĠĠ Ġreturn ĊĠĠĠ Ġ} ĊĠĠĠ Ġvar Ġste ering Difference Ġ= ĊĠĠĠĠĠ Ġ( ste ering Ġ- Ġinputs . ste ering Position ). in Radians . IEEE rem ( 2 Ġ* ĠMath . PI ). radians ĊĊĠĠĠ Ġshould Invert Ġ= Ġste ering Difference . absolute Value Ġ> Ġ( Math . PI Ġ/ Ġ 2 ). radians Ġ&& Ġoptimize ĊĠĠĠ Ġif Ġ( should Invert ) Ġ{ ĊĠĠĠĠĠ Ġste ering Difference Ġ-= ĠMath . PI . with Sign ( ste ering Difference . in Radians ). radians ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġspeed Set Point Ġ= ĊĠĠĠĠĠ Ġif Ġ( should Invert ) Ġ{ ĊĠĠĠĠĠĠĠ Ġspeed Ġ* Ġ- 1 ĊĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠ Ġspeed ĊĠĠĠĠĠ Ġ} ĊĠĠĠ Ġacceleration Set Point Ġ= ĊĠĠĠĠĠ Ġif Ġ( should Invert ) Ġ{ ĊĠĠĠĠĠĠĠ Ġacceleration Ġ* Ġ- 1 ĊĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠ Ġacceleration ĊĠĠĠĠĠ Ġ} ĊĠĠĠ Ġste ering Set Point Ġ= Ġinputs . ste ering Position Ġ+ Ġste ering Difference ĊĠĠĠ Ġ// ĠĠĠ Ġdrive F alcon . set ( speed Set Point Ġ/ ĠDrive train Constants . DRIVE _ SET POINT _ MAX ) ĊĊĠĠĠ Ġio . set Closed Loop ( ste ering Set Point , Ġspeed Set Point , Ġacceleration Set Point ) ĊĠ Ġ} ĊĊĠ Ġfun Ġset Open Loop ( ste ering : ĠAngle , Ġspeed : ĠDouble , Ġoptimize : ĠBoolean Ġ= Ġtrue ) Ġ{ ĊĠĠĠ Ġvar Ġste ering Difference Ġ= ĊĠĠĠĠĠ Ġ( ste ering Ġ- Ġinputs . ste ering Position ). in Radians . IEEE rem ( 2 Ġ* ĠMath . PI ). radians ĊĊĠĠĠ Ġshould Invert Ġ= Ġste ering Difference . absolute Value Ġ> Ġ( Math . PI Ġ/ Ġ 2 ). radians Ġ&& Ġoptimize ĊĠĠĠ Ġif Ġ( should Invert ) Ġ{ ĊĠĠĠĠĠ Ġste ering Difference Ġ-= ĠMath . PI . with Sign ( ste ering Difference . in Radians ). radians ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġval Ġoutput Power Ġ= ĊĠĠĠĠĠ Ġif Ġ( should Invert ) Ġ{ ĊĠĠĠĠĠĠĠ Ġspeed Ġ* Ġ- 1 ĊĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠ Ġspeed ĊĠĠĠĠĠ Ġ} ĊĠĠĠ Ġste ering Set Point Ġ= Ġinputs . ste ering Position Ġ+ Ġste ering Difference ĊĠĠĠ Ġio . set Open Loop ( ste ering Set Point , Ġoutput Power ) ĊĠ Ġ} ĊĊĠ Ġ/** ĠCreates Ġevent Ġof Ġthe Ġcurrent Ġpot ent io meter Ġvalue Ġas Ġneeds Ġto Ġbe Ġmanually Ġread just ed . Ġ*/ ĊĠ Ġfun Ġreset Module Zero () Ġ{ ĊĠĠĠ Ġio . reset Module Zero () ĊĠ Ġ} ĊĊĠ Ġ/** ĠZ eros Ġthe Ġste ering Ġmotor Ġ*/ ĊĠ Ġfun Ġzero Ste ering () Ġ{ ĊĠĠĠ Ġio . zero Ste ering () ĊĠ Ġ} ĊĊĠ Ġ/** ĠZ eros Ġthe Ġdrive Ġmotor Ġ*/ ĊĠ Ġfun Ġzero Drive () Ġ{ ĊĠĠĠ Ġio . zero Drive () ĊĠ Ġ} ĊĊĠ Ġfun Ġset Drive Br ake Mode ( br ake : ĠBoolean ) Ġ{ ĊĠĠĠ Ġio . set Br ake Mode ( br ake ) ĊĠ Ġ} Ċ } Ċ