Motor+Inkrementalgeber Spielereien

Ich habe mir letztens ein Getriebemotor mit Encoder gekauft und wollte sehen was ich damit anfangen kann.
Motor: https://www.pololu.com/product/2286

https://www.dropbox.com/s/8vc9us0rw9...74643.jpg?dl=0
https://youtu.be/GHN-6M3JO-g

So weit habe ich ein grey-box Modell identifiziert (Zustandsraum) und damit einen State-Space Controller mit Integration des Regelungsfehlers berechnet.
(Keine Sorge das klingt alles viel Komplexer als es wirklich ist.)

Background-Wissen:
Bei der Identifikation ging es vor allem dabei die Konstanten des Modells (Widerstand, Trägheitsmoment, Verstärkung,... ) mit Hilfe einiger Experimente zu ermitteln.
Ein State-Space Controller (Zustandsraumregler) hat den Vorteil, dass die Berechnung der Steuervariable (Spannung in den Fall hier) mittels einer Multiplikation der "States" (ein State kann eine physikalische Größe sein wie Positon, Geschwindigkeit,... - Muss es aber nicht) passiert.
Die zusätzliche Integration des Regelungsfehlers garantiert uns, dass der Motor seine Soll-Position am Ende genau trifft.
Als zusätzlicher Bonus wurde eine Anti-Wind-up Sicherung eingebaut um das zu schnelle Ansteigen des Integrators zu verhindern.

Hier die Main-Loop des Codes: (Arduino Uno, mit einen Adafruit Motor Shield v2)

Code:

---

void loop() {
    static long errorInt = 0;
    static long encoderPosOld = 0;


    while(Serial.available()){
        processInput();    // Reads serial data without blocking
    }   
   
    unsigned long currentMillis = millis();
    // target = 20Hz Loop
    if(currentMillis - previousMillis >= 50){
        long y_soll = readNumber;
        long error = y_soll - encoderPos;
       
        // Speed of Rotation in encoder-ticks per second (first order Aproximation)
        double d_encoderPos = (double)(encoderPos-encoderPosOld)/(currentMillis - previousMillis)*1000;
       
        // State Space Controller with Integration of the control-error
        // des_poles_aug=exp([-5+5i,-5-5i,-2.6]*0.05);
        errorInt = errorInt + constrain(error,-2450,2450); // Error Integration with anti-wind up
        int motorInput = 0.02622044*(double)errorInt-(0.30625320*(double)encoderPos-0.00190821*d_encoderPos);
       
        driveMotor(motorInput);
       


        encoderPosOld = encoderPos;


        previousMillis = currentMillis;
    }


}

---

Die Zeile

Code:

---

int motorInput = 0.02622044*(double)errorInt-(0.30625320*(double)encoderPos-0.00190821*d_encoderPos);

---

ist der gesamte Regler. Die Werte wurden so gewählt, dass es am Ende zu keinen Überschwingen des Zielwertes kommt, jedoch der Regler möglichst schnell ist.
Die beiden States dieses Reglers sind in diesen Fall die Position und die Rotationsgeschwindigkeit (d_encoder = d/dt encoder).

Bei Wünchen, Fragen und Anregungen bin ganz offen. Habt ihr vielleicht sogar eine bessere Idee wie man den Motor ansteuern sollte? Wenn etwas unklar ist kann ich versuchen es einfacher zu erklären.

PS: Ich hoffe das ist das richtige Sub-Forum für das hier, da der Algorithmus im Vordergrund steht.