package Ġorg . first insp ires . ft c . team code . op modes . Auto Ċ Ċ import Ġcom . qual comm . robot core . event loop . op mode . Aut onomous Ċ import Ġorg . first insp ires . ft c . team code . hardware . sub systems . Int ake Ċ import Ġorg . first insp ires . ft c . team code . hardware . sub systems . Transfer Ċ import Ġorg . first insp ires . ft c . team code . motion . Path Ċ import Ġorg . first insp ires . ft c . team code . motion . Waypoint Ċ import Ġorg . first insp ires . ft c . team code . motion . to Radians Ċ import Ġorg . first insp ires . ft c . team code . vision . Sk yst one Reader Ċ Ċ @ Aut onomous ( name Ġ= Ġ" Blue ĠAuto ĠNo ĠFoundation ") Ċ class ĠBlue Auto No Foundation : ĠAuto () Ġ{ ĊĠĠĠ Ġlateinit Ġvar Ġstone Position : ĠSk yst one Reader . Stone Position ĊĠĠĠ Ġoverride Ġfun Ġinit () Ġ{ ĊĠĠĠĠĠĠĠ Ġsuper . init () ĊĠĠĠĠĠĠĠ Ġdrive train . od ometer . reset Position ( 1 7 . 7 5 Ġ/ Ġ 2 Ġ* Ġ 2 . 5 4 , Ġ( 1 7 Ġ/ Ġ 2 Ġ+ Ġ 2 4 Ġ+ Ġ( 2 4 - 1 7 )) Ġ* Ġ 2 . 5 4 , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġvision . enable () ĊĠĠĠĠĠĠĠ Ġdrive train . foundation Down Ġ= Ġfalse ĊĠĠĠĠĠĠĠ Ġtransfer . grab ber State Ġ= ĠTransfer . Grab ber State . GR ABB ED ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġinit _ loop () Ġ{ ĊĠĠĠĠĠĠĠ Ġsuper . init _ loop () ĊĠĠĠĠĠĠĠ Ġtelemetry . addData (" POSITION ", Ġvision . sk yst one Reader . stone Position . toString ()) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġstart () Ġ{ ĊĠĠĠĠĠĠĠ Ġsuper . start () ĊĠĠĠĠĠĠĠ Ġdrive train . od ometer . reset Position ( 1 7 . 7 5 Ġ/ Ġ 2 Ġ* Ġ 2 . 5 4 , Ġ( 1 7 Ġ/ Ġ 2 Ġ+ Ġ 2 4 Ġ+ Ġ( 2 4 - 1 7 )) Ġ* Ġ 2 . 5 4 , Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġstone Position Ġ= Ġvision . sk yst one Reader . stone Position ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġloop () Ġ{ ĊĠĠĠĠĠĠĠ Ġsuper . loop () ĊĠĠĠĠĠĠĠ Ġwhen Ġ( state ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġrelease Ġint ake ĊĠĠĠĠĠĠĠĠĠĠĠĠ 0 Ġ-> Ġ{ Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . four Bar Position Ġ= ĠTransfer . Four Bar Position . READY ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake . state Ġ= ĠInt ake . State . OUT ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( state Time Elapsed Ġ>= Ġ 5 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġwall Ġto Ġpointed Ġat Ġleft Ġstone ĊĠĠĠĠĠĠĠĠĠĠĠĠ 1 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake . state Ġ= ĠInt ake . State . OFF ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . grab ber State Ġ= ĠTransfer . Grab ber State . READY _ FOR _ ST ONE Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . four Bar Position Ġ= ĠTransfer . Four Bar Position . GR AB ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpath Ġ= ĠPath () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 1 7 . 7 5 Ġ/ Ġ 2 Ġ* Ġ 2 . 5 4 , Ġ( 1 7 Ġ/ Ġ 2 Ġ+ Ġ 2 4 Ġ+ Ġ( 2 4 - 1 7 )) Ġ* Ġ 2 . 5 4 , Ġ 0 . 0 , Ġ 0 . 0 , Ġ 0 . 0 , Ġ 0 . 0 , Ġ 0 . 0 , Ġtrue )) Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 9 0 . 0 , Ġ 1 6 0 . 0 , Ġ 0 . 7 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġwhen Ġ( stone Position ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . LEFT Ġ-> Ġpath . add Waypoint ( Waypoint ( 6 5 . 0 , Ġ 1 2 1 . 5 , Ġ 0 . 7 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . CENTER Ġ-> Ġpath . add Waypoint ( Waypoint ( 6 5 . 0 , Ġ 1 0 2 . 0 , Ġ 0 . 7 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . RIGHT Ġ-> Ġpath . add Waypoint ( Waypoint ( 6 5 . 0 , Ġ 7 5 . 0 , Ġ 0 . 7 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . follow Path ( path , Ġto Radians ( 9 0 . 0 ))) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġturn Ġto Ġst ones ĊĠĠĠĠĠĠĠĠĠĠĠĠ 2 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġ- 4 2 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . point To Angle ( to Radians ( 0 . 0 ), Ġ 0 . 6 , Ġto Radians ( 1 0 . 0 ))) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġint ake Ġstone Ġ( drive Ġforward ) ĊĠĠĠĠĠĠĠĠĠĠĠĠ 3 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake . state Ġ= ĠInt ake . State . IN ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpath Ġ= ĠPath () Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 9 0 . 0 , Ġ 1 6 0 . 0 , Ġ 0 . 7 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 6 5 . 0 , Ġ 1 2 0 . 0 , Ġ 0 . 0 , Ġ 0 . 0 )) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġwhen Ġ( stone Position ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . LEFT Ġ-> Ġpath . add Waypoint ( Waypoint ( 1 0 7 . 5 , Ġ 1 2 1 . 5 , Ġ 0 . 4 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . CENTER Ġ-> Ġpath . add Waypoint ( Waypoint ( 1 0 7 . 5 , Ġ 1 0 2 . 0 , Ġ 0 . 2 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . RIGHT Ġ-> Ġpath . add Waypoint ( Waypoint ( 1 0 7 . 5 , Ġ 7 5 . 0 , Ġ 0 . 2 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . follow Path ( path , Ġto Radians ( 9 0 . 0 ))) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġwait Ġfor Ġint ake ĊĠĠĠĠĠĠĠĠĠĠĠĠ 4 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( state Time Elapsed Ġ> Ġ 5 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġback Ġup ĊĠĠĠĠĠĠĠĠĠĠĠĠ 5 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive train . y Power Ġ= Ġ- 0 . 5 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . od ometer . x Position Ġ< Ġ 6 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġgo Ġto Ġfoundation Ġand Ġlower Ġbar ĊĠĠĠĠĠĠĠĠĠĠĠĠ 6 Ġ-> Ġ{ Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . four Bar Position Ġ= ĠTransfer . Four Bar Position . READY ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpath Ġ= ĠPath () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( state Start Position . x , Ġstate Start Position . y , Ġ 0 . 0 , Ġ 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 8 0 . 0 , Ġ 1 0 0 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 8 0 . 0 , Ġ 1 8 0 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 8 0 . 0 , Ġ 2 8 5 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . follow Path ( path , Ġto Radians ( 2 7 0 . 0 ))) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠ 7 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . grab ber State Ġ= ĠTransfer . Grab ber State . GR ABB ED ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( state Time Elapsed Ġ>= Ġ 5 0 0 ) Ġ{ Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . four Bar Position Ġ= ĠTransfer . Four Bar Position . OUT ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠ 8 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( state Time Elapsed Ġ> Ġ 8 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . grab ber State Ġ= ĠTransfer . Grab ber State . RELEASE D ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġgo Ġto Ġsecond Ġsk yst one ĊĠĠĠĠĠĠĠĠĠĠĠĠ 9 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . grab ber State Ġ= ĠTransfer . Grab ber State . READY _ FOR _ ST ONE ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpath Ġ= ĠPath () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( state Start Position . x , Ġstate Start Position . y , Ġ 0 . 0 , Ġ 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 8 9 . 0 , Ġ 1 8 0 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġwhen Ġ( stone Position ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . LEFT Ġ-> Ġpath . add Waypoint ( Waypoint ( 6 5 . 0 , Ġ 5 8 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . CENTER Ġ-> Ġpath . add Waypoint ( Waypoint ( 6 5 . 0 , Ġ 4 1 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . RIGHT Ġ-> Ġpath . add Waypoint ( Waypoint ( 6 5 . 0 , Ġ 3 7 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . follow Path ( path , Ġto Radians ( 9 0 . 0 ))) Ġ{ Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . four Bar Position Ġ= ĠTransfer . Four Bar Position . GR AB ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġturn Ġto Ġqu arry ĊĠĠĠĠĠĠĠĠĠĠĠĠ 1 0 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( stone Position Ġ== ĠSk yst one Reader . Stone Position . RIGHT ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . point To Angle ( to Radians ( 0 . 0 ), Ġ 0 . 6 , Ġto Radians ( 1 0 . 0 ))) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġint ake Ġstone ĊĠĠĠĠĠĠĠĠĠĠĠĠ 1 1 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake . state Ġ= ĠInt ake . State . IN ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpath Ġ= ĠPath () Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 9 0 . 0 , Ġ 1 6 0 . 0 , Ġ 0 . 7 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( state Start Position . x , Ġstate Start Position . y , Ġ 0 . 0 , Ġ 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġwhen Ġ( stone Position ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . LEFT Ġ-> Ġpath . add Waypoint ( Waypoint ( 1 1 0 . 0 , Ġ 5 8 . 0 , Ġ 0 . 2 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . CENTER Ġ-> Ġpath . add Waypoint ( Waypoint ( 1 1 0 . 0 , Ġ 4 1 . 0 , Ġ 0 . 2 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠSk yst one Reader . Stone Position . RIGHT Ġ-> Ġpath . add Waypoint ( Waypoint ( 1 0 7 . 5 , Ġ 2 7 . 0 , Ġ 0 . 2 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . follow Path ( path , Ġto Radians ( 9 0 . 0 ))) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠ 1 2 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdrive train . y Power Ġ= Ġ- 0 . 5 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . od ometer . x Position Ġ< Ġ 7 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠ 1 3 Ġ-> Ġ{ Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . four Bar Position Ġ= ĠTransfer . Four Bar Position . READY ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġpath Ġ= ĠPath () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( state Start Position . x , Ġstate Start Position . y , Ġ 0 . 0 , Ġ 0 . 0 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 8 0 . 0 , Ġ 1 0 0 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 8 0 . 0 , Ġ 1 8 0 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġpath . add Waypoint ( Waypoint ( 8 0 . 0 , Ġ 2 8 5 . 0 , Ġ 1 . 0 , Ġ 0 . 7 )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( drive train . follow Path ( path , Ġto Radians ( 2 7 0 . 0 ))) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠ 1 4 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . grab ber State Ġ= ĠTransfer . Grab ber State . GR ABB ED ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( state Time Elapsed Ġ>= Ġ 5 0 0 ) Ġ{ Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . four Bar Position Ġ= ĠTransfer . Four Bar Position . OUT ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġincrement State () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠ 1 5 Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( state Time Elapsed Ġ> Ġ 8 0 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtransfer . grab ber State Ġ= ĠTransfer . Grab ber State . RELEASE D ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ Ċ // ĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġint ake . state Ġ= ĠInt ake . State . OFF ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġstop Movement () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} Ċ }