fix bugs for TurnForAngle and tune constants