Thanks.
The more I think about it, I'm back to tuning it manually. My problem is, I really have no idea in which range I have to be tuning the values. But I guess I will play with it more. Unfortunately I don't have any potentiometers here so I have to change the code every time which makes it very complicated.
Probably you experienced guys can me help something: If I tune the parameters, what exactly does change? All the values to 0, raising KP: The motos turn slowly, steady as I raise the value, until I reach something over 1 and it begin oscillating like hell. What are the values really changing? I found some explanations but I can't mentally take those over to my project.