Bug Fixes

Added some fixes, ready to test on the pi.