Motorenreglung: was mache ich denn falsch?

[Luca B. (lucabert)]

Hallo, Leute!

Endlich habe ich wieder Zeit für meinen Roboter...
Nun, auf dem Tisch habe ich das Problem mit der Reglung der 
Rädermotoren.
Ich habe eine Schaltung gemacht, die mit PWM die Motoren steuert, und 
mit einer optischen Encoder pro Motor eine Rückmeldung kriegt.
Nun soll das Programm geschrieben werden.

Schon vor X Monaten hatte ich das Programm für den "P" (Proportional) 
geschrieben, ich wollte jetzt erweitern mit dem "I" (Integral).

Das ist mein Code:

  uint16_t pwm, nSpeed, err, p, l, r;

  l = getLeftPWM();
  r = getRightPWM();
  if(getLeftInterval() != 0 && getDirectionLeft() != DIR_STOP)
  {
    nSpeed = (1000 / getLeftInterval());
    if(nSpeed != getSpeed())
    {
      p = getLeftPWM() * getSpeed();
      pwm = p / nSpeed;
      err = p % nSpeed;
      if(err <= (nSpeed / 2))
        errL += err;
      else
      {
        pwm++;
        errL -= err;
      }
      if(errL < 0)
      {
        while(abs(errL) >= nSpeed)
        {
          pwm--;
          errL += nSpeed;
        }
      }
      else
      {
        while(errL >= nSpeed)
        {
          pwm++;
          errL -= nSpeed;
        }
      }
      if((pwm >= MINPWM) && (pwm <= MAXPWM))
        l = pwm;
      else if(pwm < MINPWM)
        l = MINPWM;
      else
        l = MAXPWM;
      setLeftCorrectionDone();
      turnOnLED(LED2);
    }
    else
    {
      turnOffLED(LED2);
    }
  }
// das gleiche für den linken Motor...
// dann:
  setLeftPWM(l);
  setRightPWM(r);
  TCCR1A |= _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);
  TCCR1B |= _BV(WGM13) | _BV(CS12) | _BV(CS10);
  ICR1 = ICR_LIMIT;

Die Funktionen getLeftPWM() und setLeftPWM() lesen, bzw. setzen den 
neuen PWM-Wert. getLeftInterval() gibt zurück die Zeit, in 
Millisekunden, zwischen zwei Pegel des Encoders. getSpeed() gibt zurück 
den theoretische Geschwindigkeit, die ich erreichen will.

Also, das Programm funktioniert auch, aber ich habe immer eine 
Abweichung zwischen den Motoren...
Manchmal ist es klein, manchmal größer.
Um eine Großordnung zu geben, wenn ich die Motoren 10 Mal drehen lassen, 
habe ich eine Abweichung von 3-5 Schritte. Eine komplette Umdrehung des 
Motors sind 100 Schritte, also ist eine Abweichung von 3-5°...
Es mag sein, daß es klein aussieht, aber ich möchte das beste kriegen, 
was ich kriegen kann...

Eine komische Sache noch: auch wenn ich die Motoren länger drehen lasse, 
ist die Abweichung nicht proportionell größer. Also, nach 30 
Umdrehungen ist es nicht 9-15 Schritte, sondern vielleicht 4-7 
Schritte...

Ideen?

Danke
Luca Bertoncello

[Patrick B. (p51d)]

Bei deinem Code werde ich nicht ganz schlau: Wie errechnest du den 
PWM-Wert? (hier in der Codesammlung ist sonst ein PID-Beispiel abgelegt, 
welches auch schon sehr gut ist, ansonsten OpenServo, da kannst du dir 
den Code auch ansehen)
OK, Motorregelung ist nicht ganz einfach. Da habe ich mir auch schon die 
Zähne ausgebissen.
Lösungsansatz ist eine Kaskadenregelung. Soll heissen, du hast einen 
Stromregler und einen Geschwindigkeitsregler. Dabei muss der innerste 
Regler immer schneller (z.B alle 1ms) sein als die äusseren (hier z.B > 
2ms, besser 4ms).

Das Problem klingt ganz nach Schrittverlusten..., da du aber anscheinen 
nur die Geschwindigkeit und nicht noch die Position regeln willst, 
sollte das nicht zu einem grossen Problem werden.
Trajektorie ist hier auch hilfreich: Du hast die Anfangsgeschwindigkeit 
und die Endgeschwindigkeit. Hier solltest du mit Zwischenschritten 
arbeiten, um ein überschiessen zu vermeiden.
Wie liest du denn die Encoder ein? Über Interrupts?

Viel Erfolg
Patrick

[Luca B. (lucabert)]

Patrick B. schrieb:
> Bei deinem Code werde ich nicht ganz schlau: Wie errechnest du den
> PWM-Wert? (hier in der Codesammlung ist sonst ein PID-Beispiel abgelegt,
> welches auch schon sehr gut ist, ansonsten OpenServo, da kannst du dir
> den Code auch ansehen)

Naja, der Code habe ich gepostet... :)
Hier rechne ich genau das PWM-Wert...

> OK, Motorregelung ist nicht ganz einfach. Da habe ich mir auch schon die
> Zähne ausgebissen.
> Lösungsansatz ist eine Kaskadenregelung. Soll heissen, du hast einen
> Stromregler und einen Geschwindigkeitsregler. Dabei muss der innerste
> Regler immer schneller (z.B alle 1ms) sein als die äusseren (hier z.B >
> 2ms, besser 4ms).

Mmmm... Wie kann ich den Strom regeln?
Ich befürchte, daß ich nicht kann... Als Anhang den Schaltplan...

> Das Problem klingt ganz nach Schrittverlusten..., da du aber anscheinen
> nur die Geschwindigkeit und nicht noch die Position regeln willst,
> sollte das nicht zu einem grossen Problem werden.
> Trajektorie ist hier auch hilfreich: Du hast die Anfangsgeschwindigkeit
> und die Endgeschwindigkeit. Hier solltest du mit Zwischenschritten
> arbeiten, um ein überschiessen zu vermeiden.

Kannst du mir erklären, wie ich das machen kann?

> Wie liest du denn die Encoder ein? Über Interrupts?

Alle Millisekunden rufe ich folgende Routine auf (eine für jeden Motor):

  if((lastLeftStatus == HIGH) && bit_is_clear(ENCODER_PIN, ENCODERL_PIN))
  {
    lastLeftStatus = LOW;
  }
  if((lastLeftStatus == LOW) && bit_is_set(ENCODER_PIN, ENCODERL_PIN))
  {
    nStepLeft++;
    lastLeftStatus = HIGH;
    intervalL = getUptime() - lastL;
    lastL = getUptime();
  }

Grüße
Luca Bertoncello