Current and voltage collecting functions added and enabled. Test pins for debugging added. For speeds below 100Hz the voltage measure is not what it was expected yet.