Library fixes for new motors

fixed Encoder Unit test
Created PID unit test
tuned PID
Created PID Visualiser