Sub: use cork() when writing motors, move servo updates to motors_output()

Sub: use cork() when writing motors, move servo updates to motors_output() #31055