Fixed busted servocontroller

LER register was not being set correctly with multiple ports enabled
Uncommented code

Yeah