Do some fixing and make the robot work

This is currently the only branch that actually works. After successfully merging into master, this API will be considered 'stable' Any new changes will not be foolishly done in master directly. This is an attempt to fix my stupid mistakes.