Implemented a PID controller for the goStraight function.