Removed a test command not needed any longer. Also changed the algorithm to work directly on the 16 bit torque value instead of byte by byte. This simplifies the algorithm further.