Some clean up and changed forward and backwards constraints to own function
Some clean up and changed forward and backwards constraints to own function

JNeiger/robocup-software,RoboJackets/robocup-software,RoboJackets/robocup-software,JNeiger/robocup-software,JNeiger/robocup-software,RoboJackets/robocup-software,RoboJackets/robocup-software,JNeiger/robocup-software,JNeiger/robocup-software