Implemented turnAngle() method using P and D terms