Added balancing and joystick control

Still needs a bit of work and some comments but this should be a start