Set up basic movement tests in navigation.c. Will test them tonight.
Got the tests working. A few more changes, and this part of MS 4 will be finished.
fixed some minor logic. just need to show integrative tests work. TODO fix uart on pics