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