Fix wrong clamping
Use TowerPro MG995
Fix wrong command cases