Fixed Axis, also improved arm algorithm
Improved and Code Cleaning
Minor Changes