Write set speed functionk
Merge branch 'Servos' of github.com:chrisjleaf/mechatronicDesign into Servos