Write Prep Command

[ACat701]

Very similar to the previous prep command.

1. Add height selector on D-PAD
2. Create constants for elevator heights for each height (3 for cone, 3 for cube)
3. When Prep is hit:
   • check if you are at the desired height
   • If not, go to the desired height (put wrist in, THEN move)
   • Wrist goes out after to line up