Wäre es nicht vernünftiger....

erst anfangen ein PID-regler zu bauen das nur ein Motor zu einer bestimmten Drehgeschwindigkeit regelt. Und nicht gleich mit etwas anfangen das unkontrolliert im Gegend oder ein Kopf fliegen kann. Fang erst mal an mit ein 1-Achsen Problem. (Wie den Beispiel Code) Danach 2, .... und NUR DANNACH die weitere Dimensionen.