Most stuff tested and works

Just got to test with real robot and update pololu library to match!