package Ġorg . first insp ires . ft c . team code . module . vision Ċ Ċ import Ġcom . ac mer obot ics . dashboard . F tc Dashboard Ċ import Ġcom . ac mer obot ics . dashboard . config . Config Ċ import Ġcom . ac mer obot ics . dashboard . telemetry . Telemetry Packet Ċ import Ġcom . ac mer obot ics . road runner . geometry . Vector 2 d Ċ import Ġorg . first insp ires . ft c . robot core . external . Telemetry Ċ import Ġorg . first insp ires . ft c . team code . module . Op Mode Ċ import Ġorg . first insp ires . ft c . team code . module . Robot Ċ import Ġorg . opencv . core .* Ċ import Ġorg . opencv . img proc . Img proc Ċ import Ġorg . open ft c . easy opencv . Open Cv Pipeline Ċ import Ġkotlin . math .* ĊĊ Ċ class ĠRing Pipeline ( val Ġbot : ĠRobot , Ġval Ġc width : ĠInt , Ġval Ġc height : ĠInt ): ĠPipeline () Ġ{ ĊĠĠĠ Ġprivate Ġval Ġdashboard : ĠF tc Dashboard Ġ= ĠF tc Dashboard . getInstance () ĊĠĠĠ Ġvar Ġtelemetry : ĠTe lemetry Ġ= Ġbot . log ĊĠĠĠ Ġvar Ġmat : ĠMat ĊĠĠĠ Ġvar Ġret : ĠMat ĊĊĠĠĠ Ġvar Ġbe ams : ĠList < Angle Rect > Ġ= ĠlistOf () ĊĠĠĠ Ġval Ġvectors : ĠList < Vector 2 d > Ġget () Ġ= Ġbe ams . map NotNull Ġ{ Ġestimate Vector ( it ) Ġ} ĊĠĠĠ Ġval Ġab sol utes : ĠList < Vector 2 d > Ġget () Ġ= Ġvectors . map NotNull Ġ{ Ġabsolute ( it ) Ġ} ĊĊĠĠĠ Ġ@ Config ĊĠĠĠ Ġobject ĠRing Constants Ġ{ ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field ĊĠĠĠĠĠĠĠ Ġvar Ġl Y Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field ĊĠĠĠĠĠĠĠ Ġvar Ġl R Ġ= Ġ 1 4 1 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field ĊĠĠĠĠĠĠĠ Ġvar Ġl B Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field ĊĠĠĠĠĠĠĠ Ġvar Ġu Y Ġ= Ġ 2 5 5 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field ĊĠĠĠĠĠĠĠ Ġvar Ġu R Ġ= Ġ 2 3 0 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field ĊĠĠĠĠĠĠĠ Ġvar Ġu B Ġ= Ġ 1 1 0 . 0 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field ĊĠĠĠĠĠĠĠ Ġvar ĠMIN _ WIDTH Ġ= Ġ. 0 5 ĊĠĠĠĠĠĠĠ Ġ@ Jvm Field ĊĠĠĠĠĠĠĠ Ġvar ĠH ORIZ ON Ġ= Ġ. 5 ĊĊĠĠĠĠĠĠĠ Ġconst Ġval ĠR ING _ RADIUS Ġ= Ġ 2 . 5 ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġval Ġf ocal Length : ĠDouble Ġget () Ġ= ĠVision Constants . FO CAL _ RATIO Ġ* Ġc width . toDouble () Ġ/ Ġ 2 . 0 ĊĠĠĠ Ġval Ġh orizon : ĠInt Ġget () Ġ= Ġ( Ring Constants . H ORIZ ON Ġ* Ġc height ). toInt () ĊĠĠĠ Ġval ĠminWidth : ĠInt Ġget () Ġ= Ġ( Ring Constants . MIN _ WIDTH Ġ* Ġc width ). toInt () ĊĊĠĠĠ Ġcompanion Ġobject Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġlower Orange Ġget () Ġ= ĠScalar ( Ring Constants . l Y , ĠRing Constants . l R , ĠRing Constants . l B ) ĊĠĠĠĠĠĠĠ Ġval Ġupper Orange Ġget () Ġ= ĠScalar ( Ring Constants . u Y , ĠRing Constants . u R , ĠRing Constants . u B ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġdefault Ġinit Ġcall , Ġconstructor ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġinit Ġ{ ĊĠĠĠĠĠĠĠ Ġret Ġ= ĠMat () ĊĠĠĠĠĠĠĠ Ġmat Ġ= ĠMat () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// fun Ġc transform ( x : ĠInt , Ġy : ĠInt ): ĠPair < Int , ĠInt > Ġ= ĠPair ( x Ġ- Ġc width Ġ/ Ġ 2 , Ġc height Ġ/ Ġ 2 Ġ- Ġy ) ĊĠĠĠ Ġfun Ġangle ( n : ĠInt ): ĠDouble Ġ= Ġatan 2 ( n . toDouble (), Ġf ocal Length ) ĊĠĠĠ Ġfun Ġdelta ( n 1 : ĠInt , Ġn 2 : ĠInt ): ĠDouble Ġ= Ġangle ( n 2 ) Ġ- Ġangle ( n 1 ) ĊĠĠĠ Ġfun Ġestimate Distance ( da : ĠDouble ): ĠDouble Ġ= ĠRing Constants . RING _ RADIUS Ġ/ Ġsin ( da Ġ/ Ġ 2 . 0 ) ĊĊĠĠĠ Ġfun Ġrange ( rect : ĠRect ): ĠAngle Rect Ġ// Ġtodo Ġangular Ġinstead Ġof Ġlinear Ġinterpolation Ġfor Ġmid point ĊĠĠĠĠĠĠĠ Ġ= ĠAngle Rect ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġleft Ġ= Ġangle ( rect . x Ġ- Ġc width Ġ/ Ġ 2 ), ĊĠĠĠĠĠĠĠĠĠĠĠ Ġright Ġ= Ġangle ( rect . x Ġ+ Ġrect . width Ġ- Ġc width Ġ/ Ġ 2 ), ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtop Ġ= Ġangle ( che ight Ġ/ Ġ 2 Ġ- Ġrect . y ), ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbottom Ġ= Ġangle ( che ight Ġ/ Ġ 2 Ġ- Ġrect . y Ġ- Ġrect . height ) ĊĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠ Ġdata Ġclass ĠAngle Rect ( val Ġtop : ĠDouble Ġ= Ġ 0 . 0 , Ġval Ġbottom : ĠDouble Ġ= Ġ 0 . 0 , Ġval Ġleft : ĠDouble Ġ= Ġ 0 . 0 , Ġval Ġright : ĠDouble Ġ= Ġ 0 . 0 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġalpha : ĠDouble Ġget () Ġ= Ġ( left Ġ+ Ġright ) Ġ/ Ġ 2 . 0 ĊĠĠĠĠĠĠĠ Ġval Ġbeta : ĠDouble Ġget () Ġ= Ġ( top Ġ+ Ġbottom ) Ġ/ Ġ 2 . 0 ĊĊĠĠĠĠĠĠĠ Ġval Ġwidth : ĠDouble Ġget () Ġ= Ġright Ġ- Ġleft ĊĠĠĠĠĠĠĠ Ġval Ġheight : ĠDouble Ġget () Ġ= Ġtop Ġ- Ġbottom ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġget Boxes ( input : ĠMat ?): ĠPair < List < Rect >, ĠMat > Ġ{ ĊĠĠĠĠĠĠĠ Ġret . release () ĊĊĠĠĠĠĠĠĠ Ġvar Ġboxes : ĠList < Rect > Ġ= ĠlistOf () ĊĠĠĠĠĠĠĠ Ġret Ġ= ĠMat () ĊĠĠĠĠĠĠĠ Ġtry Ġ{ Ċ // ĠĠĠĠĠĠĠĠĠĠĠ Ġmat . release () ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ/** con verting Ġfrom ĠRGB Ġcolor Ġspace Ġto ĠH SV Ġcolor Ġspace **/ ĊĠĠĠĠĠĠĠĠĠĠĠ ĠImg proc . cvt Color ( input , Ġmat , ĠImg proc . COLOR _ RGB 2 Y Cr Cb ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ/** checking Ġif Ġany Ġpixel Ġis Ġwithin Ġthe Ġyellow Ġbounds Ġto Ġmake Ġa Ġblack Ġand Ġwhite Ġmask **/ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmask Ġ= ĠMat ( mat . rows (), Ġmat . cols (), ĠC v Type . CV _ 8 UC 1 ) ĊĠĠĠĠĠĠĠĠĠĠĠ ĠCore . in Range ( mat , Ġlower Orange , Ġupper Orange , Ġmask ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ/** apply ing Ġto Ġinput Ġand Ġputting Ġit Ġon Ġret Ġin Ġblack Ġor Ġyellow **/ ĊĠĠĠĠĠĠĠĠĠĠĠ ĠCore . bit wise _ and ( input , Ġinput , Ġret , Ġmask ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ ĠImg proc . Gaussian Blur ( mask , Ġmask , ĠSize ( 5 . 0 , Ġ 1 5 . 0 ), Ġ 0 . 0 0 ); ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ/** finding Ġcont ours Ġon Ġmask **/ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġcont ours : ĠList < Mat Of Point > Ġ= ĠArrayList () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġhierarchy Ġ= ĠMat () ĊĠĠĠĠĠĠĠĠĠĠĠ ĠImg proc . find Cont ours ( mask , Ġcont ours , Ġhierarchy , ĠImg proc . RE TR _ TREE , ĠImg proc . CHAIN _ APPRO X _ NONE ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ ĠImg proc . draw Cont ours ( ret , Ġcont ours , Ġ- 1 , ĠScalar ( 0 . 0 , Ġ 2 5 5 . 0 , Ġ 0 . 0 ), Ġ 3 ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ ĠImg proc . line ( ret , ĠPoint (. 0 , ĠRing Constants . H ORIZ ON . toDouble ()), ĠPoint ( c width . toDouble (), ĠRing Constants . H ORIZ ON . toDouble ()), ĠScalar ( 2 5 5 . 0 , Ġ. 0 , Ġ 2 5 5 . 0 )) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġboxes Ġ= Ġcont ours . map NotNull Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġcopy Ġ= ĠMat Of Point 2 f (* it . toArray ()) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġbox : ĠRect Ġ= ĠImg proc . bounding Rect ( copy ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġw Ġ= Ġbox . width ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġr : ĠRect ? Ġ= Ġnull ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( w Ġ> ĠminWidth Ġ&& Ġbox . y Ġ+ Ġbox . height Ġ> Ġh orizon ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠImg proc . rectangle ( ret , Ġbox , ĠScalar ( 0 . 0 , Ġ 0 . 0 , Ġ 2 5 5 . 0 ), Ġ 2 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġr Ġ= Ġbox ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġit . release () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcopy . release () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġr ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ}. sorted By Descending Ġ{ Ġit . width Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġmat . release () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmask . release () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġhierarchy . release () ĊĠĠĠĠĠĠĠ Ġ} Ġcatch Ġ( e : ĠException ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ/** error Ġhandling , Ġprints Ġstack Ġtrace Ġfor Ġspecific Ġdebug **/ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . addData ("[ ERROR ]", Ġe ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġe . stack Trace . toList (). stream (). forEach Ġ{ Ġx Ġ-> Ġtelemetry . add Line ( x . toString ()) Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġreturn ĠPair ( boxes , Ġret ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġsize Estimate ( range : ĠAngle Rect ): ĠVector 2 d ĊĠĠĠĠĠĠĠ Ġ= ĠVector 2 d ( cos (- range . alpha ), Ġsin (- range . alpha )) Ġ* Ġestimate Distance ( range . width ) ĊĊĠĠĠ Ġprivate Ġfun Ġestimate Vector ( range : ĠAngle Rect ) ĊĠĠĠĠĠĠĠ Ġ= Ġ( Vector 2 d ( 1 . 0 , Ġtan (- range . alpha )) Ġ* ĠVision Constants . CAMERA _ HEIGHT Ġ/ Ġtan ( Vision Constants . CAMERA _ VERTICAL _ ANGLE Ġ- Ġrange . bottom )). take If Ġ{ Ġit . x Ġ> Ġ 0 . 0 Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġprocess Frame ( input : ĠMat ?): ĠMat Ġ{ ĊĠĠĠĠĠĠĠ Ġ// telemetry . update () ĊĠĠĠĠĠĠĠ Ġval Ġ( boxes , Ġret Image ) Ġ= Ġget Boxes ( input ) ĊĊĠĠĠĠĠĠĠ Ġ// val Ġtop 3 Ġ= Ġboxes . sub List ( 0 , Ġmin ( boxes . size , Ġ 3 )) ĊĠĠĠĠĠĠĠ Ġ// be ams Ġ= Ġtop 3 . map Ġ{ Ġrange ( it ) Ġ} ĊĊĠĠĠĠĠĠĠ Ġbe ams Ġ= Ġboxes . map Ġ{ Ġrange ( it ) Ġ} ĊĊĠĠĠĠĠĠĠ Ġval Ġabs Ġ= Ġab sol utes ĊĠĠĠĠĠĠĠ Ġif Ġ( abs . isNotEmpty () Ġ/* && ĠOp Mode . DEBUG */) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġdraw Ring ( abs [ 0 ]) ĊĊĠĠĠĠĠĠĠ Ġreturn Ġret Image ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġabsolute ( v : ĠVector 2 d ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ= Ġ( bot . dt . pose Estimate . vec () Ġ+ Ġ( Vision Constants . camera Pose . vec () Ġ+ Ġv . rot ated ( Vision Constants . CAMERA _ TH ETA )). rot ated ( bot . dt . pose Estimate . heading )). take If Ġ{ Ġabs ( it . x ) Ġ< Ġ 7 2 . 0 Ġ&& Ġabs ( it . y ) Ġ< Ġ 7 2 . 0 Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġdraw Ring ( v : ĠVector 2 d ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġpacket Ġ= ĠTe lemetry Packet () ĊĠĠĠĠĠĠĠ Ġpacket . put (" ring X ", Ġv . x ) ĊĠĠĠĠĠĠĠ Ġpacket . put (" ring Y ", Ġv . y ) ĊĊĠĠĠĠĠĠĠ Ġval Ġfield Overlay Ġ= Ġpacket . field Overlay () ĊĠĠĠĠĠĠĠ Ġfield Overlay . setFill ("# dda 2 7 7 ") ĊĠĠĠĠĠĠĠ Ġfield Overlay . stroke Circle ( v . x , Ġv . y , Ġ 2 . 5 ) ĊĠĠĠĠĠĠĠ Ġfield Overlay . setFill ("# e 1 e 1 e 1 ") ĊĠĠĠĠĠĠĠ Ġfield Overlay . fill Circle ( v . x , Ġv . y , Ġ 1 . 5 ) ĊĊĠĠĠĠĠĠĠ Ġdashboard . send Telemetry Packet ( packet ) ĊĠĠĠ Ġ} Ċ } Ċ