Added rough torque-based PID control, needs testing
Added better enumeration of modes and partial implementation
Added rest of control mode setting functions