package Ġorg . first insp ires . ft c . team code . internal . config Ċ Ċ import Ġcom . qual comm . robot core . hardware . Dc Motor Ċ import Ġcom . qual comm . robot core . hardware . Dc Motor Simple Ċ import Ġcom . qual comm . robot core . hardware . Ser vo Ċ import Ġorg . first insp ires . ft c . robot core . external . navigation . Vu for ia Local izer Ċ import Ġorg . first insp ires . ft c . team code . internal . Base Op Mode Ċ Ċ /** Ċ Ġ* Ġ è¿Ļæĺ¯ UST ç¨ĭåºı ä¸Ń èĩ³ åħ³ éĩįè¦ģ çļĦä¸Ģä¸ª ç±» ï¼Į ä»ĸ åŃĺåĤ¨ çĿĢ UST æľºåĻ¨ äººçļĦ æīĢæľī éħįç½® ãĢģ å¸¸ éĩı åĴĮ éĩįè¦ģ å®ŀä¾ĭ ãĢĤ Ċ Ġ* Ġ è¿Ļä¸ª æĥ³ æ³ķ æĺ¯ 2 0 1 7 ~ 2 0 1 8 èµĽ åŃ £ LOG æĢ» ç»ĵ åĩº æĿ¥çļĦ ï¼Į å®ĥ æŀģ å¤§çļĦ æıĲ é«ĺ äºĨ ç¨ĭåºı åĳĺ çļĦ æŁ¥ éĶĻ èĥ½åĬĽ ï¼Į çĶļ èĩ³ åı¯ä»¥ å¸®åĬ© ä¸įä¼ļ ç¨ĭåºı çļĦ ç»ĵæŀĦ ç»Ħ ä¸´ æĹ¶ ä¿®æĶ¹ åıĤæķ° ãĢĤ Ċ Ġ* Ġ ç»´æĬ¤ å¥½ è¿Ļä¸ª ç±» å°Ĩ å¸®åĬ© ä½ł ä»¬ çļĦ éĺŁ ä¼ į èİ·å¾Ĺ æĽ´ å¤§çļĦ ç« ŀ äºī åĬĽ ãĢĤ Ċ Ġ*/ Ċ object ĠConfig Ġ{ ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ å®ŀä¾ĭ åĮº <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ// æŃ£åľ¨ è¿Ĳè¡Į çļĦ Linear Op Mode ĊĠĠĠ Ġlateinit Ġvar Ġop Mode : ĠBase Op Mode ĊĊĠĠĠ Ġ// åįķ ä¾ĭ Vu for ia ĊĠĠĠ Ġvar Ġv u for ia : ĠV u for ia Local izer ? Ġ= Ġnull ĊĠĠĠ Ġ// Vu for ia çļĦ Key ĊĠĠĠ Ġconst Ġval ĠV U FOR IA _ KEY Ġ= Ġ" AW 3 B bq H ///// AAAA m Xv EK Ye BK k QS uw ntl Of 9 l 5 4 b T ts b ZG wf Tf Ud V Sc y 6 z XY 4 1 Hd T 0 Q 5 / RL MX D Nd 3 jy 2 v 4 / A + fW t I 8 n 8 C GO IS /// h L 1 W Id zm U ut 3 j J 5 XV SI BP b SM H SU x I 2 V + M 0 e Y NX ap 7 4 3 Zm LS VK Ci Py W sv di Zs 1 Vw Es 8 Ep Y xx NW Jg k IT 1 jy X hm Rr ath 1 0 IN BW 5 BCM HM 4 Y 8 Y Rj Mo 3 Be 1 nx 4 nb 9 X 0 t R 1 Vi 2 l wt jq PN q ML 7 8 csv z I 9 J sb IV / F 7 S + IB J XO 7 GH j ZQ d K jb 6 b 7 N 0 pb 0 B ft 7 9 IPS v 0 8 AP y X du Wd 7 9 L dp AY / q RB NL X 4 5 d PU Vn 2 w BW j 9 ZM qk DT + f SH 7 c + v W KL hq eg / t 2 J RO 1 rs J MB 8 v XN r GJ t T ml tq UN p GG Dx 7 8 u " ĊĠĠĠ Ġ// Vu for ia æĳ Ħ åĥı å¤´ æĸ¹åĲĳ ĊĠĠĠ Ġval ĠCAM ERA _ DIRECTION Ġ= ĠV u for ia Local izer . Camera Direction . FRONT ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ å®ŀä¾ĭ åĮº >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> ĊĠĠĠĠ Ġ*/ ĊĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ ç¡¬ ä»¶ åĪĿå§ĭåĮĸ <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<< ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ// åºķ çĽĺ é©¬ è¾¾ ĊĠĠĠ Ġval ĠFR ONT _ LEFT _ MOTOR Ġ= ĠD c Motor Info (" m 0 ", ĠD c Motor Simple . Direction . FORWARD , ĠD c Motor . Zero Power Behavior . FLOAT , ĠD c Motor . Run Mode . RUN _ USING _ EN CODER )// å·¦ åīį æĸ¹ é©¬ è¾¾ ĊĠĠĠ Ġval ĠRE AR _ LEFT _ MOTOR Ġ= ĠD c Motor Info (" m 1 ", ĠD c Motor Simple . Direction . FORWARD , ĠD c Motor . Zero Power Behavior . FLOAT , ĠD c Motor . Run Mode . RUN _ USING _ EN CODER )// å·¦ åĲİ æĸ¹ é©¬ è¾¾ ĊĠĠĠ Ġval ĠFR ONT _ RIGHT _ MOTOR Ġ= ĠD c Motor Info (" m 2 ", ĠD c Motor Simple . Direction . RE VERSE , ĠD c Motor . Zero Power Behavior . FLOAT , ĠD c Motor . Run Mode . RUN _ USING _ EN CODER )// åı³ åīį æĸ¹ é©¬ è¾¾ ĊĠĠĠ Ġval ĠRE AR _ RIGHT _ MOTOR Ġ= ĠD c Motor Info (" m 3 ", ĠD c Motor Simple . Direction . RE VERSE , ĠD c Motor . Zero Power Behavior . FLOAT , ĠD c Motor . Run Mode . RUN _ USING _ EN CODER )// åı³ åĲİ æĸ¹ é©¬ è¾¾ ĊĊĠĠĠ Ġ// åįĩ éĻį é©¬ è¾¾ ĊĠĠĠ Ġval ĠLEFT _ LI FT _ MOTOR Ġ= ĠD c Motor Info (" m 4 ", ĠD c Motor Simple . Direction . RE VERSE , ĠD c Motor . Zero Power Behavior . FLOAT , ĠD c Motor . Run Mode . RUN _ USING _ EN CODER )// å·¦ ä¾§ åįĩ éĻį é©¬ è¾¾ ĊĠĠĠ Ġval ĠRIGHT _ LI FT _ MOTOR Ġ= ĠD c Motor Info (" m 5 ", ĠD c Motor Simple . Direction . RE VERSE , ĠD c Motor . Zero Power Behavior . FLOAT , ĠD c Motor . Run Mode . RUN _ USING _ EN CODER )// åı³ ä¾§ åįĩ éĻį é©¬ è¾¾ ĊĊĠĠĠ Ġ// æľº æ¢ ° èĩ Ĥ ĊĠĠĠ Ġval ĠARM _ DOWN _ MOTOR Ġ= ĠD c Motor Info (" m 6 ", ĠD c Motor Simple . Direction . FORWARD , ĠD c Motor . Zero Power Behavior . FLOAT , ĠD c Motor . Run Mode . RUN _ WITHOUT _ EN CODER )// ä¸ĭ éĿ¢çļĦ ĊĠĠĠ Ġval ĠARM _ UP _ MOTOR Ġ= ĠD c Motor Info (" m 7 ", ĠD c Motor Simple . Direction . RE VERSE , ĠD c Motor . Zero Power Behavior . FLOAT , ĠD c Motor . Run Mode . RUN _ USING _ EN CODER )// ä¸Ĭ éĿ¢çļĦ ĊĠĠĠ Ġval ĠARM _ E JECT Ġ= ĠSer vo Info (" s 2 ", ĠSer vo . Direction . FORWARD , Ġdouble ArrayOf ( 0 . 0 , Ġ 1 . 0 )) // å¼¹ åĩº æľº æ¢ ° èĩ Ĥ ĊĊĠĠĠ Ġ// æĶ¶éĽĨ ä¼ º æľį ĊĠĠĠ Ġval ĠCOL LECT _ TA KE _ IN Ġ= ĠSer vo Info (" s 0 ", ĠSer vo . Direction . FORWARD , Ġdouble ArrayOf ( 0 . 0 , Ġ 1 . 0 )) // åĲ ¸ åħ¥ ä¼ º æľį ï¼Ī è¿ŀ ç»Ń è½¬ åĬ¨ ï¼ī ĊĠĠĠ Ġval ĠCOL LECT _ FIL P _ PL ATE Ġ= ĠSer vo Info (" s 1 ", ĠSer vo . Direction . FORWARD , Ġdouble ArrayOf ( 0 . 0 , Ġ 1 . 0 )) // ç¿» æĿ¿ ä¼ º æľį ĊĊĠĠĠ Ġ// IM U åĳ½åĲį ĊĠĠĠ Ġconst Ġval ĠI MU _ GET NAME Ġ= Ġ" imu " ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ ç¡¬ ä»¶ åĪĿå§ĭåĮĸ >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>> ĊĠĠĠĠ Ġ*/ ĊĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ å¸¸ éĩı åĮº <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< <<<<<<<< ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ// TF åĽ¾åĥı å¤§å°ı ĊĠĠĠ Ġvar ĠImage Height Ġ= Ġ 1 9 2 0 ĊĠĠĠ Ġvar ĠImage Width Ġ= Ġ 1 0 8 0 ĊĊĠĠĠ Ġ// çĶµ æľº ç¼ĸçłģ åĻ¨ ĊĠĠĠ Ġconst Ġval ĠNe ve Rest 4 0 Gear motor _ ticks Per Rev Ġ= Ġ 1 1 2 0 ĊĠĠĠ Ġconst Ġval ĠNe ve Rest 6 0 Gear motor _ ticks Per Rev Ġ= Ġ 1 6 8 0 ĊĊĠĠĠ Ġ// åºķ çĽĺ é©¬ è¾¾ çļĦ è½¬ åĲĳ ç³» æķ° ĊĠĠĠ Ġconst Ġval ĠCH ASS IS _ T URN _ K Ġ= Ġ 1 . 0 ĊĠĠĠ Ġ// è½¬ åĲĳ PID åĴĮ æŃ» åĮº ĊĠĠĠ Ġval ĠCH ASS IS _ ROT ATE _ PID Ġ= Ġdouble ArrayOf ( 0 . 0 2 2 , Ġ 0 . 0 , Ġ 0 . 0 2 5 ) ĊĠĠĠ Ġconst Ġval ĠCH ASS IS _ ROT ATE _ BL IND Ġ= Ġ 1 . 0 ĊĠĠĠ Ġ// TF çļĦ PID åĴĮ æŃ» åĮº ĊĠĠĠ Ġval ĠCH ASS IS _ TF _ PID Ġ= Ġdouble ArrayOf ( 0 . 0 0 0 4 , Ġ 0 . 0 , Ġ 0 . 0 0 0 1 ) ĊĠĠĠ Ġconst Ġval ĠCH ASS IS _ TF _ BL IND Ġ= Ġ 1 2 5 . 0 ĊĠĠĠ Ġ// ç¼ĸçłģ åĻ¨ çļĦ PID ĊĠĠĠ Ġval ĠCH ASS IS _ EN CODER _ PID Ġ= Ġdouble ArrayOf ( 0 . 0 0 3 0 , Ġ 0 . 0 , Ġ 0 . 0 0 5 ) ĊĠĠĠ Ġconst Ġval ĠCH ASS IS _ EN CODER _ BL IND Ġ= Ġ 2 0 . 0 ĊĠĠĠ Ġ// åºķ çĽĺ PID æİ§åĪ¶ æĹ¶çļĦ æľĢå¤§ åĬŁ çİĩ ĊĠĠĠ Ġconst Ġval ĠCH ASS IS _ MAX _ POWER Ġ= Ġ 1 . 0 ĊĊĠĠĠ Ġ// æľº æ¢ ° èĩ Ĥ PID åĴĮ æŃ» åĮº ĊĠĠĠ Ġval ĠARM _ PID Ġ= Ġdouble ArrayOf ( 0 . 0 0 5 , Ġ 0 . 0 , Ġ 0 . 0 0 1 ) ĊĠĠĠ Ġconst Ġval ĠARM _ BL IND Ġ= Ġ 0 . 0 ĊĠĠĠ Ġ// éĢŁåº¦ æ¯Ķ ä¾ĭ å¸¸ æķ° ĊĠĠĠ Ġconst Ġval Ġspeed K Ġ= Ġ 2 0 0 . 0 ĊĠĠĠ Ġ// æľº æ¢ ° èĩ Ĥ æľĢå¤§ åĬŁ çİĩ ĊĠĠĠ Ġconst Ġval ĠARM _ MAX _ POWER Ġ= Ġ 1 . 0 ĊĠĠĠ Ġ// æľº æ¢ ° èĩ Ĥ æľĢå¤§ ä½įç½® ĊĠĠĠ Ġconst Ġval ĠARM _ MAX _ POSITION Ġ= Ġ 2 5 5 0 ĊĠĠĠ Ġ// æľº æ¢ ° èĩ Ĥ ä¸ŃéĹ´ ä½įç½® ĊĠĠĠ Ġconst Ġval ĠARM _ MIDDLE _ POSITION Ġ= Ġ 1 0 0 0 ĊĊĠĠĠ Ġ// å¼¹ åĩº ä¼ º æľį ĊĠĠĠ Ġconst Ġval ĠARM _ E JECT _ OUT Ġ= Ġ 1 . 0 ĊĊĠĠĠ Ġ// åįĩ éĻį PID åĴĮ æŃ» åĮº ĊĠĠĠ Ġval ĠL IFT _ PID Ġ= Ġdouble ArrayOf ( 0 . 0 0 0 7 , Ġ 0 . 0 0 0 1 , Ġ 0 . 0 0 8 ) ĊĠĠĠ Ġconst Ġval ĠL IFT _ BL IND Ġ= Ġ 7 5 . 0 ĊĠĠĠ Ġ// åįĩ éĻį çļĦ æľĢå¤§ æł¡ æŃ£ åĬŁ çİĩ ĊĠĠĠ Ġconst Ġval ĠL IFT _ CORRECT _ MAX POWER Ġ= Ġ 0 . 3 5 ĊĠĠĠ Ġ// åĲĪ éĢĤ çļĦ é«ĺ æĮ Ĥ ä½įç½® ĊĠĠĠ Ġconst Ġval ĠL IFT _ HIGH UP _ POSITION Ġ= Ġ 1 0 8 0 0 ĊĊĠĠĠ Ġ// æĶ¶éĽĨ è£ħ ç½® å¸¸ éĩı ĊĠĠĠ Ġconst Ġval ĠCOL LECT _ TA KE _ IN _ IN TO Ġ= Ġ 1 . 0 // æĶ¶ åħ¥ ĊĠĠĠ Ġconst Ġval ĠCOL LECT _ TA KE _ IN _ OUT Ġ= Ġ 0 . 0 // åĲ Ĳ åĩº ĊĠĠĠ Ġconst Ġval ĠCOL LECT _ TA KE _ IN _ STOP Ġ= Ġ 0 . 5 // åģľæŃ¢ ĊĠĠĠ Ġconst Ġval ĠCOL LECT _ FIL P _ PL ATE _ ON Ġ= Ġ 0 . 4 0 // æīĵå¼Ģ ĊĠĠĠ Ġconst Ġval ĠCOL LECT _ FIL P _ PL ATE _ OFF Ġ= Ġ 0 . 0 0 // åħ³éĹŃ ĊĊĠĠĠ Ġ// éĻ Ģ èŀ º ä» ª åĪĿå§ĭ æĸ¹åĲĳ ĊĠĠĠ Ġvar Ġinitial Angle Ġ= Ġdouble ArrayOf ( 0 . 0 , Ġ 0 . 0 , Ġ 0 . 0 ) ĊĠĠĠ Ġ// æľºåĻ¨ äºº çĿĢ èĲ½ æĹ¶ æľĢå¤§ è§Ĵ åº¦ ĊĠĠĠ Ġconst Ġval ĠL AND ING _ ANGLE Ġ= Ġ 1 . 5 0 ĊĠĠĠ Ġ// æ Ĥ¬ æĮ Ĥ è§Ĵ åº¦ ĊĠĠĠ Ġconst Ġval ĠS US P ENSION _ ANGLE Ġ= Ġ 3 . 0 ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ å¸¸ éĩı åĮº >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> ĊĠĠĠĠ Ġ*/ ĊĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ æĮī éĶ® åĮº >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġch assis Direction Change (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . left _ stick _ button // åºķ çĽĺ é©¬ è¾¾ åıį åĲĳ ĊĠĠĠ Ġfun Ġlift Up (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . y // åįĩ éĻį åĲĳ ä¸Ĭ ĊĠĠĠ Ġfun Ġlift Down (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . a Ġ&& Ġ! op Mode . gamepad 1 . start // åįĩ éĻį ä¸ĭ éĻį ĊĠĠĠ Ġfun Ġauto To Top (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . dp ad _ up // èĩªåĬ¨ åĪ° é¡¶ ĊĠĠĠ Ġfun Ġauto To Bottom (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . dp ad _ down // èĩªåĬ¨ åĪ° åºķ ĊĠĠĠ Ġfun Ġe ject Debug (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . b // å¼¹ åĩº ä¼ º æľį æŁ¥ éĶĻ ĊĠĠĠ Ġfun Ġset Fil p Plate (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . left _ bump er // æĶ¶éĽĨ ç¿» æĿ¿ ĊĠĠĠ Ġfun Ġcollect Out (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . x // æĶ¶éĽĨ åĲ Ĳ åĩº ĊĠĠĠ Ġfun Ġcollect Into (): ĠBoolean Ġ= Ġop Mode . gamepad 1 . right _ bump er // æĶ¶éĽĨ åĲ ¸ åħ¥ ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ æĮī éĶ® åĮº >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> >>>>>>>> ĊĠĠĠĠ Ġ*/ Ċ Ċ }