Add sign to current depending on direction
Add speed control to into interrupt