Refactored parameters; set control rate to 40Hz