Working version
Merge branch 'Servo-fix' into Leah
Fixed pause time.