Updates

Mostly working code from after the gps stopped working and I fixed it.