package Ġorg . first insp ires . ft c . team code Ċ Ċ import Ġcom . ac mer obot ics . dashboard . F tc Dashboard Ċ 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 . util . Angle Ċ import Ġcom . arc robot ics . ft c lib . hardware . Simple Ser vo Ċ import Ġcom . arc robot ics . ft c lib . hardware . mot ors . Motor Ċ import Ġcom . qual comm . robot core . event loop . op mode . Linear Op Mode Ċ import Ġcom . qual comm . robot core . event loop . op mode . Tele Op Ċ import Ġcom . qual comm . robot core . hardware . Dc Motor Ċ import Ġcom . qual comm . robot core . hardware . Digital Channel Ċ import Ġorg . first insp ires . ft c . team code . drive . advanced . Sample M ec an um Drive Cancel able Ċ import Ġorg . first insp ires . ft c . team code . util . storage . Pose Storage Ċ import Ġorg . first insp ires . ft c . team code . util . storage . Tr aj Storage Ċ import Ġorg . first insp ires . ft c . team code . util . storage . sh ooter Mode Ċ Ċ // declare Ġstart Ġof Ġtele OP Ċ @ Tele Op ( name Ġ= Ġ" sha ury asing ht ele op 2 ") Ċ class Ġnew Tele Op Ġ: ĠLinear Op Mode () Ġ{ ĊĊĠĠĠ Ġ// import Ġsh ooter Mode Ġand ĠTra ject ories ĊĠĠĠ Ġvar Ġsh ooter Mode Ġ= Ġsh ooter Mode () ĊĠĠĠ Ġprivate Ġvar Ġtraj Storage Ġ= ĠTr aj Storage () ĊĊĠĠĠ Ġ// initialize Ġf tc Ġdashboard Ġ( online Ġdriver Ġstation ) ĊĠĠĠ Ġvar Ġdashboard : ĠF tc Dashboard Ġ= ĠF tc Dashboard . getInstance () ĊĠĠĠ Ġ// var Ġpacket Ġ= ĠTe lemetry Packet () ĊĊĠĠĠ Ġ// 1 Ġsh ooter , Ġ 2 Ġint ake , Ġ 1 Ġlinear Ġslide ĊĠĠĠ Ġprivate Ġvar Ġsh ooter Motor Ġ= ĠMotor ( hardware Map , Ġ" motor 1 ", ĠMotor . Go B IL DA . B ARE ) ĊĠĠĠ Ġprivate Ġvar Ġint ake Motor 1 Ġ= ĠMotor ( hardware Map , Ġ" motor 2 ", ĠMotor . Go B IL DA . RP M _ 3 1 2 ) ĊĠĠĠ Ġprivate Ġvar Ġint ake Motor 2 Ġ= ĠMotor ( hardware Map , Ġ" motor 3 ", ĠMotor . Go B IL DA . RP M _ 3 1 2 ) ĊĠĠĠ Ġprivate Ġvar Ġlinear Slide Ġ= ĠMotor ( hardware Map , Ġ" motor 4 ", Ġ 2 8 8 . 0 , Ġ 1 2 5 . 0 ) Ġ// rev Ġhex ĊĊĠĠĠ Ġ// four Ġserv os Ġ( f lick er , Ġl if tings , Ġg ripper ) ĊĠĠĠ Ġprivate Ġvar Ġf lick er Ser vo Ġ= ĠSimple Ser vo ( hardware Map , Ġ" servo 1 ") ĊĠĠĠ Ġprivate Ġvar Ġl ifting Ser vo 1 Ġ= ĠSimple Ser vo ( hardware Map , Ġ" servo 2 ") ĊĠĠĠ Ġprivate Ġvar Ġl ifting Ser vo 2 Ġ= ĠSimple Ser vo ( hardware Map , Ġ" servo 3 ") ĊĠĠĠ Ġprivate Ġvar Ġg ripper Ser vo Ġ= ĠSimple Ser vo ( hardware Map , Ġ" servo 4 ") ĊĊĠĠĠ Ġ// touch Ġsensor ĊĠĠĠ Ġprivate Ġlateinit Ġvar Ġdigital Touch : ĠDigital Channel ĊĊĠĠĠ Ġ// target Ġposition Ġ( high Ġgoal ) ĊĠĠĠ Ġprivate Ġval Ġtarget Position Ġ= ĠVector 2 d ( 7 2 . 0 , Ġ 3 6 . 0 ) ĊĠĠĠ Ġprivate Ġlateinit Ġvar Ġdifference : ĠVector 2 d ĊĊĠĠĠ Ġ// name Ġge ar Ġratio Ġwheel Ġradius Ġand Ġticks Ġfor Ġencoder Ġcm Ġconversion ĊĠĠĠ Ġprivate Ġvar ĠG EAR _ RATIO Ġ= Ġ 1 . 0 Ġ// Ġfor Ġsimulator ĊĠĠĠ Ġprivate Ġvar ĠW HE EL _ RADIUS Ġ= Ġ 1 . 0 Ġ// Ġ 5 Ġcm ĊĠĠĠ Ġprivate Ġvar ĠT IC KS _ PER _ ROT ATION Ġ= Ġ 1 0 3 . 6 Ġ// ĠFrom ĠNe ve Rest Ġ( for Ġsimulator ) ĊĊĠĠĠ Ġ// calculate Ġcm ĊĠĠĠ Ġprivate Ġvar ĠCM _ PER _ TICK Ġ= Ġ 2 Ġ* ĠMath . PI Ġ* ĠG EAR _ RATIO Ġ* ĠW HE EL _ RADIUS Ġ/ ĠT IC KS _ PER _ ROT ATION ĊĊĠĠĠ Ġ// calculate Ġticks Ġfrom Ġcm , Ġreturn Ġticks ĊĠĠĠ Ġprivate Ġfun Ġcm To Ticks ( cm : ĠDouble ): ĠInt Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġcm Tick Ġ= Ġcm Ġ/ ĠCM _ PER _ TICK ĊĠĠĠĠĠĠĠ Ġreturn Ġcm Tick . toInt () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// four Ġstates Ġfor Ġf sm Ġ( driver , Ġauto , Ġsh ooter , Ġpowers hot ) ĊĠĠĠ Ġenum Ġclass ĠState Ġ{ ĊĠĠĠĠĠĠĠ ĠDRI VER _ CONTROL , ĊĠĠĠĠĠĠĠ ĠA UT OM AT IC _ CONTROL , ĊĠĠĠĠĠĠĠ ĠSHO OTER _ CONTROL , ĊĠĠĠĠĠĠĠ ĠPO WER SHOT S ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// set Ġcurrent Ġto ĠDriver ĠControl ĊĠĠĠ Ġprivate Ġvar Ġcurrent State Ġ= ĠState . DRIVER _ CONTROL ĊĊĠĠĠ Ġ@ Throws ( InterruptedException :: class ) ĊĠĠĠ Ġoverride Ġfun Ġrun Op Mode () Ġ{ ĊĊĠĠĠĠĠĠĠ Ġ// import Ġhardware Map ĊĠĠĠĠĠĠĠ Ġval Ġdrive Ġ= ĠSample M ec an um Drive Cancel able ( hardware Map ) ĊĊĠĠĠĠĠĠĠ Ġ// set Ġrun modes Ġ( PID Ġfor Ġsh ooter , Ġreg Ġfor Ġint akes ) ĊĠĠĠĠĠĠĠ Ġsh ooter Motor . set Run Mode ( Motor . Run Mode . Velocity Control ) ĊĠĠĠĠĠĠĠ Ġint ake Motor 1 . set Run Mode ( Motor . Run Mode . Raw Power ) ĊĠĠĠĠĠĠĠ Ġint ake Motor 2 . set Run Mode ( Motor . Run Mode . Raw Power ) ĊĠĠĠĠĠĠĠ Ġlinear Slide . set Run Mode ( Motor . Run Mode . Position Control ) ĊĊĠĠĠĠĠĠĠ Ġ// set Ġcoefficients Ġ+ Ġfeed forward Ġ( PID ) ĊĠĠĠĠĠĠĠ Ġsh ooter Motor . set Ve lo Coefficient s ( 0 . 0 5 , Ġ 0 . 0 1 , Ġ 0 . 3 1 ) ĊĠĠĠĠĠĠĠ Ġsh ooter Motor . set Feed forward Coefficient s ( 0 . 9 2 , Ġ 0 . 4 7 ) ĊĠĠĠĠĠĠĠ Ġlinear Slide . position Coefficient Ġ= Ġ 0 . 0 5 ĊĊĠĠĠĠĠĠĠ Ġ// Ġget Ġa Ġreference Ġto Ġour Ġdigital Touch Ġobject . ĊĠĠĠĠĠĠĠ Ġdigital Touch Ġ= Ġhardware Map . get ( Digital Channel :: class . java , Ġ" sensor _ digital ") ĊĠĠĠĠĠĠĠ Ġdigital Touch . mode Ġ= ĠDigital Channel . Mode . INPUT ĊĊĠĠĠĠĠĠĠ Ġ// set Ġto Ġrun Ġwithout Ġencoder Ġ( since Ġits Ġtele Op ), Ġset Ġpose Ġto Ġcurrent Ġpose ĊĠĠĠĠĠĠĠ Ġdrive . set Mode ( Dc Motor . Run Mode . RUN _ WITHOUT _ EN CODER ) ĊĠĠĠĠĠĠĠ Ġdrive . pose Estimate Ġ= ĠP ose Storage . current Pose ĊĊĠĠĠĠĠĠĠ Ġ// wait Ġfor Ġstart ĊĠĠĠĠĠĠĠ ĠwaitFor Start () ĊĊĠĠĠĠĠĠĠ Ġif Ġ( is Stop Requested ) Ġreturn ĊĠĠĠĠĠĠĠĠĊĠĠĠĠĠĠĠ Ġ// set Ġint akes Ġand Ġsh ooter Ġon ĊĠĠĠĠĠĠĠ Ġsh ooter Motor . set ( 1 . 0 ) ĊĠĠĠĠĠĠĠ Ġint ake Motor 1 . set ( 1 . 0 ) ĊĠĠĠĠĠĠĠ Ġint ake Motor 2 . set ( 1 . 0 ) ĊĊĠĠĠĠĠĠĠ Ġ// start Ġloop ĊĠĠĠĠĠĠĠ Ġwhile Ġ( op Mode IsActive () Ġ&& Ġ! is Stop Requested ) Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// update Ġo do , Ġload Ġestimated Ġpose ĊĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . update () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpose Estimate Ġ= Ġdrive . pose Estimate ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠPrint Ġpose Ġto Ġtelemetry ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . addData (" x ", Ġpose Estimate . x ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . addData (" y ", Ġpose Estimate . y ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . addData (" heading ", Ġpose Estimate . heading ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . addData (" mode ", Ġcurrent State ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtelemetry . update () ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// start Ġf sm , Ġdecide Ġwhat Ġto Ġdo Ġbased Ġon Ġstate ĊĠĠĠĠĠĠĠĠĠĠĠ Ġwhen Ġ( currentState ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠState . DRIVER _ CONTROL Ġ-> Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// take Ġcontroller Ġinputs Ġas Ġa Ġvector , Ġrotate Ġby Ġheading Ġfor Ġfield Ġrelative ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġ( x , Ġy ) Ġ= ĠVector 2 d ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ(- gamepad 1 . left _ stick _ y ). toDouble (), ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ(- gamepad 1 . left _ stick _ x ). toDouble () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ). rot ated ( pose Estimate . heading ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// correct Ġusing ĠPID , Ġthen Ġput Ġthe Ġtarget Ġheading Ġinto Ġthe Ġpose ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . set Weighted Drive Power ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠP ose 2 d ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġx , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġy , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ(- gamepad 1 . right _ stick _ x ). toDouble () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// list Ġout Ġall Ġof Ġthe Ġcontroller Ġmappings ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġwhen Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// drive Ġbehind Ġline , Ġturn Ġtowards Ġgoal , Ġset Ġsh ooter Ġangle ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . left _ trigger Ġ> Ġ 0 . 5 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdifference Ġ= Ġtarget Position . minus ( pose Estimate . vec ()) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġtheta : ĠDouble Ġ= Ġdifference . angle () ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġx : ĠDouble Ġ= Ġif Ġ( drive . pose Estimate . x Ġ> Ġ 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . pose Estimate . x Ġ+ Ġ 0 . 0 0 1 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġtraj 1 Ġ= Ġdrive . trajectory Builder ( pose Estimate ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. sp lineTo ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠVector 2 d ( x , Ġdrive . pose Estimate . y ), ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠAngle . norm Delta ( theta Ġ- Ġdrive . pose Estimate . heading ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. add Dis placement Marker Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġsh ooter Mode . servo Set Goal () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . follow Trajectory Async ( traj 1 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . SHO OTER _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// manual Ġshoot ing ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . right _ trigger Ġ> Ġ 0 . 5 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġunder Ġ= Ġ 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġwhile Ġ( under Ġ< Ġ 3 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġf lick er Ser vo . turn To Angle ( 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġf lick er Ser vo . turn To Angle ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġunder ++ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// g ripper Ġrelease ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . a Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġg ripper Ser vo . turn To Angle ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// go Ġto Ġpoint , Ġalign Ġto Ġpowers hot s ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . b Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġtraj 2 Ġ= Ġdrive . trajectory Builder ( pose Estimate ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. lineTo Spline Heading ( Pose 2 d (- 2 3 . 0 , Ġ- 3 6 . 0 , Ġtraj Storage . angle Theta )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. add Dis placement Marker Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġsh ooter Mode . servo Set Goal () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. build () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . follow Trajectory Async ( traj 2 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . POWER SHOT S ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// grab Ġg ripper ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . y Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġg ripper Ser vo . turn To Angle ( 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// set Ġint ake Ġto Ġint ake ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . right _ bump er Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake Motor 1 . set ( 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake Motor 2 . set ( 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// set Ġint ake Ġto Ġex haust ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . left _ bump er Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake Motor 1 . set (- 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake Motor 2 . set (- 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// extend Ġlinear Ġslide ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . dp ad _ up Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// set Ġposition Ġfor Ġ 2 0 cm , Ġ 0 Ġpower , Ġ 1 3 . 6 Ġtolerance ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . setTarget Position ( cm To Ticks ( 2 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . set ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . setPosition Tolerance ( 1 3 . 6 ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// 0 . 7 5 Ġpower Ġuntil Ġat Ġtarget Ġposition ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġwhile Ġ(! linear Slide . at Target Position ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . set ( 0 . 7 5 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// stop Ġwhen Ġdone ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġg ripper Ser vo . turn To Angle ( 0 . 0 ) Ġ// drop Ġit Ġoff ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . stop Motor () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// re tract Ġlinear Ġslide ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . dp ad _ down Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// set Ġposition Ġfor Ġ- 2 0 cm Ġ( relative ), Ġ 0 Ġpower , Ġ 1 3 . 6 Ġt oler ence ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . setTarget Position ( cm To Ticks ( 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . set ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . setPosition Tolerance ( 1 3 . 6 ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// continue Ġuntil Ġlimit Ġswitch Ġstate Ġis Ġtrue Ġ( detect s Ġslide ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġwhile Ġ(! digital Touch . state ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . set ( 0 . 7 5 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// stop Ġmotor ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Slide . stop Motor () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// dp ad Ġleft Ġto Ġturn Ġlef Ġ 9 0 , Ġd pad Ġright Ġto Ġturn Ġright Ġ 9 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . dp ad _ left Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . turn ( drive . pose Estimate . heading Ġ- Ġ 9 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . AUT OM AT IC _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġgame pad 1 . dp ad _ right Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . turn ( drive . pose Estimate . heading Ġ+ Ġ 9 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . AUT OM AT IC _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠState . AUT OM AT IC _ CONTROL Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠIf Ġx Ġis Ġpressed , Ġwe Ġbreak Ġout Ġof Ġthe Ġautomatic Ġfollowing ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( gamepad 1 . x ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . cancel Following () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . DRIVER _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠIf Ġdrive Ġfinishes Ġits Ġtask , Ġput Ġcontrol Ġto Ġthe Ġdriver ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ(! drive . is Busy ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . DRIVER _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbreak ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠState . SHO OTER _ CONTROL Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( gamepad 1 . x ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . cancel Following () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . DRIVER _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠIf Ġdrive Ġfinishes Ġits Ġtask , Ġput Ġcontrol Ġto Ġthe Ġdriver ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ(! drive . is Busy ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// have Ġsh ooter Ġgo Ġdown ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġl ifting Ser vo 1 . turn To Angle ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġl ifting Ser vo 2 . turn To Angle ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . DRIVER _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbreak ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠState . POWER SHOT S Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( gamepad 1 . x ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . cancel Following () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . DRIVER _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠIf Ġdrive Ġfinishes Ġits Ġtask , Ġput Ġcontrol Ġto Ġthe Ġdriver ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ(! drive . is Busy ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// shoot , Ġturn , Ġshoot , Ġturn , Ġshoot ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġf lick er Ser vo . turn To Angle ( 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġf lick er Ser vo . turn To Angle ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . turn ( traj Storage . angle Theta 1 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġf lick er Ser vo . turn To Angle ( 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġf lick er Ser vo . turn To Angle ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive . turn ( traj Storage . angle Theta 2 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġf lick er Ser vo . turn To Angle ( 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġf lick er Ser vo . turn To Angle ( 0 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent State Ġ= ĠState . DRIVER _ CONTROL ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbreak ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} Ċ } Ċ