Modifying P and I functions on python to test torque instead of speed.