Add motor current control with M907