Possibly fixed goto() by changing safety margin

TODO: More testing

Closes #326