Das war sehr verständlich. Danke!
Wie Realisiert man das denn programmtechnisch am sinnvollsten? Misst man die Abweichung der Gerade und wenn sie einen festgelegten Wert überschreitet regelt man nach und berechnet somit weniger Zwischenpunkte bei der Bahnberechnung? Oder Berechnet man mehr lieber mehr Zwischenpunkte um zur Laufzeit nicht nachregeln zu müssen? Oder ist eine Kombination von beiden das A und O?
Bei einer Graden ist die Bewegung für alle beteiligten Raumachsen linear und proportional zueinander (anders als bei einer Kreisbahn).
Wenn man für alle Raumachsen die Steigung hat, benötigt man keine Zwischenpunkte die erreichnet eine Steuerung sich zur Laufzeit dynamisch anhand einer definierten kleinsten Schrittweite, iterativ, bis die Istposition mit der Sollposition (Endpunkt) übereinstimmt.
Beispiel: wenn sich die Steigungen 1 zu 2 zu 3 verhalten, kann die Steuerung bei einer Startposition 100, 30, 50. zur laufzeit die Positionen 101, 32, 53; 102, 34, 56;..... berechnen.
Je nach Auflösung der Meßsysteme und Antriebe wird das dann entsprechend genau angefahren.
Bei Industriesystemen hat dann auch ein Drehgeber mal 5000 Digit auf 360°. Und mit einem Signal multiplizierer (Interpolations und Digitalisierungselektronik) wird das dann bis zu 100 Fach verfeinert.
Bei dann 50000 Digit auf 360° sind das Auflösungen von einer halben Bogenminute. Das ist dann ein PTP Inkrement das zwar mathematisch noch als solsches gesehen werden kann, aber praktisch ist das Delta so klein, das es ein Bahnsteuerung bleibt.
Und das Berechnen der Positionen erfolgt bei Industriesteuerungen meist mit 20-100 kHz. Also alle 50 bis 10 Nanosekunden.
Beim Selbstbau eines 6-Achs Verikal Knickarm Roboters werden die Encoder vermutlich alleine schon wegen den Kosten weniger Auflösung haben und die Signalvervielfachung entfällt meist auch mangels entsprechender Elektronik Kenntnisse.
Mit zwei handelsüblichen Fensterdiskriminatoren ließe sich der Sinus am A und B Kanal eines Drehgebers in je drei Spannungspegel teilen, und man erhält so eine Verdopplung der Auflösung gegenüber dem reinen Geber mit nachgeschalteten Schmitt Trigger.
Hat jetzt zwar nicht mit der mathematischen Lösung zu tun aber mit der praktischen Verbesserung der für die Lösung notwendigen Daten.
Das ergibt Sinn. Was ist mit der Orientierung?Zitat von i_make_it
Trotzdem muss doch für jeden Schritt eine Rücktransformation erfolgen. Ist das nicht ein ziemlich großer Rechenaufwand? Ist es da nicht schlauer die Gelenkstellungen schon vorher zu berechnen?
Der verbaute Encoder pro Achse hat eine Auflösung von 14-Bit also ca. 0,02° Genauigkeit. Ich hab leider noch keine praktischen Erfahrung bei der Berechnung der Inverse Kinematik. Die Idee war zur Steuerung ein Minirechner wie Arduino, Raspberry Pi etc. zu verwenden. Schaffen es solche Rechner die mögliche Genauigkeit auszunutzen?
Wenn man sich entscheidet, die Schrittweite zu vergrößern, ist es möglich die maximal mögliche Abweichung zur Bahn zu bestimmen?
Geändert von raze92 (05.04.2017 um 09:38 Uhr) Grund: Zusätzliche Frage
Die Orientierung und den Offset zwichen dem roboterbezogenen Weltkoordinatensystem und dem roboterbezogenem Polaren Weltkoordinatensystem bestimmst Du in den Systemparamtern des Roboters.
Willst Du nur in einem Raumquadranten mit positiven Werten für X, Y und Z arbeiten, dann verschiebst Du den Nullpunkt des polaren Koordinatensystems in allen Achsen jeweils soweit ins Plus, bis niemals ein negativer Wert erreicht werden kann.
Falls Du mit Orientierung A, B und C Ausrichtung des TCP meinst, die ist neben den X, Y und Z Koordinaten des TCP natürlich auch immer mit anzugeben.
Üblicherweise erfolgt das heutzutage in einer 4Kreuz4 Matrix. Da man damit auch direkt die Transformationen durchführen kann.
https://prof.beuth-hochschule.de/fil...ng_Teil_03.pdf
JA per Inverser Kinematik
JA.
Hat man früher so gemacht, als die Rechner noch langsam und groß waren.
Als ich 1984 in der ausbildung das erste mal mit Industirerobotern zu tun hatte, waren einige (altere) davon noch so das man entweder hunderte Punkte geteacht hat um mit Synchonen PTP eine Bahnsteuernug zu simmulieren oder das für diese Roboter tatsächlich die Posen entlang der Bahn vorberechnet wurden und dementsprechend viele Datensätze einafach Sequentiel durchfahren wurden.
Beim synchonen PTP fahren nicht mehr alle Achsen full speed sondern proprtional zueinander. wenn also eine Achse zwichen zwei Posen 500 digit fahren muß und eine andere nur 50 Digit, dann fährt die erste 100% und die zweite nur 10% speed.
Nur so ist es bei PTP möglich das Verhalten von Bahnsteuerungen zu simulieren und zwichenpunkte zu verschleifen ohne jedesmal auf null zu bremsen und wartezeiten für die langsamste Achse einzufügen.
Dabei dürfte es sich um die Auflösung eines ADC handeln da Encoder entweder pro Kanal seriell einfach nur ein Rechtecksignal mit geschwindigkeitsabhängiger Frequenz liefern oder pro Kanal einen Sinus.
Wobei die Kanäle um 90° (beim Sinus) oder um Lambda/4 (ein Viertel der Frequenz) bei digitalen Encodern, zueinander verschoben sind.
Bei einem ADC mit 14 Bit kann es sein das du die beiden letzten Bit im Signalrauschen verlierst. Das müsstest Du dann erst mal testen.
Minirechner mit eigenem OS, haben als Haupthandycap die mangelnde Echtzeitfähigkeit.
Du hast Tasks vom Kernel, von den Hardwaretreibern, Daemons, etc. und dann mal den oder die Tasks von der Steuersoftware.
Vom Prinzip her funktioniert das und mit CNC Software wie z.B. Mach3 sieht man auch das es bis zu einer gewissen Geschwindigkeit auch mit der notwendigen Präzission und Wiederholgenauigkeit funktioniert.
Darüber hinaus werden dann spezielle G-Code Steuerkarten angeboten.
Zum Einsteigen kann man durchaus damit anfangen. wenn man aber hohe Positioniergenauigkeit, hohe Wiederholgenauigkeit und hohe Geschwindigkeit (kurze Zykluszeiten der Positions- und Geschwindikeitsregelung) haben will, kann man sich darauf einstellen auf eine Lösung mit besserer Echtzeitfähigkeit umsteigen zu müssen.
Da kommen dann Microkontroller oder proprietäre Singletask Betriebssysteme ins Spiel. (nicht umsonst kostet oft eine Driveunit mit Lizenz mehr als der dazugehörige Arm).
Ja kann man berechnen.
Ist genau der Selbe Aufwand der zur Regelung des eigenlichen Arms notwendig ist, da man bei der Berechnung eigentlich nur das verhalten simuliert.
Wobei halt nicht nur Entfernung sondern auch Geschwindigkeit maßgeblich sind.
(das Beispiel nicht real nachmachen)
Mann fährt mit 50 eine langestreckte Kurve und macht einfach mal für 1 Sekunde die Augen zu.
Dann das selbe mit Tempo 100. Die gefahrene Strecke und wie weit man aus der Spur getragen wurde unterscheiden sich.
Jetzt hat man eine Beifahrer, der alle 100 Meter ein Signal gibt. und man macht das selbe nochmal mit 50 und mit 100 Sachen.
Die Streke die man mit geschlossenen Augen fährt ist also immer gleich. aber wegen der unterschiedlichen Geschwindigkeit unterscheidet sich immer noch die Dynamik und damit die Größe der Abweichung.
Macht man das ein drittes mal, aber diesmal mit 50 Metern dann wird auch die Abweichung kleiner.
Das Delta kann man dann soweit verkleinenr bis man an die Grenzen der Sensorauflösung kommt, Sprich Du kannst nicht mehr sagen ob Du aus der Spur getragen wurdest oder nicht, weil beide Durchläufe für Dich gleich aussehen.
Geändert von i_make_it (06.04.2017 um 06:34 Uhr)
Vielen Dank! Soweit hat sich einiges geklärt.
Ich will eine gerade Bahn fahren. Der Steuerung übergebe ich also Zielpunkt und Zielorientierung und zur Laufzeit wird nun iterativ die zu fahrenden Positionen berechnet. Wie bestimme ich aber die Geschwindigkeit/Beschleunigung? Es kommt ja vor, dass ein Gelenk mal seine Richtung änder muss, dann muss ich doch schon vorher wissen, an welcher stelle das passiert, um so Beschleunigen/Bremsen zu können. Rechne ich schon so weit vor, um entsprechend Anpassung an die Geschwindigkeit machen zu können?
Geändert von raze92 (05.04.2017 um 14:24 Uhr)
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.
Geändert von i_make_it (06.04.2017 um 06:57 Uhr)
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.
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:
Code://*************************************************************//************************************************************* // 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 ^^^^^^^^^^^^^ //*************************************************************
Berechtigungen
Zitieren
Lesezeichen