Fixed initSensor() function for SMUX usage

SMUX sensor port was badly initialised, causing interference if it was
not on port 1.  This should fix the problems where the SMUX was messing
with the motor controllers.

Thanks to Steven Malson, Jarrod Sinclair and Connor Sinclair for
reporting and testing the fix!