[MagiChau]

Used the E4P quadrature encoders provided in the KoP. We had the robot driven the correct distance for different modes, and I plugged them in for drive distance. Had some other encoder counts noted to do some calculations without trial and error. Arm height at our starting position angle needed to go through the peg was noted. Program ended up drive robot until encoder count got close to target. When robot stops moving and the code that moves the robot mechanisms to position returns a boolean of complete another stage is triggered that lowers the arm and shoots the tube out with the rollers.

Had to do some trial and error with drive speeds at competition so lift gets out before the robot goes up to the peg.