Refactored Proportional and Derivative control and started adding them to the main code