Added balance method which includes all stabilization code.
Fixed and enabled arms.