Lineare Bewegung

[i_make_it]

Sehr oft definiert man die maximal nutzbare Geschwindigkeit bei geregelter Bahnsteuerung als Bruchteil des Produkts der tatsächlich möglichen Geschwindigkeit und der maximalen Beschleunigung.
Damit ist dann sichergestellt, das jede Achse in Extremsituationen (z.B. Drehrichtungsumkehr) die Geschwindigkeit zur Einhaltung der Bahn erreichen kann.

Grade bei Aufgaben wie Lackieren, wo das Einhalten der Geschwindigkeit proportional zur Farbmenge das wichtigste ist, darf es nicht zu Geschwindigkeitsschwankungen kommen. entweder wird die Schichtdicke zu dünn (zu schnell Verfahren)
oder zu Tropfnasen (extrem zu langsam).
Das eine ist aufwendige manuelle Nacharbeit das andere ist Ausschuß oder muß komplett entlackt werden um dann noch mal neu bearbeitet zu werden.
Auch bei der Materialbearbeitung ist so was tödlich fürs Werkstück.
Beim Entgraten einer Kontour mal wegen Schleppfehler einer Achse ins Werkstück reingefahren und man hat Ausschuß, Bis hin zum zerstörten Werkzeug mit entsprechenden Reparaturzeiten.

Der Steuerung übergibst Du nur Deine Bahn und die Parameter für Geschwindigkeit und Beschleunigungsrampen/-Kurven am Anfang und Ende.
den Rest muß die Regelung mit den transformierten Daten rausholen.
Über die vorher definierten Rampen/Kurven ist ja vorab schnell zu berechnen wo auf der Graden welche Geschwindigkeit zu fahren ist.
Das Reale System muß halt diesem virtuellen Modell folgen können damit das Ergebniss stimmt.

Genau das selbe macht auch eine karthesisch aufgebaute Fräsmaschine wenn man der entsprechende im Raum liegende Kurven gibt.
Bei einem 6-Achs Vertikal Knickarm wird das allerdings noch eine Ecke schwieriger.
In dem Link aus meinem letzten Post steht ja auch irgendwo das es keine allgemeingültige Lösung gibt.

###

Bei Beschleunigungsrampen, hast Du in gewisser Weise 2 Zwischenpunkte die bei der Bahnplanung vorab zu berechnen sind. Denn dort ändert sich ja die Beschleunigung.
Am Startpunkt beginnt man, von 0 aus, mit einer monoton steigenden Geschwindigkeit, bis man den Punkt erreicht an dem man die eigentliche Verfahrgeschwindigkeit erreicht hat dann fährt man mit gleichbleibender Geschwindigkeit bis zu dem Punkt ab dem man mit der monoton fallenden Geschwindigkeit beginnen muß um am Endpunkt eine Geschwindigkeit von 0 zu haben.

Erst wenn man mit Beschleunigungskurven arbeitet, also alle Geschwindigkeiten tangential, knickfrei ineinander übergehen sollen, dann kommen mehr Zwichenpunkte ins Spiel die zu berechnen sind. Dazu muß man dann aber auch für jeden Abschnitt der Spline das Polynom ermitteln mit dem dieser Abschnitt beschrieben wird. Denn nur so kann zur Laufzeit jeder Punkt iterativ berechent werden.

Je mehr Du Dich mit dem Thema befasst um so mehr wirst Du feststellen, das der mechanische Aufbau, die Antriebe und die Elektronik das kleinste Problem sind. Die Mathematik und diese dann clever in Software zu packen sind der haarige Teil.

Wenn man schneller zu positiven Ergebnissen kommen will, baut man zum einen erst mal einen Arm den man nur über Achspositionen steuert (der beherscht dann halt keine Graden- und Kreisinterpolation) und zum anderen, parallel einen karthesischen Aufbau (wie eine Fräsmaschine) und fängt dort dann mit Graden- und Kreisinterpolation an. Als Ergebniss kann mam da dann einen 3D-Drucker oder eine kleine Fräse schaffen. Mit den Erkenntnissen aus beiden Projekten kann man dann stückweise das Systemprogramm des Arms erweitern/umbauen.

[raze92]

Bei Beschleunigungsrampen, hast Du in gewisser Weise 2 Zwischenpunkte die bei der Bahnplanung vorab zu berechnen sind. Denn dort ändert sich ja die Beschleunigung.
Am Startpunkt beginnt man, von 0 aus, mit einer monoton steigenden Geschwindigkeit, bis man den Punkt erreicht an dem man die eigentliche Verfahrgeschwindigkeit erreicht hat dann fährt man mit gleichbleibender Geschwindigkeit bis zu dem Punkt ab dem man mit der monoton fallenden Geschwindigkeit beginnen muß um am Endpunkt eine Geschwindigkeit von 0 zu haben.

Das heißt bei der Bahnplanung muss ich schon vorher die 2 Zwischenpunkte der Beschleunigungsphase jedes Gelenks einzeln bestimmen? Dann muss ich schon vorher die Bahn bestimmen um zu wissen wo Richtungsänderungen eines Gelenks auftauchen um dort die Zwischenpunkte der Beschleunigung zu bestimmen?

Ich Danke vielmals für die Hilfe bis jetzt. Das hat mir sehr geholfen.

[oberallgeier]

Hi Micha,

für nen Fünfachser (klick) hatte ich mal ne Bahnplanung mit sehr minimaler Mathematik geschreiben:

......Bild hier  

Wenige Stützpunkte zur Bahndefinition, Verbindung durch gerade Linien, Verschleifung der Punktumgebung und Synchronisation der verschiedenen Gelenke um eine möglichst glatte Bewegung zu gewährleisten (lief etliche Jahre in industrieller Umgebung), Parameteriesierung der Bahnkurve und funktionstechnische Doppeldeutigkeiten zu vermeiden.

.. bei der Bahnplanung muss ich schon vorher die 2 Zwischenpunkte der Beschleunigungsphase jedes Gelenks einzeln bestimmen? Dann muss ich schon vorher die Bahn bestimmen um zu wissen wo Richtungsänderungen eines Gelenks auftauchen um dort die Zwischenpunkte der Beschleunigung zu bestimmen ..

Mal etwas ausführlicher (trotzdem viel zu kurz) zu einem möglichen Prinzip: Auf den TCP bezogen kann ein Ausschnitt der Bewegung etwa so aussehen wie hier. Diese Darstellung kannst Du als (parameterisierte) Raumkurve interpretieren oder als Bewegung in einer (Kooridnaten-) Ebene usf. Diesen Ausschnitt hatte ich als planerischen Horizont benutzt. Sobald der TCP beim Punkt L vorbei war, wurde der Horizont geshiftet - L wurde K, M wurde L usf, bis am Ende des neuen Horizontes ein neuer Zielpunkt aus einer Liste eingefügt wurde. Wird das Bild als Darstellung der Bahn auf einer Koordinatenebene benutzt, so kannst Du Dir vorstellen dass Du nun einzelne Vorbeifahrten an Stützpunkten prüfen kannst auf die Geschwindigkeit in dieser Ebene - und die zu einer wahren Geschwindigkeit (insbes linear) im Raum zusammensetzen und mit diesen Werten die erforderlichen Gelenkgeschwindigkeiten (max. Drehzahl der Motoren!). Bei zu hohen Geschwindigkeit wird dann aus dem zulässigen Maximum zurückgerechnet und die Planung korrigiert - Horizontänderung und neue Planung der Kurve um den (neuen) Punkt L bei Laufbo=1. Dann wird die vorberechnete Bahn in zeitlich! gleich lange Abschnitte unterteilt werden die vom Roboter gefahren werden. OHNE Übergeschwindigkeit in den Gelenken, ohne zu hohe Beschleunigungen etc. Hier etwas mehr Text dazu.

Genaueres - und ausführlich(er) - zur Bahnplanung dieses hier.

Natürlich soll/muss immer überlegt werden, FMEA, ob Fehler, z.B. durch Doppeldeutigkeit, Nulldurchgänge, interne Gelenkanschläge etc möglich sind und wie die verhindert werden können. (Hilft Dir das zur klaren Vorstellung oder ist es schon zu aufwendig-kompliziert-etc ? )

[i_make_it]

Nein.
Die Punkte beziehen sich auf die lineare (virtuelle) Bahn.
Deine Gelenke werden dann rein durch die Sollwerte der Achsregelungen gesteuert, die sich durch die inverse Kinematik aus den X,Y,Z,A,B,C Koordinaten des jeweils zur Laufzeit berechneten nächsten Punktes ergibt.

Du errechnest bei der Bahnplanung ausgehend von Startkoordinate Xstart,Ystart,Zstart,Astart,Bstart,Cstart, der Zielkoordinate Xziel,Yziel,Zziel,Aziel,Bziel,Cziel, der Geschwindigkeit Vsoll, der Startbeschleunigung astart und der Bremsbeschleunigung aziel die koordinaten für die beiden Punkte wo die beschleunigte Bewegung in die gleichförmigen Bewegung übergeht und wo die gleichförmige Bewegung in die verzögerte Bewegung übergeht.
Jetzt hast Du drei Streckenabschnitte. Abschnitt 1 wo ausgehend von der Geschwindigkeit 0 für jedes Delta der Position ein Delta bei der Vergrößerung der Geschwindigkeit gilt. Im Abschnitt 2 gilt für jedes Delta der Position das die Geschwindigkeit gleich bleibt und im Abschnitt 3 gilt entsprechend eine Verkleinerung der Geschwindigkeit.
Macht man das nicht, dann kann es die Getriebe, Motoren oder Regler zerreißen, wenn die Antriebe versuchen mit einer (theoretisch) unendlichen Beschleunigung auf die Sollgeschwindigkeit zu kommen.

Wenn Deine Regleung nun z.B.: mit 1kHz läuft, berechnest Du alle 1ms die nächste X,Y,Z,A,B,C Position und per inverser Kinematik daraus die nächsten Achspositionen A1,A2,A3,A4,A5,A6 für jede Achse gibt es einen momentanen Positions Istwert. Und Du lieferst jetzt den Sollwert. Die Positionsregelung jeder Achse gibt dann per Soll-Istwert Vergleich eine Steuergröße an den jeweiligen Motortreiber. Ändert sich dabei die Drehrichtung einer Achse, dann ist das nur für die Achsregelung interessant und für Dich ob das Delta zwichen Istwert und Sollwert kleiner ist als die Grenzwerte des Antriebs. Deshalb hat man üblicherweise bei einer NC Achse eine Positionsregelung, eine nachgeschaltete Geschwindigkeitsregelung und eine nachgeschaltete Stromregleung. alle drei Regelungen haben dabei noch Überwachungen ob Grenzwerte überschritten werden und können so entweder einen Notstop auslösen oder eine Runterskalierung der Sollwerte der anderen Achsen durchführen, so das alle Achsen innerhalb der Betriebsgrenzwerte bleiben und die Bah nnicht verlassen wird.
Bei der Positionsregelung wird meist gestoppt, wenn eine Bahn eine Schutzzone verletzt.
Das dabei trotzdem was schiefgehen kann, merkt man, wenn zwar der TCP (dessen Position man so überwacht) immer im grünen Bereich ist, aber z.B.: der Ellenbogen hinten einmal alles abräumt. (selbst einmal erlebt, wie aus einer vollverkleideten Schleifmaschine ein Cabrio wurde).

Aus dem Grund werden komplette Roboterzellen trotzdem virtuell einmal vorher simuliert und alle Kollisionen der Volumenkörper zeigen dann, das man die Zelle umbauen muß oder die Bahn anders Planen muß.
Nich um sonst sieht man oft Roboter schöne Pirouetten mit einem Teil durchführen. Da get es oft darum, das Kabel oder Schläuche nicht abgerissen werden, oder das Teil nur durch das Drehen kollisionsfrei durch die umgebenden Objekte manövriert werden kann.

[raze92]

Jetzt hab ich es, Danke!

Die Kommunikation der Gelenke folgt über ein CAN-Bus. Somit ist eine gleichzeitige Ansteuerung der Achsen nicht möglich. Wie geht man damit um?
Meine Ideen waren:
1. Verzögerung vernachlässigen, aber die Gelenke immer in der selben Reihenfolge steuern oder
2. eine geplante Versetzung der Steuerung und diese Verzögerung in der Steuerung mit berücksichtigen.

[i_make_it]

CAN Bus ist von hause aus nicht echtzeitfähig, zwar kann man durch entsprechende Identifier eine Nachricht priorisieren, aber schon bei der Rückantwort (z.B.: bei Regelkreisen) kann nicht sichergestellt werden innerhalb welcher Zeitspanne die antworten.
Kommt es jetzt noch zu Störungen und Nachrichten schaffen den Annahmetest beim Empfänger nicht, hat man das selbe Problem wie bei Ethernet mit CSMA/CD.
Irgendwann kommt das Paket mal an. aber Echtzeitfähigkeit und somit Regelung ist damit nicht möglich.
Das ginge nur wenn man für jede Achse einen eigenen CAN Bus mit genau 2 Knoten betreibt (Achse und Controller).
Wenn man entsprechend langsam fährt, kann man mit Varinate 1 arbeiten, aber sicher ist man nie wirklich.
Variante 1 ist der zeitgesteuerte Buszugriff (Token passing)
CAN Bus funktioniert aber Bedarfsorientiert.
Also muß man über Prioritäten versuchen das hinzubekommen. Da gibts dann halt die Probleme mit den Antworten wenn man nur einem Knoten erlaubt Nachrichten mit den höchsten Prioritäten zu versenden.
Erlaubt man es allen kann alles durcheinander kommen.
mehr wie PTP wird man da niemals sicher hinbekommen.
Im Idealfall kappt es aber es gibt keine Vorhersagbarkeit für das Timingverhalten und somit keine verlässliche, robuste Regelung über mehr als eine Achse.

Vom Prinzip her kann man übrigens im CAN Bus alle Achsen auf einmal ansprechen. Man benötigt nur eine Nachricht in der die Daten für alle Achsen stehen. Dann müssen alle Achsen sich auch als Empfänger dieser Nachricht fühlen und dann muß jede Achse aus der Nachricht ausschließlich die Daten verarbeiten die für sie bestimmt sind und den Rest ignorieren.

[raze92]

Die Hauptaufgabe des Roboters wird es sein, Gegenstände an eine Bestimmte Position zu fahren und dort zu halten, so wie das auch ein Mensch Kann und in der ähnlichen Präzision. Aufgaben wie Abarbeitung von Materialien wo eine hohe Präzision gefordert wird, wird der Roboter nicht erfüllen müssen.

[raze92]

Wie sieht es bei einer PTP Bewegung aus, wird da auch vorher in die 3 Phasen unterteilt oder Regelt die Beschleunigung jede Achse für sich selbst?

[HaWe]

die Abb. 3.2 und 3.3 zu PTP
(asynchron und synchron, mit dem nicht zu vernachlässigenden Hinweis "Damit ist die PTP-Steuerung für alle Bewegungen ungeeignet, bei denen es auf eine genaue Bahnführung ankommt. Beispiele dafür sind Schweißen, Montage, Kleben, Falzen, Entgraten usw.")
und die Abb 3.4 zu CP
mit den entsprechenden Erklärungen machen doch eigentlich die Sache sehr klar, oder habe ich deine Frage vlt nicht recht verstanden?

- - - Aktualisiert - - -
(edit, OP-post wurde zwischenzeitlich gelöscht)

zunächst ist das ganze in deinem pdf nur ein Modell zur Erklärung, nicht zur praktischen Implementierung.

Praktisch wird man Geschwindigkeiten und Streckenpunkte idealerweise durch unabhängige Setpoints per PID-Steuerung implementieren, mit Rampup- und Rampdown-Funktionen für Anfang und Ende und konstanten Geschwindigkeits-Setpoints für die Strecke dazwischen.

Wie man das richtig programmiert, ist eine ziemliche Herausforderung, schon alleine die korrekte Zielansteuerung ohne Geschwindigkeitssteuerung "in der Mitte" ist in C einigermaßen unübersichtlich, wenn man Oszillationen und Überschwingen und Verrecken unter Last kurz vor dem Ziel vermeiden will:

//*************************************************************//*************************************************************
//                      motor API                              //                      motor API
//*************************************************************//*************************************************************

#define  MAXMOTORS          10    // maximum number of motors
#define  MAXMOTORSLOCAL      2    // maximum number of local motors
#define  MAXPWMRANGE       255    // maximum software-pwm range (0-255)

// motor control structure

typedef struct  {   
                // electrical motor pins
      uint8_t   pind1, pind2, pinpwm;    // dir + pwm L293 H-Bridge type
      uint8_t   pinQa, pinQb;            // rotary enc pins Qa,Qb
     
                // pwm and encoder values
      int16_t   dirpwm;     
      int32_t   motenc, oldenc;          // rotary encoder values
     
                // PID   
      pthread_t tid;
   
                // PID custom target values
      int32_t   target;                  // set target
      int16_t   tarpwm;                  // motor target speed
                // PID custom regulation parameters
      double    P;                       // P: basic propotional to error
      double    I;                       // I: integral: avoid perish
      double    D;                       // D: derivative: avoid oscillating
      double    precis;                  // error precision to target
      int32_t   regtime;                 // PID loop time
      double    damp;                    // damp the integral memory
      int8_t    cont;                    // target: continue or hit once
                // internal control variables
      int16_t   runstate;                // monitors runstate
      int16_t   outp;                    // PID control output value
      int16_t   maxout;                  // max output (max motor pwr)
      int32_t   read;                    // current sensor reading
      double    err;                     // current error
      double    integr;                  // integral of errors
      double    speed;                   // current speed
      int8_t    stopPIDcontrol;          // flag for external termination
     
} tEncMotor;


tEncMotor motor[MAXMOTORS];


#define motorLeft  motor[0]
#define motorRight motor[1]



// motor runstates:

#define OUT_REGSTATE_NULL           0
#define OUT_REGSTATE_COAST          2
#define OUT_REGSTATE_BRAKE          3
#define OUT_REGSTATE_EMERG_STOP     5

#define OUT_REGSTATE_ON             8
#define OUT_REGSTATE_RAMPUP         9
#define OUT_REGSTATE_RAMPDOWN      10

#define OUT_REGSTATE_PIDIDLE       15

#define OUT_REGSTATE_ACTIVE        16 
#define OUT_REGSTATE_PIDSTART      17
#define OUT_REGSTATE_PIDEND        18
#define OUT_REGSTATE_PIDHOLD       19
#define OUT_REGSTATE_PIDHOLDCLOSE  20





//*************************************************************

#define motorCoast(nr) motorOn(nr, 0)  // alias for motor coast

//*************************************************************



inline void motorBrake(uint nr, int dirpwm) {      // brake by pwm power
   int pwm;
   
   pwm = abs(dirpwm);
   
   digitalWrite(motor[nr].pind1, HIGH);
   digitalWrite(motor[nr].pind2, HIGH);     
   
   motor[nr].dirpwm = pwm;
   softPwmWrite(motor[nr].pinpwm, pwm);    // brake power always > 0   
   
}

//*************************************************************

inline void motorOn(uint nr, int dirpwm) { // motor On (nr, dir_pwm)
   int dir, pwm;                             // signed pwm:
   
   if(dirpwm > 0) dir = +1;                   // pwm > 0: forward
   else if(dirpwm < 0) dir = -1;              // pwm < 0: reverse
   else dir = 0;                              // pwm = 0: coast
   
   if(! _REMOTE_OK_) dirpwm=0;
   pwm = abs(dirpwm);
   
     
   if(dir> 0) {
      digitalWrite( motor[nr].pind1, HIGH);
      digitalWrite( motor[nr].pind2, LOW);     
   }
   else
   if(dir==0) {
      digitalWrite( motor[nr].pind1, LOW);
      digitalWrite( motor[nr].pind2, LOW);
   }
   else {
      digitalWrite( motor[nr].pind1, LOW);
      digitalWrite( motor[nr].pind2, HIGH);
   }
   motor[nr].dirpwm = dirpwm;
   softPwmWrite( motor[nr].pinpwm, pwm);
   
   
}

                       
                       
//*************************************************************//*************************************************************
//                       PID control                           //                       PID control
//*************************************************************//*************************************************************   

// forward: motor API functions
inline void RotatePIDtoTarget (uint nr, int32_t Target, double RotatSpeed); // approach absolute target once
inline void RotatePIDdegrees  (uint nr, int32_t Target, double RotatSpeed); // turn relative degrees
inline void RotatePIDcontinue (uint nr, int32_t Target, double RotatSpeed); // approach target continuously
inline void StopPIDcontrol    (uint nr);               
inline void PIDinit(); // P=0.4, I=0.4, D=10.0
// simple customized PID setting:
inline void SetPIDparam(uint nr, double P,double I,double D);
// extended customized parameter setting:
inline void SetPIDparamEx(uint nr, double P, double I, double D, double prec, int16_t regtime, double damp);   


//*************************************************************                   
                       
inline void PIDcleanup(uint nr) {   
        motorCoast(nr);
        motor[nr].runstate = OUT_REGSTATE_NULL;
        motor[nr].speed    = 0;
        motor[nr].outp     = 0;
        motor[nr].cont     = 0;
}                       
 

//*************************************************************                   

void * PID_calc(void *arg) {
  double  aspeed, damp, PWMpwr, readold, errorold, tprop;
  int32_t    readstart, cmax, cmin;                    // for monitoring
  int32_t    starttime, runtime, clock, dtime;         // timer
  int     regloop;

 
  // arg nach index casten
  unsigned nr = (unsigned)arg;
 
 
  motor[nr].runstate = OUT_REGSTATE_PIDSTART   ;           // reg state: RAMPUP
  motor[nr].read     = motor[nr].motenc;             // get current encoder reading
  motor[nr].err      = motor[nr].target - motor[nr].read; // error to target

  readstart      = motor[nr].read;
  regloop        = 1;


  damp=0;                                   // damp the integral memory

  starttime= millis();


  // appoach target
  _Astart:
  motor[nr].runstate = OUT_REGSTATE_ACTIVE;  // run state: RUNNING

  do {
     
    pthread_testcancel();
    if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
    } 

    dtime    = millis() - clock;
    clock    = millis();
    runtime  = clock - starttime;
    tprop    = dtime/20.0;
   
   
    if ((motor[nr].err==errorold)&& (abs(motor[nr].err)>motor[nr].precis)) damp=1;    // stalling
    else
    damp=motor[nr].damp;

    motor[nr].integr = (damp * motor[nr].integr) + motor[nr].err;

    if((motor[nr].integr) > 3*motor[nr].maxout) motor[nr].integr = 3*motor[nr].maxout; // cut away
    else
    if((motor[nr].integr) <-3*motor[nr].maxout) motor[nr].integr =-3*motor[nr].maxout;

    PWMpwr= (motor[nr].P*motor[nr].err) + (motor[nr].I*motor[nr].integr)*tprop + (motor[nr].D*(motor[nr].err-errorold))/tprop;


    if(PWMpwr >  motor[nr].maxout) PWMpwr=  motor[nr].maxout;   // forward maxout
    else
    if(PWMpwr < -motor[nr].maxout) PWMpwr= -motor[nr].maxout;   // reverse maxout


    motor[nr].speed= (motor[nr].read-readold)*100/dtime;  // rotat speed [degrees/100ms]
    aspeed = abs(motor[nr].speed) ;
       
    if (abs(PWMpwr) > motor[nr].tarpwm )  {
        PWMpwr = sign(PWMpwr) * motor[nr].tarpwm ;
    }

    motor[nr].outp = round(PWMpwr);


    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                      // PID regulation !
    motorOn(nr, motor[nr].outp);                  // action !
    delay(motor[nr].regtime);                       // wait regulation time
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    readold     = motor[nr].read;                           // save old sensor
    errorold    = motor[nr].err;                            // save old error

    motor[nr].read  = motor[nr].motenc;                   // get new encoder value
    motor[nr].err   = motor[nr].target-motor[nr].read;  // new error to target

    if (motor[nr].read>cmax) cmax=motor[nr].read;         // monitor overshooting
    else
    if (motor[nr].read<cmin) cmin=motor[nr].read;         // monitor overshooting

    if ((motor[nr].cont)&& (abs(motor[nr].err)<=motor[nr].precis))
         motor[nr].runstate = OUT_REGSTATE_PIDSTART   ;
    else motor[nr].runstate = OUT_REGSTATE_ACTIVE;

    if (motor[nr].cont) continue;
    if (abs(motor[nr].err)<=motor[nr].precis) {
        regloop +=1 ;
        motor[nr].runstate = OUT_REGSTATE_PIDEND  ;
    }
   
    if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
    }


  } while ((abs(motor[nr].err)>=motor[nr].precis) && (regloop<=5));  // target reached

  motorCoast(nr);                                 // finished - stop motor
  motor[nr].runstate = OUT_REGSTATE_PIDEND;       // run state: RAMPDOWN
  motor[nr].outp=0;

  delay(50);
  motor[nr].read = motor[nr].motenc;
  regloop=1;

  if (motor[nr].read>cmax) cmax=motor[nr].read;            // detect overshooting
  if (motor[nr].read<cmin) cmin=motor[nr].read;            // detect overshooting
  motor[nr].err = motor[nr].target-motor[nr].read;


  if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
  }

  if ((abs(motor[nr].err)>motor[nr].precis))  {goto _Astart;}

  motor[nr].runstate=0;
  delay(1);                                //runstate = IDLE

  return (NULL);
}



//*************************************************************

inline void RotatePID(uint nr, int32_t Target, double RotatSpeed, int8_t cont) {
   
    if( !motor[nr].tid && motor[nr].runstate) {
        motor[nr].runstate=0; // repair
        delay(1);
    }
    if( ( motor[nr].runstate ) || ( motor[nr].tid ) ) {
       motor[nr].stopPIDcontrol = 1;
       delay(1);
       while( motor[nr].runstate || motor[nr].tid );  // wait for PID task to terminate
       delay(1);
       PIDcleanup(nr);
    }
    // init new PID structure 
    motor[nr].runstate = 1;               // set runstate: PID active
    // custom init PID [nr]   
    motor[nr].target = Target;                   // assign target
    motor[nr].tarpwm = abs(RotatSpeed);          // assign max rotation speed
    motor[nr].cont=cont;                         // cont vs. hit once
    // Reset PID control defaults
    motor[nr].outp    = 0;                // PID control output value
    motor[nr].maxout  = MAXPWMRANGE;      // absolute max possible output (max pwr)
    motor[nr].read    = 0;                // current reading
    motor[nr].err     = 0;                // current error
    motor[nr].integr  = 0;                // integral of errors
    motor[nr].speed   = 0;                // current speed
    motor[nr].stopPIDcontrol = 0;         // flag for external termination
   
    // start PID control task
    pthread_create( & motor[nr].tid,      // id speichern in dem dazugehörigen Array-Element
                    0,                    // Default Attribute (threads cancelbar)
                    PID_calc,             // Name der PID-Kalkulationsroutinge
                    (void *) nr);         // der Index des Array-Elements für eine PID Struktur,
                                          // die mit dem Motorindex gleich ist.   
   
    // run pthread task in detached mode, auto-cleanup                                   
    pthread_detach(motor[nr].tid);

}

//*************************************************************                   

inline void StopPIDcontrol (uint nr) {

  if (motor[nr].tid) { // stop PID task if already running
     motor[nr].stopPIDcontrol = 1;   
  }

}
               
//*************************************************************                         
                     
// custom PID parameters Extended
inline void SetPIDparamEx(uint nr, double P, double I, double D, double prec, int16_t regtime, double damp) {
    motor[nr].P       = P;             // P: propotional to error
    motor[nr].I       = I;             // I: avoid perish
    motor[nr].D       = D;             // D: derivative: avoid oscillating
    motor[nr].precis  = prec;          // error precision to target
    motor[nr].regtime = regtime;       // PID regulation time
    motor[nr].damp    = damp;          // damp error integral
}


//************************************************************* 
                 
// custom PID parameters
inline void SetPIDparam(uint nr, double P, double I, double D) { 
    motor[nr].P     = P;             // P: propotional to error
    motor[nr].I     = I;             // I: integral: avoid perish
    motor[nr].D     = D;             // D: derivative: avoid oscillating
}                       
   
                   
//*************************************************************                                       
                     
inline void PIDinit() {
  for (uint nr=0; nr < MAXMOTORS; ++nr) {
    SetPIDparamEx(nr, 0.40, 0.40, 10.0, 1.0, 5, 0.75); // p,i,d, precis, reg_time, damp
    delay(1);
  }
}

//*************************************************************                                       


inline void RotatePIDtoTarget(uint nr, int32_t Target, double RotatSpeed) {
   RotatePID(nr, Target, RotatSpeed, false);
   delay(1);
}

//*************************************************************                                       

inline void RotatePIDcont(uint nr, int32_t Target, double RotatSpeed) {
   RotatePID(nr, Target, RotatSpeed, true);
   delay(1);
}

//*************************************************************                                       

inline void RotatePIDdegrees(uint nr, int32_t Target, double RotatSpeed)  {
   RotatePID(nr, Target+motor[nr].motenc, RotatSpeed, false);
   delay(1);
}


//*************************************************************
//  ^^^^^^^^^^^^^^ PID end ^^^^^^^^^^^^^
//*************************************************************

[raze92]

Ich habe PTP und LIN seperat betrachtet. Für beide Bewegungen erzeuge ich eine Tabelle die die Gelenkstellungen beinhaltet für die zu fahrende Bahn.

Ich übergebe der Gelenksteuerung die Tabelle mit den Gelenkstellungen mit zusätzlich den Geschwindigkeiten jedes Gelenks an jedem Zwischenpunkt. Die Gelenke sollen dann mit der Geschwindigkeit fahren, bis die Soll-Gelenkstellung erreicht wurde. Dann wird die nächste Geschwindigkeit an die Gelenke gesendet usw.
Da kam ich aber jetzt auf den Schluss, dass mit diesem vorgehen eine Bahn schnell abweichen kann und nicht mehr in die Bahn zurück kommt.

Wenn ich jetzt stattdessen eine Interpolation durchführe der Linearen Bahn und mit den 3 Phasen das Geschwindigkeitsprofil der Bahn bestimme und jeden Interpolationspunkt nun über eine synchrone PTP-Steuerung übergebe:
Wie Regel ich die Geschwindigkeit in der PTP-Steuerung des TCP?