Fixed bugs with arm not moving unless an interrupt is triggered, z-axis motor control now supported