Route Completion

- All routes are now added
- Threshold added for reaching location
- 2nd test train added