added invariant to motor code

Details on lines 153 to 167