Add "sort" Command

Can now use sort command to send trains into a certain order.
Works for 2 trains so far.
Decrease Reservation Buffer

We only reserve 5cm past the back of the train now. This is so
that we can line up 3 trains sensor to sensor when doing our
sorting stuff.

Also fixed a bug with the deadlock check that restarted trains
without a route.