[WIP] Cleanup mutex

This should work a lot better for proper threading. Requires the associated fork on artoo-sphero, if you want to test with artoo itself.

Ready for hardware testing.