/* Ċ The ĠMIT ĠLicense Ġ( MIT ) Ċ Ċ Copyright ĠÂ© Ġ 2 0 2 1 ĠCheck mate ĠRobot ics Ċ Ċ Permission Ġis Ġhereby Ġgranted , Ġfree Ġof Ġcharge , Ġto Ġany Ġperson Ġobtaining Ġa Ġcopy Ġof Ġthis Ġsoftware Ġand Ċ associated Ġdocumentation Ġfiles Ġ( the ĠâĢľ Software âĢĿ ), Ġto Ġdeal Ġin Ġthe ĠSoftware Ġwithout Ġrestriction , Ġincluding Ċ without Ġlimitation Ġthe Ġrights Ġto Ġuse , Ġcopy , Ġmodify , Ġmerge , Ġpublish , Ġdistribute , Ġsublicense , Ġand / or Ġsell Ċ copies Ġof Ġthe ĠSoftware , Ġand Ġto Ġpermit Ġpersons Ġto Ġwhom Ġthe ĠSoftware Ġis Ġfurnished Ġto Ġdo Ġso , Ġsubject Ġto Ġthe Ċ following Ġconditions : Ċ Ċ The Ġabove Ġcopyright Ġnotice Ġand Ġthis Ġpermission Ġnotice Ġshall Ġbe Ġincluded Ġin Ġall Ġcopies Ġor Ġsubstantial Ċ port ions Ġof Ġthe ĠSoftware . Ċ Ċ THE ĠSOFTWARE ĠIS ĠPROVIDED ĠâĢľ AS ĠIS âĢĿ, ĠWITHOUT ĠWARRANTY ĠOF ĠANY ĠKIND , ĠEXPRESS ĠOR ĠIMPLIED , ĠINCLUDING ĠBUT ĠNOT Ċ LIMIT ED ĠTO ĠTHE ĠWARRANTIES ĠOF ĠMERCHANTABILITY , ĠFITNESS ĠFOR ĠA ĠPARTICULAR ĠPURPOSE ĠAND ĠNONINFRINGEMENT . ĠIN Ċ NO ĠEVENT ĠSHALL ĠTHE ĠAUTHORS ĠOR ĠCOPYRIGHT ĠHOLDERS ĠBE ĠLIABLE ĠFOR ĠANY ĠCLAIM , ĠDAMAGES ĠOR ĠOTHER ĠLIABILITY , Ċ WH ETHER ĠIN ĠAN ĠACTION ĠOF ĠCONTRACT , ĠTORT ĠOR ĠOTHERWISE , ĠARISING ĠFROM , ĠOUT ĠOF ĠOR ĠIN ĠCONNECTION ĠWITH ĠTHE Ċ SOFTWARE ĠOR ĠTHE ĠUSE ĠOR ĠOTHER ĠDEALINGS ĠIN ĠTHE ĠSOFTWARE . Ċ */ Ċ package Ġorg . first insp ires . ft c . team code . robot . sub systems Ċ Ċ import Ġcom . ac mer obot ics . dashboard . config . Config Ċ import Ġcom . ac mer obot ics . road runner . control . PID Coefficient s Ċ import Ġcom . ac mer obot ics . road runner . drive . M ec an um Drive Ċ import Ġcom . ac mer obot ics . road runner . followers . H ol onom ic PID V AF ollow er Ċ import Ġcom . ac mer obot ics . road runner . followers . Trajectory Follower Ċ import Ġcom . ac mer obot ics . road runner . geometry . Pose 2 d Ċ import Ġcom . ac mer obot ics . road runner . trajectory . Trajectory Ċ import Ġcom . ac mer obot ics . road runner . trajectory . Trajectory Builder Ċ import Ġcom . ac mer obot ics . road runner . trajectory . constraints .* Ċ import Ġcom . qual comm . hardware . bos ch . B NO 0 5 5 IM U Ċ import Ġcom . qual comm . robot core . hardware .* Ċ import Ġorg . first insp ires . ft c . team code . op modes . fre ight f ren zy . auto . util . Op Mode Util Ċ import Ġorg . first insp ires . ft c . team code . op modes . fre ight f ren zy . auto . util . to Super Pose 2 d Ċ import Ġorg . first insp ires . ft c . team code . robot . Hardware Names . Mot ors Ċ import Ġorg . first insp ires . ft c . team code . robot . abstract s . Abstract Subsystem Ċ import Ġorg . first insp ires . ft c . team code . robot . abstract s . Subsystem Map Ċ import Ġorg . first insp ires . ft c . team code . robot . sub systems . drive train . Drive Constants Ċ import Ġorg . first insp ires . ft c . team code . robot . sub systems . drive train . local izers . T 2 6 5 Local izer Ċ import Ġorg . first insp ires . ft c . team code . robot . sub systems . drive train . trajectory sequence . Super Trajectory Sequence Runner Ċ import Ġorg . first insp ires . ft c . team code . robot . sub systems . drive train . trajectory sequence . Trajectory Sequence Ċ import Ġorg . first insp ires . ft c . team code . robot . sub systems . drive train . trajectory sequence . Trajectory Sequence Builder Ċ import Ġkotlin . math . abs Ċ Ċ /** Ċ Ġ* ĠM ec an um Ġdrive Ġimplementation Ġto Ġwork Ġwith Ġroad runner Ċ Ġ* Ċ Ġ* Ġ@ see Ġorg . first insp ires . ft c . team code . robot . sub systems . drive train Ċ Ġ*/ Ċ @ Config Ċ class ĠDrive train ( hardware Map : ĠHardware Map ) Ġ: ĠM ec an um Drive ( ĊĠĠĠ ĠDrive Constants . k V , ĊĠĠĠ ĠDrive Constants . k A , ĊĠĠĠ ĠDrive Constants . k Static , ĊĠĠĠ ĠDrive Constants . TRACK _ WIDTH , ĊĠĠĠ ĠDrive Constants . TRACK _ WIDTH , ĊĠĠĠ ĠL ATER AL _ MULTI PLIER Ċ ), ĠAbstract Subsystem Ġ{ ĊĠĠĠ Ġoverride Ġval Ġtag Ġ= Ġ" Drive train " ĊĠĠĠ Ġoverride Ġval Ġsub systems Ġ= ĠSub system Map { Ġtag Ġ} ĊĊĠĠĠ Ġprivate Ġval Ġtrajectory Sequence Runner : ĠSuper Trajectory Sequence Runner ĊĠĠĠ Ġprivate Ġval Ġleft Front Ġ= ĠMot ors . LEFT _ FRONT . get ( hardware Map ) ĊĠĠĠ Ġprivate Ġval Ġleft Re ar Ġ= ĠMot ors . LEFT _ RE AR . get ( hardware Map ) ĊĠĠĠ Ġprivate Ġval Ġright Re ar Ġ= ĠMot ors . RIGHT _ RE AR . get ( hardware Map ) ĊĠĠĠ Ġprivate Ġval Ġright Front Ġ= ĠMot ors . RIGHT _ FRONT . get ( hardware Map ) ĊĠĠĠ Ġprivate Ġval Ġmot ors Ġ= ĠlistOf ( left Front , Ġleft Re ar , Ġright Re ar , Ġright Front ) ĊĠĠĠ Ġprivate Ġval Ġim u : ĠB NO 0 5 5 IM U ĊĠĠĠ Ġprivate Ġval Ġbattery Voltage Sensor : ĠVol tage Sensor ĊĊĠĠĠ Ġfun Ġtrajectory Builder ( start Pose : ĠP ose 2 d ): ĠTra jectory Builder Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn ĠTra jectory Builder ( start Pose , Ġfalse , ĠV EL _ CONSTRAINT , ĠAC CEL _ CONSTRAINT ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġtrajectory Builder ( start Pose : ĠP ose 2 d , Ġreversed : ĠBoolean ): ĠTra jectory Builder Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn ĠTra jectory Builder ( start Pose , Ġreversed , ĠV EL _ CONSTRAINT , ĠAC CEL _ CONSTRAINT ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġtrajectory Builder ( start Pose : ĠP ose 2 d , Ġstart Heading : ĠDouble ): ĠTra jectory Builder Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn ĠTra jectory Builder ( start Pose , Ġstart Heading , ĠV EL _ CONSTRAINT , ĠAC CEL _ CONSTRAINT ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġtrajectory Sequence Builder ( start Pose : ĠP ose 2 d ): ĠTra jectory Sequence Builder Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn ĠTra jectory Sequence Builder ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġstart Pose , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠV EL _ CONSTRAINT , ĠAC CEL _ CONSTRAINT , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠDrive Constants . MAX _ ANG _ VEL , ĠDrive Constants . MAX _ ANG _ AC CEL ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġturn Async ( angle : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġtrajectory Sequence Runner . follow Trajectory Sequence Async ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtrajectory Sequence Builder ( pose Estimate ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. turn ( angle ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġturn ( angle : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġturn Async ( angle ) ĊĠĠĠĠĠĠĠ ĠwaitFor Idle () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġfollow Trajectory Async ( trajectory : ĠTra jectory ) Ġ{ ĊĠĠĠĠĠĠĠ Ġtrajectory Sequence Runner . follow Trajectory Sequence Async ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtrajectory Sequence Builder ( trajectory . start ()) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. add Trajectory ( trajectory ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġfollow Trajectory ( trajectory : ĠTra jectory ) Ġ{ ĊĠĠĠĠĠĠĠ Ġfollow Trajectory Async ( trajectory ) ĊĠĠĠĠĠĠĠ ĠwaitFor Idle () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġfollow Trajectory Sequence Async ( trajectory Sequence : ĠTra jectory Sequence ?) Ġ{ ĊĠĠĠĠĠĠĠ Ġtrajectory Sequence Runner . follow Trajectory Sequence Async ( trajectory Sequence ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġfollow Trajectory Sequence ( trajectory Sequence : ĠTra jectory Sequence ?) Ġ{ ĊĠĠĠĠĠĠĠ Ġfollow Trajectory Sequence Async ( trajectory Sequence ) ĊĠĠĠĠĠĠĠ ĠwaitFor Idle () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġval Ġlast Error : ĠP ose 2 d ? ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġtrajectory Sequence Runner . last Pose Error ĊĊĠĠĠ Ġoverride Ġfun Ġloop () Ġ{ ĊĠĠĠĠĠĠĠ Ġupdate Pose Estimate () ĊĠĠĠĠĠĠĠ Ġpersistent Pose Estimate Ġ= Ġpose Estimate . to Super Pose 2 d () ĊĠĠĠĠĠĠĠ Ġtrajectory Sequence Runner . update ( pose Estimate , Ġpose Velocity , Ġvoltage )?. let Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġset Drive Signal ( it ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun ĠwaitFor Idle () Ġ{ ĊĠĠĠĠĠĠĠ Ġwhile Ġ(! Thread . currentThread (). is Inter rupted Ġ&& Ġis Busy ) Ġloop () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġfun Ġcancel Sequence () Ġ{ ĊĠĠĠĠĠĠĠ Ġtrajectory Sequence Runner . cancel Sequence () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġval Ġis Busy : ĠBoolean ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġtrajectory Sequence Runner . is Busy ĊĊĠĠĠ Ġfun Ġset Mode ( run Mode : ĠD c Motor . Run Mode ?) Ġ{ ĊĠĠĠĠĠĠĠ Ġfor Ġ( motor Ġin Ġmot ors ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmotor . mode Ġ= Ġrun Mode ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġval Ġvoltage ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġbattery Voltage Sensor . voltage ĊĊĠĠĠ Ġfun Ġset Weighted Drive Power ( drive Power : ĠP ose 2 d ) Ġ{ ĊĠĠĠĠĠĠĠ Ġvar Ġvel Ġ= Ġdrive Power ĊĠĠĠĠĠĠĠ Ġif Ġ(( abs ( drive Power . x ) Ġ+ Ġabs ( drive Power . y ) Ġ+ Ġabs ( drive Power . heading )) Ġ> Ġ 1 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġre - normalize Ġthe Ġpowers Ġaccording Ġto Ġthe Ġweights ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġdenominator Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠV X _ WEIGHT Ġ* Ġabs ( drive Power . x ) Ġ+ ĠV Y _ WEIGHT Ġ* Ġabs ( drive Power . y ) Ġ+ ĠO ME GA _ WEIGHT Ġ* Ġabs ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive Power . heading ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvel Ġ= ĠP ose 2 d ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠV X _ WEIGHT Ġ* Ġdrive Power . x , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠV Y _ WEIGHT Ġ* Ġdrive Power . y , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠO ME GA _ WEIGHT Ġ* Ġdrive Power . heading ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ). div ( denom inator ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġset Drive Power ( vel ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġget Wheel Positions (): ĠList < Double > Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġwheel Positions : ĠMutable List < Double > Ġ= ĠArrayList () ĊĠĠĠĠĠĠĠ Ġfor Ġ( motor Ġin Ġmot ors ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġwheel Positions . add ( Drive Constants . encoder Ticks To In ches ( motor . current Position . toDouble ())) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġreturn Ġwheel Positions ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġget Wheel Ve locities (): ĠList < Double > Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġwheel Ve locities : ĠMutable List < Double > Ġ= ĠArrayList () ĊĠĠĠĠĠĠĠ Ġfor Ġ( motor Ġin Ġmot ors ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġwheel Ve locities . add ( Drive Constants . encoder Ticks To In ches ( motor . velocity )) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġreturn Ġwheel Ve locities ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġset Motor Pow ers ( front Left : ĠDouble , Ġrear Left : ĠDouble , Ġrear Right : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġfront Right : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġleft Front . power Ġ= Ġfront Left ĊĠĠĠĠĠĠĠ Ġleft Re ar . power Ġ= Ġrear Left ĊĠĠĠĠĠĠĠ Ġright Re ar . power Ġ= Ġrear Right ĊĠĠĠĠĠĠĠ Ġright Front . power Ġ= Ġfront Right ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġval Ġraw External Heading : ĠDouble ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġim u . angular Orientation . first Angle . toDouble () ĊĊĠĠĠ Ġoverride Ġfun Ġget External Heading Velocity (): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġ// ĠDONE : ĠThis Ġmust Ġbe Ġchanged Ġto Ġmatch Ġyour Ġconfiguration ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ| ĠZ Ġaxis ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ| ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠ Ġ( Motor ĠPort ĠSide ) ĠĠĠĠ Ġ| ĠĠ Ġ/ ĠX Ġaxis ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ__ __| __/ ____ ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠĠĠĠĠĠ ĠY Ġaxis ĠĠĠĠ Ġ/ Ġ* ĠĠ Ġ| Ġ/ ĠĠĠ Ġ/ | ĠĠ Ġ( IO ĠSide ) ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠĠĠĠĠĠ Ġ__ ____ ___ Ġ/ ____ __| / ĠĠĠ Ġ// ĠĠĠĠĠ ĠI 2 C ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ/ ________ ___ Ġ// ĠĠĠĠ ĠDigital ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ| ________ ____ | / ĠĠĠĠĠ ĠAn alog ĊĠĠĠĠĠĠĠ Ġ// ĊĠĠĠĠĠĠĠ Ġ// ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ( Ser vo ĠPort ĠSide ) ĊĠĠĠĠĠĠĠ Ġ// ĊĠĠĠĠĠĠĠ Ġ// ĠThe Ġpositive Ġx Ġaxis Ġpoints Ġtoward Ġthe ĠUSB Ġport ( s ) ĊĠĠĠĠĠĠĠ Ġ// ĊĠĠĠĠĠĠĠ Ġ// ĠAdjust Ġthe Ġaxis Ġrotation Ġrate Ġas Ġnecessary ĊĠĠĠĠĠĠĠ Ġ// ĠRotate Ġabout Ġthe Ġz Ġaxis Ġis Ġthe Ġdefault Ġassuming Ġyour ĠRE V ĠHub / Control ĠHub Ġis Ġlay ing ĊĠĠĠĠĠĠĠ Ġ// Ġflat Ġon Ġa Ġsurface ĊĠĠĠĠĠĠĠ Ġreturn Ġim u . angular Velocity . x Rotation Rate . toDouble () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġcompanion Ġobject Ġ{ ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field Ġvar Ġcamera Robot Offset Ġ= ĠOp Mode Util . Super Pose 2 d (- 6 . 0 , Ġ- 6 . 0 , Ġ 9 0 . 0 ) ĊĊĠĠĠĠĠĠĠ Ġ@ Jvm Field Ġvar Ġpersistent Pose Estimate Ġ= ĠOp Mode Util . Super Pose 2 d () ĊĊĠĠĠĠĠĠĠ Ġ@ Jvm Field Ġvar ĠTRAN SL ATION AL _ PID Ġ= ĠPID Coefficient s ( 8 . 0 , Ġ 0 . 0 , Ġ 1 . 0 ) ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field Ġvar ĠHEAD ING _ PID Ġ= ĠPID Coefficient s ( 8 . 0 , Ġ 0 . 0 , Ġ 1 . 0 ) ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field Ġvar ĠL ATER AL _ MULTI PLIER Ġ= Ġ 1 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field Ġvar ĠV X _ WEIGHT Ġ= Ġ 1 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field Ġvar ĠV Y _ WEIGHT Ġ= Ġ 1 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field Ġvar ĠO ME GA _ WEIGHT Ġ= Ġ 1 . 0 ĊĠĠĠĠĠĠĠ Ġval ĠV EL _ CONSTRAINT ĊĠĠĠĠĠĠĠĠĠĠĠ Ġget () Ġ= Ġget Velocity Constraint ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠDrive Constants . MAX _ VEL , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠDrive Constants . MAX _ ANG _ VEL , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠDrive Constants . TRACK _ WIDTH ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠ Ġval ĠAC CEL _ CONSTRAINT ĊĠĠĠĠĠĠĠĠĠĠĠ Ġget () Ġ= Ġget Acceleration Constraint ( Drive Constants . MAX _ AC CEL ) ĊĊĠĠĠĠĠĠĠ Ġprivate Ġfun Ġget Velocity Constraint ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmax Vel : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmax Angular Vel : ĠDouble , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtrack Width : ĠDouble ĊĠĠĠĠĠĠĠ Ġ): ĠTra jectory Velocity Constraint Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn ĠMin Velocity Constraint ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠlistOf ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠAngular Velocity Constraint ( max Angular Vel ), ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠM ec an um Velocity Constraint ( max Vel , Ġtrack Width ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġprivate Ġfun Ġget Acceleration Constraint ( max Accel : ĠDouble ): ĠTra jectory Acceleration Constraint Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn ĠProfile Acceleration Constraint ( max Accel ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġinit Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġfollow er : ĠTra jectory Follower Ġ= ĠHol onom ic PID V AF ollow er ( ĊĠĠĠĠĠĠĠĠĠĠĠ ĠTRAN SL ATION AL _ PID , ĠTRAN SL ATION AL _ PID , ĠHEAD ING _ PID , ĊĠĠĠĠĠĠĠĠĠĠĠ ĠP ose 2 d ( 0 . 5 , Ġ 0 . 5 , ĠMath . to Radians ( 5 . 0 )), Ġ 0 . 5 ĊĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠĠĠĠĠ Ġbattery Voltage Sensor Ġ= Ġhardware Map . voltage Sensor . iterator (). next () ĊĊĠĠĠĠĠĠĠ Ġim u Ġ= Ġhardware Map . get ( B NO 0 5 5 IM U :: class . java , Ġ" imu ") ĊĠĠĠĠĠĠĠ Ġval Ġparameters Ġ= ĠB NO 0 5 5 IM U . Parameters () ĊĠĠĠĠĠĠĠ Ġparameters . angle Unit Ġ= ĠB NO 0 5 5 IM U . Angle Unit . RAD IAN S ĊĠĠĠĠĠĠĠ Ġim u . initialize ( parameters ) ĊĠĠĠĠĠĠĠ Ġfor Ġ( motor Ġin Ġmot ors ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmotor Configuration Type Ġ= Ġmotor . motor Type . clone () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmotor Configuration Type . ach ieve able Max RP M Fraction Ġ= Ġ 1 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmotor . motor Type Ġ= Ġmotor Configuration Type ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmotor . zero Power Behavior Ġ= ĠD c Motor . Zero Power Behavior . BR A KE ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ// ĠDONE : Ġif Ġdesired , Ġuse Ġset Local izer () Ġto Ġchange Ġthe Ġlocalization Ġmethod ĊĠĠĠĠĠĠĠ Ġval Ġt 2 6 5 Local izer Ġ= ĠT 2 6 5 Local izer ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcamera Robot Offset . pose 2 d , Ġ. 8 , Ġhardware Map ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠ Ġsub systems . register ( t 2 6 5 Local izer ) ĊĠĠĠĠĠĠĠ Ġlocal izer Ġ= Ġt 2 6 5 Local izer ĊĊĠĠĠĠĠĠĠ Ġtrajectory Sequence Runner Ġ= ĠSuper Trajectory Sequence Runner ( follow er , ĠHEAD ING _ PID ) ĊĠĠĠĠĠĠĠ Ġpose Estimate Ġ= Ġpersistent Pose Estimate . pose 2 d ĊĠĠĠ Ġ} Ċ }