package Ġorg . first insp ires . ft c . team code . auto Ċ Ċ import Ġcom . ac mer obot ics . road runner . geometry . Pose 2 d Ċ import Ġcom . ac mer obot ics . road runner . geometry . Vector 2 d Ċ import Ġcom . ac mer obot ics . road runner . trajectory . Trajectory Ċ import Ġcom . qual comm . robot core . event loop . op mode . Aut onomous Ċ import Ġcom . qual comm . robot core . event loop . op mode . Linear Op Mode Ċ import Ġcom . qual comm . robot core . hardware . Dc Motor Ċ import Ġorg . first insp ires . ft c . team code . drive . Sample M ec an um Drive Ċ import Ġorg . first insp ires . ft c . team code . util . Int ake Ċ import Ġorg . first insp ires . ft c . team code . util . W ob ble Goal Ċ import Ġjava . lang . Math . to Radians Ċ Ċ @ Aut onomous Ċ class ĠBlue Side Auto : ĠLinear Op Mode () Ġ{ ĊĠĠĠ Ġoverride Ġfun Ġrun Op Mode () Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġdrive Ġ= ĠSample M ec an um Drive ( hardware Map ) ĊĠĠĠĠĠĠĠ Ġval Ġw ob ble Goal Ġ= ĠW ob ble Goal . standard ( hardware Map ) ĊĊĠĠĠĠĠĠĠ Ġval Ġint ake Ġ= ĠInt ake . standard ( hardware Map ) ĊĠĠĠĠĠĠĠ Ġval Ġsh ooter Motor Ġ= Ġhardware Map . dc Motor [" sh ooter 1 "] ĊĠĠĠĠĠĠĠ Ġsh ooter Motor . zero Power Behavior Ġ= ĠD c Motor . Zero Power Behavior . BR A KE ĊĠĠĠĠĠĠĠ Ġval Ġsh ooter Motor 2 Ġ= Ġhardware Map . dc Motor [" sh ooter 2 "] ĊĠĠĠĠĠĠĠ Ġsh ooter Motor 2 . zero Power Behavior Ġ= ĠD c Motor . Zero Power Behavior . BR A KE ĊĠĠĠĠĠĠĠ Ġval Ġsh ooter Ser vo Ġ= Ġhardware Map . servo [" pin ball "] ĊĠĠĠĠĠĠĠ Ġval Ġfl ap Ġ= Ġhardware Map . servo [" Fl ap "] ĊĊĠĠĠĠĠĠĠ Ġ// Ġinit Ġstuff ĊĠĠĠĠĠĠĠ Ġw ob ble Goal . lift Arm () ĊĠĠĠĠĠĠĠ Ġw ob ble Goal . grab () ĊĠĠĠĠĠĠĠ Ġfl ap . position Ġ= Ġ 0 . 0 0 Ġ// High Ġgoal : Ġ 0 . 0 0 ĠĠĠĠĠ ĠPS : Ġ 0 . 0 5 ĊĠĠĠĠĠĠĠ Ġsh ooter Ser vo . position Ġ= Ġ 0 . 0 0 ĊĊĠĠĠĠĠĠĠ ĠwaitFor Start () ĊĊĠĠĠĠĠĠĠ Ġval Ġbox Ġ= Ġ 0 Ġ// ĠTODO : ĠVision Ġstuff ĊĊĠĠĠĠĠĠĠ Ġval Ġstart Pose Ġ= ĠP ose 2 d (- 6 4 . 0 , Ġ 5 0 . 0 , Ġto Radians ( 1 8 0 . 0 )) ĊĠĠĠĠĠĠĠ Ġdrive . pose Estimate Ġ= Ġstart Pose ĊĊĠĠĠĠĠĠĠ Ġval Ġshoot Trajectory Ġ= Ġdrive . trajectory Builder ( start Pose , Ġto Radians (- 9 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. sp lineTo Constant Heading ( Vector 2 d (- 5 . 0 , Ġ 5 7 . 0 ), Ġto Radians ( 2 0 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠ Ġdrive . follow Trajectory ( shoot Trajectory ) ĊĊĠĠĠĠĠĠĠ Ġlateinit Ġvar Ġtrajectory : ĠTra jectory ĊĊĠĠĠĠĠĠĠ Ġwhen Ġ( box ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠ 0 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtrajectory Ġ= Ġdrive . trajectory Builder ( shoot Trajectory . end (), Ġto Radians (- 9 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. sp lineTo Constant Heading ( Vector 2 d (- 4 0 . 0 , Ġ 5 7 . 0 ), Ġto Radians ( 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. sp lineTo Constant Heading ( Vector 2 d ( 3 8 . 0 , Ġ 5 7 . 0 ), Ġto Radians ( 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . follow Trajectory ( trajectory ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠ 1 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtrajectory Ġ= Ġdrive . trajectory Builder ( shoot Trajectory . end (), Ġto Radians (- 9 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. sp lineTo Constant Heading ( Vector 2 d ( 1 3 . 0 , Ġ 5 7 . 0 ), Ġto Radians ( 1 8 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. sp lineTo Constant Heading ( Vector 2 d ( 1 3 . 0 , Ġ 3 5 . 0 ), Ġto Radians ( 9 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . follow Trajectory ( trajectory ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠ 2 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtrajectory Ġ= Ġdrive . trajectory Builder ( shoot Trajectory . end (), Ġto Radians (- 9 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. sp lineTo Constant Heading ( Vector 2 d (- 4 8 . 0 , Ġ 5 0 . 0 ), Ġto Radians ( 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . follow Trajectory ( trajectory ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġval Ġpar k Trajectory Ġ= Ġdrive . trajectory Builder ( trajectory . end (), Ġto Radians (- 9 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. sp lineTo Constant Heading ( Vector 2 d ( 3 . 0 , Ġ 6 0 . 0 ), Ġto Radians ( 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠ Ġdrive . follow Trajectory ( park Trajectory ) ĊĠĠĠ Ġ} Ċ }