package Ġorg . team 4 0 1 . robot 2 0 2 0 . control . t urret Ċ Ċ import Ġedu . wpi . first . wp ilib j . controller . Simple Motor Feed forward Ċ import Ġedu . wpi . first . wp ilib j . geometry . Rotation 2 d Ċ import Ġedu . wpi . first . wpi util . math . Math Util Ċ import Ġf rc . robot . Constants Ċ import Ġfr ck it . util . Geom Util Ċ import Ġorg . team 4 0 1 . units . measure . acceleration . angular . Measure Radians PerSecond PerSecond Ċ import Ġorg . team 4 0 1 . units . measure . distance . angular . Measure Radians Ċ import Ġorg . team 4 0 1 . units . measure . time . Measure Seconds Ċ import Ġorg . team 4 0 1 . units . measure . velocity . angular . Measure Radians PerSecond Ċ import Ġorg . team 4 0 1 . util . PID Controller Lite Ċ import Ġorg . team 4 0 1 . util . Profile d PID Controller Lite Ċ import Ġorg . team 4 0 1 . util . Tr ape z oid Profile Lite Ċ import Ġorg . team 4 0 1 . robot 2 0 2 0 . util . get Acceleration Ċ import Ġorg . team 4 0 1 . units . measure .* Ċ import Ġjava . util . function . Double Consumer Ċ import Ġjava . util . function . Double Supplier Ċ Ċ /** Ċ Ġ* ĠClass Ġimplementing Ġcontrol Ġlogic Ġfor Ġa Ġmulti - rot ational Ġt urret . Ġ ĠAutomatically Ġhandles Ġdifferent Ġcontrol Ġmodes Ċ Ġ* Ġsuch Ġas Ġrapid Ġmoves , Ġtracking , Ġholding , Ġand Ġj og ging . Ċ Ġ* Ċ Ġ* Ġ@ param ĠK p ĠThe ĠP Ġgain Ġfor Ġrapid Ġmoves Ġand Ġholding Ċ Ġ* Ġ@ param ĠK d ĠThe ĠD Ġgain Ġfor Ġrapid Ġmoves Ġand Ġholding Ċ Ġ* Ġ@ param Ġtracking Kp ĠThe ĠP Ġgain Ġfor Ġtracking Ġmoves Ċ Ġ* Ġ@ param Ġtracking Kd ĠThe ĠD Ġgain Ġfor Ġtracking Ġmoves Ċ Ġ* Ġ@ param ĠK s ĠThe Ġstatic Ġf riction Ġfeed Ġforward Ġcoefficient Ċ Ġ* Ġ@ param ĠK v ĠThe Ġvelocity Ġfeed Ġforward Ġcoefficient Ċ Ġ* Ġ@ param ĠK a ĠThe Ġacceleration Ġfeed Ġforward Ġcoefficient Ċ Ġ* Ġ@ param Ġvelocity Constraint ĠThe Ġvelocity Ġconstraint Ġfor Ġrapid Ġmoves Ċ Ġ* Ġ@ param Ġacceleration Constraint ĠThe Ġacceleration Ġconstraint Ġfor Ġrapid Ġmoves Ċ Ġ* Ġ@ param Ġrapid Threshold Distance ĠThe Ġminimum Ġrequired Ġrotation Ġwhich Ġwill Ġtrigger Ġa Ġrapid Ġmove Ċ Ġ* Ġ@ param Ġlower Limit Angle Absolute ĠThe Ġabsolute Ġlower Ġtravel Ġlimit Ġof Ġthe Ġt urret , Ġas Ġmeasured Ġby Ġthe Ġencoder Ċ Ġ* Ġ@ param Ġupper Limit Angle Absolute ĠThe Ġabsolute Ġupper Ġtravel Ġlimit Ġof Ġthe Ġt urret , Ġas Ġmeasured Ġby Ġthe Ġencoder Ċ Ġ* Ġ@ param Ġinput Provider ĠThe Ġprovider Ġfor Ġinput Ġdata Ġabout Ġthe Ġt urret Ġ( position , Ġvelocity ) Ċ Ġ* Ġ@ param Ġoutput Provider ĠThe Ġprovider Ġfor Ġoutput Ġsignals Ġof Ġthe Ġt urret Ġ( percent Ġoutput Ġcontrol ) Ċ Ġ* Ġ@ param Ġrate ĠThe Ġrate Ġthe Ġcontroller Ġwill Ġbe Ġupdated Ġat , Ġin Ġseconds Ċ Ġ*/ Ċ class ĠT urret Controller ( ĊĠĠĠ ĠK p : ĠDouble , ĊĠĠĠ ĠK d : ĠDouble , ĊĠĠĠ Ġtracking Kp : ĠDouble , ĊĠĠĠ Ġtracking Kd : ĠDouble , ĊĊĠĠĠ Ġprivate Ġval ĠK s : ĠDouble , ĊĠĠĠ Ġprivate Ġval ĠK v : ĠDouble , ĊĠĠĠ Ġprivate Ġval ĠK a : ĠDouble , ĊĊĠĠĠ Ġprivate Ġval Ġvelocity Constraint : ĠMeasure Radians PerSecond , ĊĠĠĠ Ġprivate Ġval Ġacceleration Constraint : ĠMeasure Radians PerSecond PerSecond , ĊĊĠĠĠ Ġprivate Ġval Ġrapid Threshold Distance : ĠMeasure Radians , ĊĊĠĠĠ Ġprivate Ġval Ġlower Limit Angle Absolute : ĠMeasure Radians , ĊĠĠĠ Ġprivate Ġval Ġupper Limit Angle Absolute : ĠMeasure Radians , ĊĊĠĠĠ Ġprivate Ġval Ġposition Radians Supplier : ĠDouble Supplier , ĊĠĠĠ Ġprivate Ġval Ġvelocity Rad Per Sec Supplier : ĠDouble Supplier , ĊĠĠĠ Ġprivate Ġval Ġvoltage Consumer : ĠDouble Consumer , ĊĊĠĠĠ Ġprivate Ġval Ġrate : ĠMeasure Seconds Ċ ) Ġ{ ĊĠĠĠ Ġcompanion Ġobject Ġ{ ĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠCreates Ġa ĠT urret Controller Ġfrom ĠJava . Ġ ĠJava Ġcode Ġcannot Ġinteract Ġwith Ġmethods ĊĠĠĠĠĠĠĠĠ Ġ* Ġthat Ġuse Ġinline Ġclasses Ġ( unit Ġclasses ) Ġso Ġthey Ġare Ġreplaced Ġwith ĠDouble Ġhere . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġ@ Jvm Static ĊĠĠĠĠĠĠĠ Ġfun Ġcreate From Java ( ĊĠĠĠĠĠĠĠĠĠĠĠ ĠK p : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠK d : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtracking Kp : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtracking Kd : ĠDouble , ĊĊĠĠĠĠĠĠĠĠĠĠĠ ĠK s : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠK v : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠK a : ĠDouble , ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġvelocity Constraint : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġacceleration Constraint : ĠDouble , ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġrapid Threshold Distance : ĠDouble , ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġlower Limit Angle Absolute : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġupper Limit Angle Absolute : ĠDouble , ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġposition Radians Supplier : ĠDouble Supplier , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvelocity Rad Per Sec Supplier : ĠDouble Supplier , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvoltage Consumer : ĠDouble Consumer , ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġrate : ĠDouble ĊĠĠĠĠĠĠĠ Ġ) Ġ= ĠT urret Controller ( ĊĠĠĠĠĠĠĠĠĠĠĠ ĠK p , ĠK d , Ġtracking Kp , Ġtracking Kd , ĠK s , ĠK v , ĠK a , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvelocity Constraint . Radians PerSecond , Ġacceleration Constraint . Radians PerSecond PerSecond , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrapid Threshold Distance . Radians , Ġlower Limit Angle Absolute . Radians , Ġupper Limit Angle Absolute . Radians , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġposition Radians Supplier , Ġvelocity Rad Per Sec Supplier , Ġvoltage Consumer , Ġrate . Seconds ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// Controllers Ġand Ġmodel ĊĠĠĠ Ġprivate Ġval Ġrapid Constraints Ġ= ĊĠĠĠĠĠĠĠ ĠTr ape z oid Profile Lite . Constraints ( velocity Constraint . value , Ġacceleration Constraint . value ) ĊĠĠĠ Ġprivate Ġval Ġrapid Controller Ġ= ĠProfile d PID Controller Lite ( Kp , Ġ 0 . 0 , ĠK d , Ġrapid Constraints , Ġrate . value ) ĊĠĠĠ Ġprivate Ġvar Ġlast R apid State Ġ= ĠTr ape z oid Profile Lite . State () ĊĠĠĠ Ġprivate Ġval Ġhold Controller Ġ= ĠPID Controller Lite ( Kp , Ġ 0 . 0 , ĠK d , Ġrate . value ) ĊĠĠĠ Ġprivate Ġval Ġtracking Controller Ġ= ĠPID Controller Lite ( tracking Kp , Ġ 0 . 0 , Ġtracking Kd , Ġrate . value ) ĊĠĠĠ Ġprivate Ġval Ġmodel Ġ= ĠSimple Motor Feed forward ( Ks , ĠK v , ĠK a ) ĊĊĠĠĠ Ġfun Ġset R apid G ains ( Kp : ĠDouble , ĠK d : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġrapid Controller . set PID ( Kp , Ġ 0 . 0 , ĠK d ) ĊĠĠĠĠĠĠĠ Ġhold Controller . set PID ( Kp , Ġ 0 . 0 , ĠK d ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġset Tracking G ains ( Kp : ĠDouble , ĠK d : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġtracking Controller . set PID ( Kp , Ġ 0 . 0 , ĠK d ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// State Ġinfo ĊĠĠĠ Ġprivate Ġvar Ġcurrent Position Ġ= Ġ 0 . 0 . Radians ĊĠĠĠ Ġprivate Ġvar Ġcurrent Velocity Ġ= Ġ 0 . 0 . Radians PerSecond ĊĊĠĠĠ Ġ// Status Ġinfo ĊĠĠĠ Ġvar Ġse tpoint Position Radians Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĠĠĠ Ġvar Ġse tpoint Velocity Rad Per Sec Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠ Ġinit Ġ{ ĊĠĠĠĠĠĠĠ Ġrapid Controller . set Tolerance ( Double . POSITIVE _ INFINITY ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// Limit Ġchecking Ġfunctions . Ġ ĠReturn Ġtrue Ġfor Ġa Ġsafe Ġangle , Ġfalse Ġfor Ġan Ġunsafe Ġangle ĊĠĠĠ Ġprivate Ġfun Ġcheck Lower Limit ( angle : ĠMeasure Radians ) Ġ= Ġ( angle Ġ>= Ġlower Limit Angle Absolute ) ĊĠĠĠ Ġprivate Ġfun Ġcheck Upper Limit ( angle : ĠMeasure Radians ) Ġ= Ġ( angle Ġ<= Ġupper Limit Angle Absolute ) ĊĠĠĠ Ġprivate Ġfun Ġcheck Limits ( angle : ĠMeasure Radians ) Ġ= Ġcheck Lower Limit ( angle ) Ġ&& Ġcheck Upper Limit ( angle ) ĊĊĠĠĠ Ġ// Handles Ġt urret Ġwrap around Ġand Ġshortest Ġdistance Ġtracking Ġusing Ġlast Ġknown Ġposition ĊĠĠĠ Ġprivate Ġfun Ġwrap Angle ( target Rotation : ĠRotation 2 d ): ĠMeasure Radians Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġcurrent Rotation Ġ= ĠRotation 2 d ( current Position . value ) Ġ// Clamp Ġcurrent Ġposition Ġto Ġ- 1 8 0 .. 1 8 0 ĊĠĠĠĠĠĠĠ Ġval Ġdelta Ġ= Ġcurrent Rotation . unary Minus (). rotate By ( target Rotation ) Ġ// Solve Ġdistance Ġto Ġgo ĊĊĠĠĠĠĠĠĠ Ġreturn ĠMath Util . clamp ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent Position . value Ġ+ Ġdelta . radians , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠConstants . k T urret Lower Limit Angle Absolute Rad , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠConstants . k T urret Upper Limit Angle Absolute Rad ĊĠĠĠĠĠĠĠ Ġ). Radians ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// Checks Ġif Ġa Ġmove Ġshould Ġbe Ġa Ġrapid ĊĠĠĠ Ġprivate Ġfun Ġshould R apid ( target Position : ĠMeasure Radians ): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ( target Position Ġ- Ġcurrent Position ). abs () Ġ> Ġrapid Threshold Distance ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġenum Ġclass ĠControl Mode Ġ{ ĊĠĠĠĠĠĠĠ ĠAngle , ĊĠĠĠĠĠĠĠ ĠJ og , ĊĠĠĠĠĠĠĠ ĠHold , ĊĠĠĠĠĠĠĠ ĠAbsolute Angle ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġvar Ġcurrent Control Mode Ġ= ĠControl Mode . Hold ĊĊĠĠĠ Ġprivate Ġfun Ġupdate State () Ġ{ ĊĠĠĠĠĠĠĠ Ġ// Read Ġinputs Ġfrom Ġthe Ġt urret ĊĠĠĠĠĠĠĠ Ġcurrent Position Ġ= Ġposition Radians Supplier . as Double . Radians ĊĠĠĠĠĠĠĠ Ġcurrent Velocity Ġ= Ġvelocity Rad Per Sec Supplier . as Double . Radians PerSecond ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġupdate Tracking ( goal : ĠRotation 2 d , Ġfeed Velocity : ĠMeasure Radians PerSecond ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġangle Ġ= Ġwrap Angle ( goal ) ĊĊĠĠĠĠĠĠĠ Ġif Ġ( should R apid ( angle )) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// We Ġneed Ġto Ġrapid Ġto Ġthe Ġangle ĊĠĠĠĠĠĠĠĠĠĠĠ Ġupdate R apid ( angle ) ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// No Ġrapid , Ġstandard Ġtracking ĊĠĠĠĠĠĠĠĠĠĠĠ ĠisIn R apid Ġ= Ġfalse ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġfeedback Vol ts Ġ= Ġtracking Controller . calculate ( current Position . value , Ġangle . value ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġff Vol ts Ġ= Ġmodel . calculate ( feed Velocity . value ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġvoltage Consumer . accept ( feedback Vol ts Ġ+ Ġff Vol ts ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠUpdate Ġdiagnostics ĊĠĠĠĠĠĠĠĠĠĠĠ Ġse tpoint Position Radians Ġ= Ġangle . value ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// R apid ĊĠĠĠ Ġvar ĠisIn R apid Ġ= Ġfalse ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠ Ġprivate Ġfun Ġupdate R apid ( goal : ĠMeasure Radians ) Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ(! isIn R apid ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// R apid Ġis Ġstarting ĊĠĠĠĠĠĠĠĠĠĠĠ Ġlast R apid State Ġ= ĠTr ape z oid Profile Lite . State ( current Position . value , Ġcurrent Velocity . value ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrapid Controller . reset ( last R apid State ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrapid Controller . set Goal ( goal . value ) ĊĠĠĠĠĠĠĠĠĠĠĠ ĠisIn R apid Ġ= Ġtrue ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ// Update Ġthe Ġrapid ĊĠĠĠĠĠĠĠ Ġval Ġfeedback Vol ts Ġ= Ġrapid Controller . calculate ( current Position . value , Ġgoal . value ) ĊĠĠĠĠĠĠĠ Ġval Ġff Vol ts Ġ= Ġmodel . calculate ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrapid Controller . set point . velocity , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrapid Controller . get Acceleration ( last R apid State ) ĊĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠĠĠĠĠ Ġlast R apid State Ġ= Ġrapid Controller . set point ĊĠĠĠĠĠĠĠ Ġvoltage Consumer . accept ( feedback Vol ts Ġ+ Ġff Vol ts ) ĊĊĠĠĠĠĠĠĠ Ġ// ĠUpdate Ġdiagnostics ĊĠĠĠĠĠĠĠ Ġse tpoint Position Radians Ġ= Ġrapid Controller . set point . position ĊĠĠĠĠĠĠĠ Ġse tpoint Velocity Rad Per Sec Ġ= Ġrapid Controller . set point . velocity ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// Hold Ġmode ĊĠĠĠ Ġfun Ġenter Hold () Ġ{ ĊĠĠĠĠĠĠĠ Ġupdate State () ĊĠĠĠĠĠĠĠ Ġhold Controller . reset () ĊĠĠĠĠĠĠĠ Ġhold Controller . set point Ġ= Ġcurrent Position . value ĊĠĠĠĠĠĠĠ Ġcurrent Control Mode Ġ= ĠControl Mode . Hold ĊĠĠĠĠĠĠĠ ĠisIn R apid Ġ= Ġfalse ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġupdate Hold () Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ( current Control Mode Ġ!= ĠControl Mode . Hold ) Ġreturn ĊĊĠĠĠĠĠĠĠ Ġupdate State () ĊĠĠĠĠĠĠĠ Ġval Ġfeedback Vol ts Ġ= Ġhold Controller . calculate ( current Position . value ) ĊĠĠĠĠĠĠĠ Ġvoltage Consumer . accept ( feedback Vol ts ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// J og Ġmode ĊĠĠĠ Ġprivate Ġvar Ġj og Set point Ġ= ĠGe om Util . IDENTITY _ ROT ATION ĊĊĠĠĠ Ġfun Ġenter J og () Ġ{ ĊĠĠĠĠĠĠĠ Ġupdate State () ĊĠĠĠĠĠĠĠ Ġtracking Controller . reset () ĊĠĠĠĠĠĠĠ Ġj og Set point Ġ= ĠRotation 2 d ( current Position . value ) ĊĠĠĠĠĠĠĠ Ġcurrent Control Mode Ġ= ĠControl Mode . J og ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġupdate J og ( rate Rad Per Sec : ĠDouble , Ġdt Seconds : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġrate Ġ= Ġrate Rad Per Sec . Radians PerSecond ĊĠĠĠĠĠĠĠ Ġval Ġdt Ġ= Ġdt Seconds . Seconds ĊĠĠĠĠĠĠĠ Ġif Ġ( current Control Mode Ġ!= ĠControl Mode . J og ) Ġreturn ĊĊĠĠĠĠĠĠĠ Ġupdate State () ĊĠĠĠĠĠĠĠ Ġval Ġj og Advance Ġ= ĠRotation 2 d (( rate Ġ* Ġdt ). value ) ĊĠĠĠĠĠĠĠ Ġj og Set point Ġ= Ġj og Set point . rotate By ( j og Advance ) ĊĊĠĠĠĠĠĠĠ Ġupdate Tracking ( j og Set point , Ġ 0 . 0 . Radians PerSecond ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġenter Angle () Ġ{ ĊĠĠĠĠĠĠĠ Ġupdate State () ĊĠĠĠĠĠĠĠ Ġtracking Controller . reset () ĊĠĠĠĠĠĠĠ Ġcurrent Control Mode Ġ= ĠControl Mode . Angle ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ@ Jvm Over loads Ġfun Ġupdate Angle ( angle Goal : ĠRotation 2 d , Ġfeed Velocity Rad Per Sec : ĠDouble Ġ= Ġ 0 . 0 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġfeed Velocity Ġ= Ġfeed Velocity Rad Per Sec . Radians PerSecond ĊĠĠĠĠĠĠĠ Ġif Ġ( current Control Mode Ġ!= ĠControl Mode . Angle ) Ġreturn ĊĊĠĠĠĠĠĠĠ Ġupdate State () ĊĠĠĠĠĠĠĠ Ġupdate Tracking ( angle Goal , Ġfeed Velocity ) ĊĠĠĠĠĠĠĠ Ġse tpoint Velocity Rad Per Sec Ġ= Ġfeed Velocity Rad Per Sec ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġenter Absolute Angle () Ġ{ ĊĠĠĠĠĠĠĠ Ġupdate State () ĊĠĠĠĠĠĠĠ Ġlast R apid State Ġ= ĠTr ape z oid Profile Lite . State ( current Position . value , Ġcurrent Velocity . value ) ĊĠĠĠĠĠĠĠ Ġrapid Controller . reset ( last R apid State ) ĊĠĠĠĠĠĠĠ Ġcurrent Control Mode Ġ= ĠControl Mode . Absolute Angle ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġupdate Absolute Angle ( angle Absolute Radians : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġangle Absolute Ġ= ĠMath Util . clamp ( angle Absolute Radians , ĠConstants . k T urret Lower Limit Angle Absolute Rad , ĠConstants . k T urret Upper Limit Angle Absolute Rad ). Radians ĊĠĠĠĠĠĠĠ Ġupdate State () ĊĠĠĠĠĠĠĠ Ġval Ġfeedback Vol ts Ġ= Ġrapid Controller . calculate ( current Position . value , Ġangle Absolute . value ) ĊĠĠĠĠĠĠĠ Ġval Ġff Vol ts Ġ= Ġmodel . calculate ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrapid Controller . set point . velocity , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrapid Controller . get Acceleration ( last R apid State ) ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠ Ġlast R apid State Ġ= Ġrapid Controller . set point ĊĊĠĠĠĠĠĠĠ Ġvoltage Consumer . accept ( feedback Vol ts Ġ+ Ġff Vol ts ) ĊĊĠĠĠĠĠĠĠ Ġ// ĠUpdate Ġdiagnostics ĊĠĠĠĠĠĠĠ Ġse tpoint Position Radians Ġ= Ġrapid Controller . set point . position ĊĠĠĠĠĠĠĠ Ġse tpoint Velocity Rad Per Sec Ġ= Ġrapid Controller . set point . velocity ĊĠĠĠ Ġ} Ċ }