Merge Elevator Code into Main

[hdsbbrooks]

Code should be tested on robot before requesting a merge into main. Please test code and then re-open the commit for merging.