Patch 1

Basically, I tried to improve the response of the PID, to make it more soft and reduce the stress of the vessel.

Now the constants "Kp", "Ki" and "Kd" are calculated from one another "Tf", with which you can change the speed of response. I also had to make changes so that the drive action is limited directly by the PID. (between +1 and -1), this prevents if it is saturated that the integrator keep adding (windup effects).