Implemented basic motor PWM, encoder readings