Added the proportional term to the take back half formula
Merge branch 'develop' of https://github.com/2729StormRobotics/Storm2014 into develop