Man könnte den Drehgeschwindigkeits-Algorithmus weiterverwenden und zusätzlich vorgeben:
Drehgeschwindigkeit = Abstand_zum_Sollwinkel * k

Noch was eher theoretisches zum Verständnis:
Wenn du den Drehwinkel regeln willst, ist das, was du da als PI-Regler verwendest, eigentlich ein PD-Regler, weil das Gyro-Signal ja schon die Änderung des Drehwinkels ist. Ich würde da dann (glaube ich) eher einen PID-Regler verwenden.