package Ġorg . first insp ires . ft c . team code . internal . units Ċ Ċ import Ġcom . qual comm . ft c robot controller . R Ċ import Ġcom . qual comm . robot core . hardware . Dc Motor Ċ import Ġcom . qual comm . robot core . util . Elapsed Time Ċ import Ġorg . first insp ires . ft c . team code . internal . config . Dc Motor Info Ċ import Ġkotlin . math . log Ċ Ċ /** Ċ Ġ* Ġ è¿Ļæĺ¯ åºķ çĽĺ æİ§åĪ¶ ç±» ãĢĤ Ċ Ġ* Ġ åºķ çĽĺ æĺ¯ ä¸Ģ åı° æľºåĻ¨ äºº æľĢ éĩįè¦ģ çļĦ ç»Ħä»¶ ä¹ĭ ä¸Ģ ï¼Į åĽłæŃ¤ å¦Ĥ ä½ł æīĢ è§ģ è¿Ļä¸ª ç±»çļĦ ä½ĵ ç§¯ éĿŀå¸¸ åº ŀ å¤§ ãĢĤ Ċ Ġ*/ Ċ class ĠCh assis : ĠUnit () Ġ{ ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ åºķ çĽĺ é©¬ è¾¾ ç¡¬ ä»¶ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġfront Left Motor : ĠD c Motor Ġ= ĠD c Motor Info . build Motor ( config (). FRONT _ LEFT _ MOTOR )// å·¦ åīį æĸ¹ ĊĠĠĠ Ġvar Ġrear Left Motor : ĠD c Motor Ġ= ĠD c Motor Info . build Motor ( config (). RE AR _ LEFT _ MOTOR )// å·¦ åĲİ æĸ¹ ĊĠĠĠ Ġvar Ġfront Right Motor : ĠD c Motor Ġ= ĠD c Motor Info . build Motor ( config (). FRONT _ RIGHT _ MOTOR )// åı³ åīį æĸ¹ ĊĠĠĠ Ġvar Ġrear Right Motor : ĠD c Motor Ġ= Ġ ĠD c Motor Info . build Motor ( config (). RE AR _ RIGHT _ MOTOR )// åı³ åĲİ æĸ¹ ĊĊĠĠĠ Ġ// éĻ Ģ èŀ º ä» ª çļĦ PID ĊĠĠĠ Ġprivate Ġval Ġpid IM U Ġ= ĠPID ( config (). CH ASS IS _ ROT ATE _ PID , Ġconfig (). CH ASS IS _ ROT ATE _ BL IND ) ĊĠĠĠ Ġprivate Ġvar Ġlast Angle A im Ġ= Ġ 0 . 0 ĊĠĠĠ Ġ// TF çļĦ PID ĊĠĠĠ Ġprivate Ġval Ġpid TF Ġ= ĠPID ( config (). CH ASS IS _ TF _ PID , Ġconfig (). CH ASS IS _ TF _ BL IND ) ĊĠĠĠ Ġ// ç¼ĸçłģ åĻ¨ çļĦ PID ĊĠĠĠ Ġprivate Ġval Ġpid Encoder Ġ= ĠPID ( config (). CH ASS IS _ EN CODER _ PID , Ġconfig (). CH ASS IS _ EN CODER _ BL IND ) ĊĊĠĠĠ Ġ// æĸ¹åĲĳ æĺ¯åĲ¦ åħ¨éĥ¨ æĶ¹åıĺ ĊĠĠĠ Ġvar Ġdirection All Changed Ġ= Ġfalse ĊĠĠĠ Ġprivate Ġval Ġtime Ġ= ĠE lapsed Time () ĊĊĠĠĠ Ġ// æŀĦéĢł åĩ½æķ° ĊĠĠĠ Ġinit Ġ{ ĊĠĠĠĠĠĠĠ Ġtime . reset () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġè®¾ç½® åºķ çĽĺ åĬŁ çİĩ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Power ( left Power : ĠDouble /* å·¦ é©¬ è¾¾ åĬŁ çİĩ */, Ġright Power : ĠDouble /* åı³ é©¬ è¾¾ åĬŁ çİĩ */, Ġtrans verse Power : ĠDouble Ġ= Ġ 0 . 0 /* å¹³ ç§» åĬŁ çİĩ */) Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ( direction All Changed ) Ġ{// è¿ĻéĩĮ çļĦ åĪ¤æĸŃ çĶ¨äºİ è¿Ľè¡Į åºķ çĽĺ åıį åĲĳ æĵįä½ľ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfront Left Motor . power Ġ= Ġright Power Ġ+ Ġtrans verse Power ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfront Right Motor . power Ġ= Ġleft Power Ġ- Ġtrans verse Power ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrear Left Motor . power Ġ= Ġright Power Ġ- Ġtrans verse Power ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrear Right Motor . power Ġ= Ġleft Power Ġ+ Ġtrans verse Power ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfront Left Motor . power Ġ= Ġleft Power Ġ+ Ġtrans verse Power ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfront Right Motor . power Ġ= Ġright Power Ġ- Ġtrans verse Power ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrear Left Motor . power Ġ= Ġleft Power Ġ- Ġtrans verse Power ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrear Right Motor . power Ġ= Ġright Power Ġ+ Ġtrans verse Power ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ éĢ Ĳ ä¸Ģ è®¾ç½® é©¬ è¾¾ çļĦ åĬŁ çİĩ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Power One By One ( power : ĠDouble Array ) Ġ{ ĊĠĠĠĠĠĠĠ Ġfront Left Motor . power Ġ= Ġpower [ 0 ] ĊĠĠĠĠĠĠĠ Ġfront Right Motor . power Ġ= Ġpower [ 1 ] ĊĠĠĠĠĠĠĠ Ġrear Left Motor . power Ġ= Ġpower [ 2 ] ĊĠĠĠĠĠĠĠ Ġrear Right Motor . power Ġ= Ġpower [ 3 ] ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠéĢļè¿ĩ æīĭ æŁ Ħ è®¾ç½® åºķ çĽĺ åĬŁ çİĩ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Power By Gamepad () Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġgame pad Ġ= Ġgame pad 1 () // éĢīæĭ© æīĢ ä½¿çĶ¨çļĦ æīĭ æŁ Ħ ĊĠĠĠĠĠĠĠ Ġset Power ( gamepad . left _ stick _ x * config (). CH ASS IS _ T URN _ K Ġ- Ġgame pad . left _ stick _ y . toDouble (), ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ- gamepad . left _ stick _ x * config (). CH ASS IS _ T URN _ K Ġ- Ġgame pad . left _ stick _ y . toDouble (), ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ- gamepad . left _ trigger Ġ+ Ġgame pad . right _ trigger . toDouble ()) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ èĩªåĬ¨ å¯ » æī¾ éĩĳ çŁ ¿ çŁ³ ï¼Į åľ¨ ç¬¬äºĮ ä»£ çļĦ èĩªåĬ¨ ä¸Ń æ²¡æľī ä½¿çĶ¨ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġsearch Gold Min eral (): ĠInt Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ(! op Mode (). run Tensor Flow ) Ġreturn Ġ- 1 ĊĠĠĠĠĠĠĠ Ġval Ġtensor Flow Ġ= Ġop Mode (). tensor Flow ĊĠĠĠĠĠĠĠ Ġval Ġtime Ġ= ĠE lapsed Time () ĊĠĠĠĠĠĠĠ Ġtensor Flow . activate () ĊĠĠĠĠĠĠĠ Ġtime . reset () ĊĠĠĠĠĠĠĠ Ġset Angle ( 0 . 0 , Ġ 1 5 0 0 ) ĊĊĠĠĠĠĠĠĠ Ġ// æĪĲåĬŁ è¯ĨåĪ« ĊĠĠĠĠĠĠĠ Ġfun Ġsuccess (): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġposition Ġ= Ġtensor Flow . get Gold Position () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġx Ġ= Ġposition [ 0 ] ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġy Ġ= Ġposition [ 1 ] ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġ( y Ġin Ġconfig (). Image Height . toDouble ()* 0 . 2 5 .. config (). Image Height . toDouble ()* 0 . 7 5 ) Ġ&& ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ( x Ġ> Ġconfig (). Image Width . toDouble ()* 0 . 5 0 ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( success ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtensor Flow . deactivate () ĊĠĠĠĠĠĠĠĠĠĠĠĠ 2 // ä¸ŃéĹ´ ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġset Angle ( 4 5 . 0 , Ġ 2 0 0 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( success ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtensor Flow . deactivate () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ 3 // åı³ è¾¹ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġset Angle (- 4 5 . 0 , Ġ 2 0 0 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( success ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtensor Flow . deactivate () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ 1 // å·¦ è¾¹ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtensor Flow . deactivate () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ 0 // æ²¡æľī ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ å»¶ æĹ¶ æİ§åĪ¶ ï¼Į æĹ¶éĹ´ ä¸ĩ å² ģ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Target By Time ( left Power : ĠDouble , Ġright Power : ĠDouble , Ġtrans verse Power : ĠDouble Ġ= Ġ 0 . 0 , Ġmilli Seconds : ĠLong ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġstart MS Ġ= ĠSystem . currentTimeMillis () ĊĠĠĠĠĠĠĠ Ġwhile Ġ( op Mode IsActive () Ġ&& ĠSystem . currentTimeMillis ()- start MS Ġ< Ġmilli Seconds ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġset Power ( left Power , Ġright Power , Ġtrans verse Power ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 , Ġ 0 . 0 ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠçĶ¨ ç¼ĸçłģ åĻ¨ æİ§åĪ¶ è¡Į è¿Ľ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Str aight By Encoder ( target : ĠInt , Ġout Time : ĠInt Ġ= Ġ 2 5 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġset Target By Encoder ( true , Ġtarget , Ġout Time ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠçĶ¨ ç¼ĸçłģ åĻ¨ æİ§åĪ¶ æĸ ľ åĲĳ 4 5 Â° åīį è¿Ľ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Ob lique By Encoder ( target : ĠInt , Ġright : ĠBoolean , Ġout Time : ĠInt Ġ= Ġ 3 0 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġzero Power Behavior : ĠD c Motor . Zero Power Behavior Ġ= Ġfront Left Motor . zero Power Behavior ĊĠĠĠĠĠĠĠ Ġval Ġaim Ġ= Ġif Ġ( right ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtarget Ġ+ Ġfront Left Motor . current Position ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtarget Ġ+ Ġfront Right Motor . current Position ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġset Zero Power Behavior ( Dc Motor . Zero Power Behavior . FLOAT ) ĊĠĠĠĠĠĠĠ Ġif Ġ( right ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfront Right Motor . zero Power Behavior Ġ= ĠD c Motor . Zero Power Behavior . BR A KE ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrear Left Motor . zero Power Behavior Ġ= ĠD c Motor . Zero Power Behavior . BR A KE ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfront Left Motor . zero Power Behavior Ġ= ĠD c Motor . Zero Power Behavior . BR A KE ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrear Right Motor . zero Power Behavior Ġ= ĠD c Motor . Zero Power Behavior . BR A KE ĊĠĠĠĠĠĠĠ Ġ} Ċ // ĠĠĠĠĠĠĠ Ġop Mode (). telemetry Log (" å®ŀéĻħ :${ front Left Motor . current Position }, çĽ®æłĩ :${ aim }") ĊĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġvar Ġfinished Ġ= Ġ 0 ĊĠĠĠĠĠĠĠ Ġval ĠstartTime Ġ= ĠSystem . currentTimeMillis () ĊĠĠĠĠĠĠĠ Ġwhile Ġ( op Mode IsActive () Ġ&& ĠSystem . currentTimeMillis ()- startTime < out Time ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( right ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// åĲĳ åı³ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġpower Ġ= Ġpid Encoder . run ( front Left Motor . current Position . toDouble (), Ġaim . toDouble ()) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpower Ġ= ĠMath . sign um ( power ) Ġ* ĠMath . min ( Math . abs ( power ), Ġconfig (). CH ASS IS _ MAX _ POWER ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġset Power One By One ( double ArrayOf ( power , Ġ 0 . 0 , Ġ 0 . 0 , Ġpower )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// åĲĳ å·¦ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġpower Ġ= Ġpid Encoder . run ( front Right Motor . current Position . toDouble (), Ġaim . toDouble ()) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpower Ġ= ĠMath . sign um ( power ) Ġ* ĠMath . min ( Math . abs ( power ), Ġconfig (). CH ASS IS _ MAX _ POWER ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġset Power One By One ( double ArrayOf ( 0 . 0 , Ġpower , Ġpower , Ġ 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( pid Encoder . isIn Err Allow able ) Ġfinished ++ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġfinished Ġ= Ġ 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( finished Ġ>= Ġ 3 ) Ġbreak ĊĠĠĠĠĠĠĠĠĠĠĠ ĠThread . sleep ( 1 0 ) Ċ // ĠĠĠĠĠĠĠĠĠĠĠ Ġpid Encoder . debug () Ċ // ĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry (). update () ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġset Zero Power Behavior ( zero Power Behavior ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠçĶ¨ ç¼ĸçłģ åĻ¨ æİ§åĪ¶ å¹³ ç§» ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Trans verse By Encoder ( target : ĠInt , Ġout Time : ĠInt Ġ= Ġ 3 0 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġset Target By Encoder ( false , Ġtarget , Ġout Time ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ ç¼ĸçłģ åĻ¨ æİ§åĪ¶ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġset Target By Encoder ( stra ight : ĠBoolean , Ġtarget : ĠInt , Ġout Time : ĠInt ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġzero Power Behavior Ġ= Ġfront Left Motor . zero Power Behavior ĊĠĠĠĠĠĠĠ Ġset Zero Power Behavior ( Dc Motor . Zero Power Behavior . FLOAT ) ĊĠĠĠĠĠĠĠ Ġval Ġaim Ġ= Ġtarget Ġ+ Ġfront Left Motor . current Position Ċ // ĠĠĠĠĠĠĠ Ġop Mode (). telemetry Log (" å®ŀéĻħ :${ front Left Motor . current Position }, çĽ®æłĩ :${ aim }") ĊĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġvar Ġfinished Ġ= Ġ 0 ĊĠĠĠĠĠĠĠ Ġval ĠstartTime Ġ= ĠSystem . currentTimeMillis () ĊĠĠĠĠĠĠĠ Ġwhile Ġ( op Mode IsActive () Ġ&& ĠSystem . currentTimeMillis ()- startTime < out Time ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġpower Ġ= Ġpid Encoder . run ( front Left Motor . current Position . toDouble (), Ġaim . toDouble ()) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġpower Ġ= ĠMath . sign um ( power ) Ġ* ĠMath . min ( Math . abs ( power ), Ġconfig (). CH ASS IS _ MAX _ POWER ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( stra ight ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġset Power ( power , Ġpower , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 , Ġpower ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( pid Encoder . isIn Err Allow able ) Ġfinished ++ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġfinished Ġ= Ġ 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( finished Ġ>= Ġ 3 ) Ġbreak ĊĠĠĠĠĠĠĠĠĠĠĠ ĠThread . sleep ( 1 0 ) Ċ // ĠĠĠĠĠĠĠĠĠĠĠ Ġpid Encoder . debug () Ċ // ĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry (). update () ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġset Zero Power Behavior ( zero Power Behavior ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠçĶ¨ Tensor Flow æİ§åĪ¶ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġset Trans verse Target By TF ( target : ĠDouble , Ġout Time : ĠInt Ġ= Ġ 2 5 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġif Ġ(! op Mode (). run Tensor Flow ) Ġreturn // çĽ´æİ¥ è¿ĶåĽŀ ĊĠĠĠĠĠĠĠ Ġval Ġzero Power Behavior Ġ= Ġfront Left Motor . zero Power Behavior ĊĠĠĠĠĠĠĠ Ġset Zero Power Behavior ( Dc Motor . Zero Power Behavior . FLOAT ) ĊĠĠĠĠĠĠĠ Ġval ĠstartTime Ġ= ĠSystem . currentTimeMillis () ĊĠĠĠĠĠĠĠ Ġwhile Ġ( op Mode IsActive () Ġ&& ĠSystem . currentTimeMillis ()- startTime < out Time ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( Math . abs ( op Mode (). imu . get Angle ( IM U . Direction . X )) Ġ>= Ġ 8 ) Ġset Angle ( 0 . 0 , Ġ 5 0 0 )// åģı å·® è¿ĩ å¤§ æł¡ åĩĨ è§Ĵ åº¦ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpower Ġ= Ġpid TF . run ( op Mode (). tensor Flow . get Gold Position ()[ 1 ], Ġtarget ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 , Ġpower ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( pid TF . isIn Err Allow able ) Ġbreak // è¿Ľåħ¥ çĽ®æłĩ èĮĥåĽ´ ï¼Į è·³ åĩº ĊĠĠĠĠĠĠĠĠĠĠĠ ĠThread . sleep ( 1 0 ) ĊĠĠĠĠĠĠĠ Ġ} Ċ // ĠĠĠĠĠĠĠ Ġset Trans verse By Encoder ( 1 0 0 ) ĊĠĠĠĠĠĠĠ Ġset Angle ( 0 . 0 , Ġ 1 0 0 0 )// æł¡ åĩĨ è§Ĵ åº¦ ĊĠĠĠĠĠĠĠ Ġset Zero Power Behavior ( zero Power Behavior ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġè®¾ç½® åºķ çĽĺ æľ Ŀ åĲĳ ï¼Į éľĢè¦ģ çĶ¨ åĪ° IM U ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Angle ( angle : ĠDouble , Ġout Time : ĠInt Ġ= Ġ 2 5 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ(! op Mode (). run IM U ) Ġreturn // å¦Ĥæŀľ ä¸į åĲ¯åĬ¨ IM U ï¼Į åĪĻ çĽ´æİ¥ è¿ĶåĽŀ ĊĠĠĠĠĠĠĠ Ġvar Ġpid IM U In Bl ind Ġ= Ġ 0 ĊĠĠĠĠĠĠĠ Ġval Ġzero Power Behavior Ġ= Ġfront Left Motor . zero Power Behavior ĊĠĠĠĠĠĠĠ Ġset Zero Power Behavior ( Dc Motor . Zero Power Behavior . BR A KE ) ĊĠĠĠĠĠĠĠ Ġval Ġim u Ġ= Ġop Mode (). imu // è¿ĶåĽŀ éĻ Ģ èŀ º ä» ª çļĦ çĽĳ æµĭ åĻ¨ ĊĠĠĠĠĠĠĠ Ġval ĠstartTime Ġ= ĠSystem . currentTimeMillis () ĊĠĠĠĠĠĠĠ Ġwhile Ġ( op Mode IsActive () Ġ&& ĠSystem . currentTimeMillis ()- startTime < out Time ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġactual Ġ= Ġim u . get Angle With Cycle ( IM U . Direction . X ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġtarget Ġ= Ġim u . convert Angle ( IM U . Direction . X , Ġangle ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpower Ġ= Ġpid IM U . run ( actual , Ġtarget )// è®¡ç®Ĺ åĬŁ çİĩ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġset Power ( power , Ġ- power , Ġ 0 . 0 )// ç»Ļ é©¬ è¾¾ è¾ĵåĩº åĬŁ çİĩ Ċ // ĠĠĠĠĠĠĠ Ġpid IM U . debug ( op Mode ()); telemetry (). update () // debug éĥ¨åĪĨ ï¼Į åı¯ä»¥ æ³¨éĩĬ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( pid IM U . isIn Err Allow able ) Ġpid IM U In Bl ind ++ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġpid IM U In Bl ind Ġ= Ġ 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( pid IM U In Bl ind Ġ>= Ġ 3 ) Ġbreak ĊĠĠĠĠĠĠĠĠĠĠĠ ĠThread . sleep ( 1 0 ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġset Power ( 0 . 0 , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġset Zero Power Behavior ( zero Power Behavior ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠéĢļè¿ĩ æīĭ æŁ Ħ æİ¨ æĿ Ĩ è®¾ç½® åºķ çĽĺ æĸ¹åĲĳ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Angle By Gamepad () Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ( pid IM U . isIn Err Allow able ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġset Power ( get Angle By Gamepad ()[ 1 ], Ġget Angle By Gamepad ()[ 1 ]) ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġset Angle ( get Angle By Gamepad ()[ 0 ]) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠéĢļè¿ĩ æīĭ æŁ Ħ æİ¨ æĿ Ĩ æĿ¥ èİ·åıĸ æĸ¹åĲĳ åĴĮ éĢŁåº¦ ï¼Ī åį³ åĲĳ éĩı ä¸İ åĿĲæłĩ è½´ å¤¹ è§Ĵ åĴĮ æķ°éĩı ç§¯ ï¼ī ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġget Angle By Gamepad (): ĠDouble Array Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġgame pad Ġ= Ġgame pad 1 () ĊĠĠĠĠĠĠĠ Ġreturn Ġif Ġ( gamepad . right _ stick _ x . equals ( 0 ) Ġ&& Ġgame pad . right _ stick _ y . equals ( 0 )) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( last Angle A im , Ġ 0 . 0 )// å½ĵ æīĭ æŁ Ħ ä¸į æĭ ¨ åĬ¨ æĹ¶ ï¼Į ä¿ĿæĮģ ä¸Ĭ ä¸Ģæ¬¡ çļĦ çĬ¶æĢģ ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġif Ġ( gamepad . right _ stick _ x . equals ( 0 ) Ġ&& Ġ! gamepad . right _ stick _ y . equals ( 0 )) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġlast Angle A im Ġ= ĠMath . sign um (- gamepad . right _ stick _ y )* 9 0 . 0 // æĽ´æĸ° ä¸Ĭ æ¬¡ çĽ®æłĩ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( last Angle A im , ĠMath . abs ( gamepad . right _ stick _ y ). toDouble ()) // æĸ ľ çİĩ ä¸įåŃĺåľ¨ åĪĻ ä¸º Â ± 9 0 Â° ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġlast Angle A im Ġ= ĠMath . atan 2 (- gamepad . right _ stick _ y . toDouble (), Ġgame pad . right _ stick _ x . toDouble ()) / Math . PI * 1 8 0 // æĽ´æĸ° ä¸Ĭ æ¬¡ çĽ®æłĩ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( last Angle A im , ĠMath . sqrt (( gamepad . right _ stick _ x * gamepad . right _ stick _ x ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ+ Ġgame pad . right _ stick _ y * gamepad . right _ stick _ y ). toDouble ())) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// æĸ ľ çİĩ åŃĺåľ¨ åĪĻ ä»£ åħ¥ çĤ¹ è®¡ç®Ĺ ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ é©¬ è¾¾ æĸ¹åĲĳ åħ¨éĥ¨ æĶ¹åıĺ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġdirection All Change () Ġ{ ĊĠĠĠĠĠĠĠ Ġdirection All Changed Ġ= Ġ! direction All Changed ĊĠĠĠĠĠĠĠ Ġif Ġ( direction All Changed ) Ġop Mode (). sound ( R . raw . high _ up _ model ) ĊĠĠĠĠĠĠĠ Ġelse Ġop Mode (). sound ( R . raw . cr ater _ model ) ĊĠĠĠĠĠĠĠ Ġfront Left Motor . direction Ġ= ĠD c Motor Info . direction Change ( front Left Motor ) ĊĠĠĠĠĠĠĠ Ġfront Right Motor . direction Ġ= ĠD c Motor Info . direction Change ( front Right Motor ) ĊĠĠĠĠĠĠĠ Ġrear Left Motor . direction Ġ= ĠD c Motor Info . direction Change ( rear Left Motor ) ĊĠĠĠĠĠĠĠ Ġrear Right Motor . direction Ġ= ĠD c Motor Info . direction Change ( rear Right Motor ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ èĩªåĬ¨ ä¿®æĶ¹ æĸ¹åĲĳ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġauto Direction Change () Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ( time . milliseconds () Ġ>= Ġ 5 0 0 Ġ&& Ġconfig (). ch assis Direction Change ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġdirection All Change () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtime . reset () ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ éĩį ç½® åºķ çĽĺ ç¼ĸçłģ åĻ¨ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġreset Encoder () Ġ{ ĊĠĠĠĠĠĠĠ ĠD c Motor Info . reset Encoder ( front Left Motor ) ĊĠĠĠĠĠĠĠ ĠD c Motor Info . reset Encoder ( front Right Motor ) ĊĠĠĠĠĠĠĠ ĠD c Motor Info . reset Encoder ( rear Left Motor ) ĊĠĠĠĠĠĠĠ ĠD c Motor Info . reset Encoder ( rear Right Motor ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ åĽŀ ä¼ł ä½įç½® ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġtelemetry Position () Ġ{ ĊĠĠĠĠĠĠĠ Ġtelemetry (). addData (" fl ", Ġfront Left Motor . current Position ) ĊĠĠĠĠĠĠĠ Ġtelemetry (). addData (" fr ", Ġfront Right Motor . current Position ) ĊĠĠĠĠĠĠĠ Ġtelemetry (). addData (" rl ", Ġrear Left Motor . current Position ) ĊĠĠĠĠĠĠĠ Ġtelemetry (). addData (" rr ", Ġrear Right Motor . current Position ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġè®¾ç½® é©¬ è¾¾ åĪ ¹ è½¦ ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Zero Power Behavior ( zero Power Behavior : ĠD c Motor . Zero Power Behavior ) Ġ{ ĊĠĠĠĠĠĠĠ ĠThread . sleep ( 1 0 ) ĊĠĠĠĠĠĠĠ Ġfront Left Motor . zero Power Behavior Ġ= Ġzero Power Behavior ĊĠĠĠĠĠĠĠ Ġfront Right Motor . zero Power Behavior Ġ= Ġzero Power Behavior ĊĠĠĠĠĠĠĠ Ġrear Left Motor . zero Power Behavior Ġ= Ġzero Power Behavior ĊĠĠĠĠĠĠĠ Ġrear Right Motor . zero Power Behavior Ġ= Ġzero Power Behavior ĊĠĠĠĠĠĠĠ ĠThread . sleep ( 1 0 ) ĊĠĠĠ Ġ} Ċ Ċ }