Changed PID Calculation again - Still type C. Works a lot better now.