Balance Bot with Arduino Due

So here is my Balance bot. I've used the Arduino Due, the Motor Shield and the MPU6050.

The desired angle is set to 15 degress (0 degree is aligned with the vertical axis), so its moving.

At the moment I'm trying to fix this shaking around the desired angle.

The main controller is runing at 1kHz, the current controllers are running at 5kHz. These controller frequencies make it that fast.