Further changes to test_stabilize

Made more changes after doing a dry run of test_stabilize. Made debug output more clear, and changed the order of code in drive.cpp so that the two motors that aren't supposed to move actually don't move.