identified issue with the encode torque calculate, was not accounting for the fact that the values were based on the mathematical model and not the motor values
added coordinate transformation angles