package Ġca . warp 7 . f rc 2 0 1 9 . sub systems Ċ Ċ import Ġca . warp 7 . f rc 2 0 1 9 . constants . Field Constants Ċ import Ġca . warp 7 . f rc 2 0 1 9 . constants . H atch Cargo Ċ import Ġca . warp 7 . f rc 2 0 1 9 . constants . Lift Constants Ċ import Ġca . warp 7 . f rc 2 0 1 9 . io . Base IO Ċ import Ġca . warp 7 . f rc 2 0 1 9 . io . io Instance Ċ import Ġcom . ct re . phoenix . motor control . Control Mode Ċ import Ġkotlin . math . with Sign Ċ Ċ object ĠLift Ġ{ ĊĊĠĠĠ Ġprivate Ġval Ġio : ĠBase IO Ġ= Ġio Instance () ĊĊĠĠĠ Ġvar Ġse tpoint Level Ġ= Ġ 0 ĊĠĠĠ Ġvar Ġse tpoint Type Ġ= ĠH atch Cargo . H atch ĊĊĠĠĠ Ġfun Ġget Cool Set point (): ĠDouble Ġ= Ġwhen Ġ( set point Level ) Ġ{ ĊĠĠĠĠĠĠĠĠ 0 Ġ-> Ġwhen Ġ( set point Type ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ ĠH atch Cargo . H atch Ġ-> ĠLift Constants . k Home Height In ches Ġ+ Ġ 3 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ ĠH atch Cargo . Cargo Ġ-> ĠField Constants . k Cargo 1 Height ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠ 1 Ġ-> Ġwhen Ġ( set point Type ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ ĠH atch Cargo . H atch Ġ-> ĠField Constants . k H atch 2 Height Ġ+ Ġ 3 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ ĠH atch Cargo . Cargo Ġ-> ĠField Constants . k Cargo 2 Height ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠ 2 Ġ-> Ġwhen Ġ( set point Type ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ ĠH atch Cargo . H atch Ġ-> ĠField Constants . k H atch 3 Height Ġ+ Ġ 3 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ ĠH atch Cargo . Cargo Ġ-> ĠField Constants . k Cargo 3 Height ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġelse Ġ-> ĠLift Constants . k Home Height In ches ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġincrease Set point () Ġ{ ĊĠĠĠĠĠĠĠ Ġse tpoint Level Ġ= Ġ( set point Level Ġ+ Ġ 1 ). coerce At Most ( 2 ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġdecrease Set point () Ġ{ ĊĠĠĠĠĠĠĠ Ġse tpoint Level Ġ= Ġ( set point Level Ġ- Ġ 1 ). coerce At Least ( 0 ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġvar Ġnominal Zero Ġ= Ġ 0 ĊĠĠĠ Ġvar Ġfeed forward Enabled Ġ= Ġtrue ĊĠĠĠ Ġvar Ġis Manual Ġ= Ġfalse ĊĠĠĠ Ġvar Ġmanual Speed Ġ= Ġ 0 . 0 ĊĠĠĠ Ġvar Ġse tpoint In ches Ġ= Ġ 0 . 0 ĊĊĠĠĠ Ġval Ġadjusted Position Ticks Ġget () Ġ= Ġio . lift Position Ġ- Ġnominal Zero ĊĠĠĠ Ġval Ġheight Ġget () Ġ= Ġadjusted Position Ticks Ġ/ ĠLift Constants . k Ticks Per In ch ĊĊĠĠĠ Ġfun Ġset Feed forward () Ġ{ ĊĠĠĠĠĠĠĠ Ġio . lift Feed forward Ġ= Ġif Ġ( feed forward Enabled ) ĠLift Constants . k Primary Feed forward Ġelse Ġ 0 . 0 ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġupdate Manual Control () Ġ{ ĊĠĠĠĠĠĠĠ Ġio . lift Control Mode Ġ= ĠControl Mode . Percent Output ĊĠĠĠĠĠĠĠ Ġio . lift Demand Ġ= Ġ( manual Speed Ġ* Ġmanual Speed ). with Sign ( manual Speed ) ĊĠĠĠĠĠĠĠ Ġset Feed forward () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġupdate Position Control () Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ( io . hall Effect Triggered ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnominal Zero Ġ= Ġio . lift Position ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġif Ġ( set point In ches Ġ< ĠLift Constants . k PID Dead Spot Height ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ|| Ġheight Ġ< ĠLift Constants . k PID Dead Spot Height ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ|| Ġio . hall Effect Triggered ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġio . lift Control Mode Ġ= ĠControl Mode . Position ĊĠĠĠĠĠĠĠĠĠĠĠ Ġio . lift Demand Ġ= Ġ-( set point In ches Ġ* ĠLift Constants . k Ticks Per In ch ) Ġ+ Ġnominal Zero ĊĠĠĠĠĠĠĠĠĠĠĠ Ġset Feed forward () ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġio . lift Control Mode Ġ= ĠControl Mode . Percent Output ĊĠĠĠĠĠĠĠĠĠĠĠ Ġio . lift Demand Ġ= ĠLift Constants . k MoveTo Bottom Demand ĊĠĠĠĠĠĠĠĠĠĠĠ Ġio . lift Feed forward Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} Ċ }