V4.3 Bug Fix for test mode

-Set p2sel to make motors stay off during test mode
-moved initialize bits to start to try and solve jerking
-Buildable, untested