Update to robot 5 to have long drives (left of keyboard) and short drives (right of keyboard)

Add a servo_test in order to drive testing of location of servos as a universal function for teams