I'll bet you thought this was a code-for-demand shop. It was nice of you to ask nicely, but you are the one with the responsibility for the project. The subject is advanced enough that you should have learned enough to make a start.
Come back when you have some results and need help with actual code (don't forget the comments).
By the way, have you arrived at a hardware solution for measuring the torque?