Torque calculation added to leg readout

Dependencies:   AS5048 LCM101 MODSERIAL PinDetect SDFileSystem mbed

Fork of heros_leg_readout by Martijn Grootens