added anti wind up to pid controller

This fixes the overshooting seen on servo motors.
reformating in SimMotor.cpp

This makes the code way more readable.