FSM and globalization

Globalize more variables and consts
Added the FSM in an h and c file
Created a temp "motors" file to test compiling with the changes