Archiv verlassen und diese Seite im Standarddesign anzeigen : pthread-Task als Methode einer C++ Klasse?
hallo,
ist es möglich, einen pthread-Task als Methode eines C++ Objekts zu implementieren, um mehrere solcher Tasks über mehrere Instanzen des Objekts unabhängig und pseudo-parallel starten zu können ?
Es geht um kompliziertere PID-Regler für Motoren mit Quadraturencodern, jeder Motor soll über eigene PID-Task-Instanzen (endlos oder selbstterminierend) gesteuert werden. Der Aufruf der Motor-PID-Tasks wird sehr häufig gebraucht und nötig sein, und zwar für jede einzelne Motorstellung, die die einzelnen Motoren jeweils einzeln und unabhängig / asynchron immer wieder neu und mit veränderten Zielwerten und Abbruchbedingungen anfahren müssen (Zielstellung anfahren, dann coasten oder dann bremsen, oder dann dauerhaft gegen Widerstand oder passive externe Verstellkräfte bis zum gesonderten Abbruchbefehl approximiert halten), teilweise werden auch 2 Motoren paarweise miteinander per PID z.B für "perfekten" Geradeauslauf zu synchronisieren sein.
Als Einzel-Tasks mit Einzel-Routinen gibt es bereits eine Lösung, ich möchte nur nicht alle Methoden für 8 oder 10 Motoren 8 oder 10x bandwurmartig immer einzeln neu schreiben.
Du willst ein Objekt der Klasse A erstellen, welches beim Aufruf von Methode B einen Task mit PID-Regler startet?
mfg
hm, vlt doch nicht ganz so.... ich glaube soooo könnte man es vlt ausdrücken....:
ich habe mir vorgestellt, ein Objekt PID zu entwerfen mit verschiedenen Variablen (für kP, kI, kD u.a.m.) und einer Methode
PID.startPIDtask();
dieser soll dann einen spezifischen pthread task starten
pthread_t threadID;
pthread_create(&threadID, NULL, threadName, NULL);
jede Instanz
new PID PID_A;
new PID PID_B;
new PID PID_C;
...
soll dann in der Lage sein, ihren eigenen task zu starten und nach dessen Beendigung wieder zu "joinen",
pthread_join(threadID, NULL);
um ihn bei Bedarf jederzeit erneut starten zu können.
Um das Konzepz etwas konkreter zu zeigen, hier wäre ein Setup ohne diese Objekte, wo für jeden Motor eine eigene globale statische Task-Methode programmiert werden musste (mit "NXC" für lego NXT, mangels OOP und Funktionspointer nicht anders möglich):
http://www.mindstormsforum.de/viewtopic.php?f=25&t=7521&p=61935#p61930
ich habe 3 feste (Motor-spezifische) task Routinen plus eine (Motor-spezifische) PID-Struktur, für jeden Motor eine eigene,die über reichlich komplizierte und umständliche Hilfs-Funktions-Aufrufe immerhin selektiv asynchron gestartet und beendet werden können (start task / stop task).
So kompliziert und "ungelenk" wollte ich es jetzt für 8 -10 Motoren nicht wieder machen müssen, daher die Idee mit dem PID-Objekt statt der PID-Struktur.
Ich hoffe, das könnte das Problem vlt ein wenig erhellen..... :-/
Ich habs auch noch nicht ganz verstanden.
Hier nur mal ein Beispiel in normalem C++ (11 oder 14), nur erstmal um zu verstehen, wie die Klasse aussehen soll
#include <cstdio>
#include <thread>
class PidClass
{
public:
void Control(unsigned int n)
{
for (unsigned int i = 0; i < n; i++)
{
printf("Controlling ...\n");
}
}
std::thread MakeTask()
{
return std::thread([this] { Control(3); });
}
};
int main()
{
PidClass pid;
auto t1 = pid.MakeTask();
// ...
t1.join();
auto t2 = pid.MakeTask();
// ...
t2.join();
return 0;
}
Da passiert nicht viel, es gibt nur sechs mal "Controlling ..." aus.
Ist das so gemeint ? Die Klasse hat eine Methode die was tut (mit Parametern) und eine zweite, die einen Thread erzeugt der die erste irgendwie verwendet ?
---
Achso vergessen.
Ja, kann man auch zu Fuss machen, wird aber unschön
http://stackoverflow.com/questions/1151582/pthread-function-from-a-class
die Methode soll im Prinzip diesen Task als pthread-Task als "Muster" beinhalten - allerdings müssen später die Instanzen des Objekts für die pthread-Tasks natürlich eigene IDs und Namen bekommen, wenn sie von den Instanzen "erschaffen" und gestartet werden. C11-Tasks will ich nicht verwenden, sondern nur pthread Tasks:
// "Pseudocode"-Vorlage aus NXC:
task task_PID_A() {
float aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
char regloop;
PID_A.runstate = 0x10; // reg state: RAMPUP
PID_A.read = (MotorRotationCount(OUT_A)); // get current encoder reading
PID_A.err = PID_A.target - PID_A.read; // error to target
readstart = PID_A.read;
regloop = 1;
damp=0; // damp the integral memory
starttime= CurrentTick();
// appoach target
_Astart:
PID_A.runstate = 0x10; // run state: RUNNING
do {
dtime = CurrentTick() - clock;
clock = CurrentTick();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((PID_A.err==errorold)&& (abs(PID_A.err)>PID_A.precis)) damp=1; // stalling
else
damp=PID_A.damp;
PID_A.integr = (damp * PID_A.integr) + PID_A.err;
if((PID_A.integr) > 3*PID_A.maxout) PID_A.integr = 3*PID_A.maxout; // cut away
else
if((PID_A.integr) <-3*PID_A.maxout) PID_A.integr =-3*PID_A.maxout;
PWMpwr= (PID_A.P*PID_A.err) + (PID_A.I*PID_A.integr)*tprop + (PID_A.D*(PID_A.err-errorold))/tprop;
if(PWMpwr > PID_A.maxout) PWMpwr= PID_A.maxout; // forward maxout
else
if(PWMpwr < -PID_A.maxout) PWMpwr= -PID_A.maxout; // reverse maxout
PID_A.speed= (PID_A.read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(PID_A.speed) ;
if (aspeed > PID_A.tarpwm) {
PWMpwr = sign(PWMpwr)*PID_A.tarpwm;
}
PID_A.outp = round(PWMpwr);
//************************************************** ************************
// PID regulation !
OnFwd(OUT_A, (PID_A.outp)); // action !
Wait(PID_A.regtime); // wait regulation time
//************************************************** ************************
readold = PID_A.read; // save old sensor
errorold = PID_A.err; // save old error
PID_A.read = (MotorRotationCount(OUT_A)); // get new encoder value
PID_A.err = PID_A.target-PID_A.read; // new error to target
if (PID_A.read>cmax) cmax=PID_A.read; // monitor overshooting
else
if (PID_A.read<cmin) cmin=PID_A.read; // monitor overshooting
if ((PID_A.cont)&& (abs(PID_A.err)<=PID_A.precis)) PID_A.runstate = 0x60;
else PID_A.runstate = 0x20;
if (PID_A.cont) continue;
if (abs(PID_A.err)<=PID_A.precis) { regloop +=1 ; PID_A.runstate = 0x40; }
} while ((abs(PID_A.err)>=PID_A.precis) && (regloop<=5)); // target reached
Off(OUT_A); // finished - brake motor
PID_A.runstate = 0x40; // run state: RAMPDOWN
PID_A.outp=0;
Wait(50);
PID_A.read = MotorRotationCount(OUT_A);
regloop=1;
if (PID_A.read>cmax) cmax=PID_A.read; // detect overshooting
if (PID_A.read<cmin) cmin=PID_A.read; // detect overshooting
PID_A.err = PID_A.target-PID_A.read;
if ((abs(PID_A.err)>PID_A.precis)) {goto _Astart;}
PID_A.runstate=0;
Wait(1); //runstate = IDLE
}
aufgerufen werden soll das Objekt bzw. seine Instanz mit seinem Task von so einer Funktion
(port == "Motornummer" oder auch A, B, C, D, E,.... ):
void RotatePIDtoTarget (char port, long Target, float RotatSpeed); // approach absolute target once
void RotatePIDdegrees (char port, long Target, float RotatSpeed); // turn relative degrees
void RotatePIDcontinue (char port, long Target, float RotatSpeed); // approach target continuously
void StopPIDcontrol (char port); // stop PIDummer):
Das Objekt soll wie diese Struktur aussehen plus alle notwendigen Hilfs-Funktionen und pthread-tasks enthalten, damit sie von allen Instanzen benutzt werden können (kann eigentlich alles public sein):
struct PIDstruct {
// custom target values
long target; // set target
int tarpwm; // motor target speed
// custom regulation parameters
float P; // P: proportional to error
float I; // I: integral: avoid perish
float D; // D: derivative: avoid oscillating
float precis; // error precision to target
int regtime; // PID loop time
float damp; // damp the integral memory
char cont; // target: continue or hit once
// internal control variables
char runstate; // monitors runstate
int outp; // PID control output value
int maxout; // max output (max motor pwr)
long read; // current sensor reading
float err; // current error
float integr; // integral of errors
float speed; // current speed
} ;
- - - Aktualisiert - - -
edit:
ich muss auch sagen, die Erklärungen in http://stackoverflow.com/questions/1151582/pthread-function-from-a-class sind mir erheblich zu schwierig - ich habe bisher nie selber Klassen erstellt - mein Code ist fast immer nur ANSI C. Wahrscheinlich muss ich das aufgeben, wenn es wirklich so kompliziert sein muss.
Gibt es einen Grund warum es pthread direkt sein muss und das C++ 11 frontend nicht? (Btw. du redest über Tasks meinst aber Threads. Das ist ein ziemlicher Unterschied)
Mit C++ 11 ist es super einfach einen Thread in einer klasse laufen zulassen (Siehe Post von Mxt - Wobei der Code meiner Ansicht nach so nicht funktionieren wird ;)
Ich finde das Beispiel auf Stackoverflow ganz gut: https://stackoverflow.com/questions/10998780/stdthread-calling-method-of-class
Wenn du unbedingt direkt auf die pthread API programmieren willst wirst du um eine kompliziertere Lösung wie hier ( https://stackoverflow.com/questions/21725935/pass-the-class-method-as-pthread-start-function ) nicht drumherumkommen.
der Grund für pthread ist, dass ich mich mittlerweile ENDLICH einigermaßen damit zurechtfinde, dass ich die thread priority sinnvoll justieren kann, dass auch wiringPi mit pthread arbeitet (auch mit vereinfachten Mutexen) und ich auch sonst immer nur pthread einsetze. C++ ist mir eigentlich ein Greuel, so wie auch andere OOP Sprachen (Java, C#) und ich nutze es nur, wenn zufällig ein paar libs ihre API Funktionen eben so zur Verfügung stellen (Arduino automatisch, beim Raspi v.a. wenn zwingend iostream gebraucht wird). Auch Gordon Henderson mit seinem wiringPi teilt übrigens meine Abneigung gegen C++, auch er nutzt (wie er mir mal schrieb) "aus Prinzip" nur ANSI C.
Ich habe aber keinen Schimmer, was (wie bei stackoverflow) in diesem Zusammenhang vektoren, this, eine static class method, eine plain ordinary function, to bootstrap the class, virtual, protected, public, private, static void * InternalThreadEntryFunc(void * This) {((MyThreadClass *)This)->InternalThreadEntry(); return NULL;}, boost::thread, ein pointer to "context", ein pointer to an instance, C++ construct - 'friend' function und all dieses Gedöns ist und den Unterschied zwischen :: und -> werde ich mit Sicherheit auch nie kapieren.
Ich gebe ntl zu, dass OOP hier und da Vorteile hat (wie bei Arduino Serial und TWI) und hatte vermutet, dass ich dieses schnell mal so auch ganz einfach bei meinem PID Controller nutzen kann, wenn ich nur ein PID Muster erstelle und dieses dann per new PID... samt seiner dann miterschaffenen Tasks einfach "vervielfachen" könnte. Leider ist mir der ganze OOP Rattenschwanz, der offenbar zwangsweise mitgeschleppt werden muss und der mir regelrechte Hirnkonvulsionen verursacht mit progressivem Gefühl mich unstillbar übergeben zu müssen, über alle Maßen bei weitem zu wirr, zu umständlich und zu aufwändig.
Ich wollte in mein Objekt nur irgendwie ganz einfach reinschreiben
pthread_t threadID;
pthread_create(&threadID, NULL, threadName, NULL);
und hatte gehofft, alle Instanzen würden dann auch ganz einfach so ihren eigenen pthread task starten, laufen lassen und beenden können, ohne dass es Probleme bei 8 oder 10 gleichzeitig oder nacheinander aufgerufenen Instanzen gegeben hätte.
Da ist es 1000x einfacher, wenn ich meine statischen Task Funktionen 10x untereinander per copy + paste schreibe als
void * PID_A
void * PID_B
void * PID_C
usw.
wie ich es in NXC gemacht habe - und bleibe beim guten alten ANSI C . :(
Also std::thread ist nur eine dünne Verpackung um entweder pthread oder Windows threads. Man komm auch noch an den verpackten "nativen" Thread heran. Muss man auch, z.B. zum Setzen von Prioritäten.
Ich wollte in mein Objekt nur irgendwie ganz einfach reinschreiben
Da sind die Regeln ganz einfach: In C-Funktionen kann man nur statische Memberfunktionen von Objekten als Parameter übergeben. Objektinstanzen werden als void* übergeben.
ich weiß gar nicht was statische Funktionen oder statische Memberfunktionen von Objekten sind und was nicht.... und was das für mein PID Problem jetzt eigentlich bedeutet.... :confused:
http://en.cppreference.com/w/cpp/language/static
- - - Aktualisiert - - -
(Siehe Post von Mxt - Wobei der Code meiner Ansicht nach so nicht funktionieren wird ;)
Auf einem Pi habe ich ihn nicht getestet, aber in Visual Studio läuft er.
".... und was das für mein PID Problem jetzt eigentlich bedeutet.... :confused: "
Ohne dir zu nahe treten zu wollen, du schimpfst auf OOP ohne dich damit genau beschäftigt zu haben. Das C++ nunmal anders programmiert wird als C, auch wenn C eine Untermenge von C++ ist, ist nunmal dem geschuldet, dass es eine andere Programmiersprache ist. Und eine andere Programmiersprache muss man eben auch erst mal lernen. Zu einer Programmiersprache gehören eben nicht nur die keywords und die Syntax sondern eben auch Konzepte und Hintergrundwissen bezüglich dem was unter dem Oberfläche passiert.
Mein Vorschlag wäre nimm std::threads in deiner Klasse. Ein std::thread ist unter Linux genau das selbe wie ein thread der über Posix (pthread) gestartet wurde. Wenn du sachen machen willst wie thread abbrechen oder priority ändern. Lässt du dir von dem std::thread sein native handle (Das auch nur ein Verweis auf einen pthread ist) geben und kannst das ganz normal den pthread funktionen übergeben. Der Vorteil dabei ist, du sparst dir eben das etwas komplizierte Aufrufen eines pthreads auf eine Klassenfunktion.
".... und was das für mein PID Problem jetzt eigentlich bedeutet.... :confused: "
Es beantwortet die Frage.
Statt im obigen Beispiel
std::thread MakeTask()
{
return std::thread([this] { Control(3); });
}
müsste man etwas in der Art
static void MakeTask(void* this_instance)
{
((PidClass*) this_instance)->Control(3);
}
schreiben. Dann könnte man
int main()
{
PidClass pid;
pthread_t t;
pthread_create(&t, NULL, &PidClass::MakeTask, &pid);
Ungetestet und aus dem Kopf geschrieben, zeigt nur das Prinzip.
ich will doch gar nichts "returnen"...?
ich will doch nur eine Funktion einfügen, die was ausführt, aber nur eben als task-Funktion, nicht als "normale" Funktion... :confused:
return ((PidClass*) this_instance)->Control(3);
das hier ist mir allerdings schon wieder komplett unverständlich:
pthread_create(&t, NULL, &PidClass::MakeTask, &pid);
nee, ich gebe es auf, hat keinen Sinn, OOP mit C++ ist wirklich nicht mein Ding.
Ja, da war ein return zu viel.
Hallo HaWe,
also nachdem ich mir dein Mindstrom Code angesehen habe, wäre wohl der erste Schritt Deine Berechnungsroutinen "task_PID_[ABC]" so umzuscheiben, das sie als Parameter einen Zeiger auf eine struct eines PIDs bekommen. Ansonsten landest Du wieder bei der Code-Redundanz, wie Du sie im Mindstorm-Projekt hast.
#include <stdio.h>
#include <pthread.h>
struct pid {
pthread_t tid;
char* name;
float p;
float i;
float d;
};
struct pid pids[] = {
{0, "Task A", 0.015, 0.2, 0.7 },
{0, "Task B", 0.3, 0.5, 0.80 }
};;
/* thread main */
void *calc_pid(void *arg) {
struct pid* pid = (struct pid*)arg;
int idx = 0;
for(idx = 0; idx < 8; idx++) {
pid->p += .03;
printf("[%d]: %s, p=%f, i=%f, d=%f\n", idx, pid->name, pid->p,
pid->i, pid->d);
}
return 0;
}
int main(int argc, char** argv) {
int idx = 0;
int size = sizeof(pids)/sizeof(struct pid);
/* Konfig fuer eine Aktion beider Motoren,
wie immer die aussehen mag */
for(idx = 0; idx < size; idx++)
if(pthread_create(&pids[idx].tid, 0, calc_pid, &pids[idx])) {
perror("pthread not creatable ");
return 1;
}
for(idx = size - 1; idx >= 0; idx--) {
if(pthread_join(pids[idx].tid, 0)) {
fprintf(stderr, "Couldn't join thread for task %s ", pids[idx].name);
perror(" ");
continue;
}
pids[idx].tid = 0;
}
return 0;
}
Das was in dem einfachen Beispiel in der main() steht würdest du halt für jede Motoraktion (Rotate, Continue etc...) durchimplementieren müssen. Auch müßtest Du den Zeiger auf die einzelne struct ordentlich an die anderen Funktionen weiterreichen, damit diese immer nur auf dieser einen Variable operieren und so keine Wettbewerbsbedingungen entstehen.
Der Nachteil ist IHMO, dass Threads erzeugt und wieder vernichtet werden, was Resourcen kostet.
Ein anderer Ansatz wäre, für jeden PID einmal einen Thread anzulegen, der aus einer synchronisierten Schlange für einen Motor Kommandos liest, ausführt und zurück meldet das es fertig ist. Dann gäbe es einen Thread der den anderen sagt was zu tun ist - quasi das Kommando über sie hat.
Nachteil hier ist, das Du einen Synchronisationsmechanismus implementieren müßtest.
Auch könntest du dir vielleicht mal die OMP (https://www.google.de/search?q=einf%C3%BChrung+omp&ie=utf-8&oe=utf-8&client=firefox-b&gfe_rd=cr&ei=5SvhV47ZG_GA8Qfs-6aQCA) Threading-Variante anschauen, dann hättest du weniger mit Synchronisationsaufwand zu tun und ist möglicher Weise der übersichtlichere Weg für dich.
Das sind so meine Ideen zu deinem Problem.
Gruß
botty
ach ja, gerade erst gecheckt - vlt war es doch noch auch ein Kommunikationsproblem und ich hatte es nicht ausreichend erläutert:
das pid-task create soll nicht von main oder einer anderen Funktion aus, sondern automatisch innerhalb der PID Klasse selber gestartet werden.
Die PID Klasse sollte dafür dann (mindestens) 3 Methoden enthalten,
zum einen eine für den Funktionsaufruf selber ähnlich wie
PID.start(double p, double i, double d, char mode);
PID.start initialisiert damit die p,i,d Konstanten und die Variable Modus (erreichen und dann abschalten oder kontinuierlich halten)
und dann wird damit anschließend ebenfalls die 2. Methode, nämlich
PID.thread
quasi lokal gestartet, nicht global.
später, nach
new PID PID_A;
soll dann alles automatisch laufen durch
PID_A.start(0.5, 10.0, 0.1, true);
Nun läuft der damit intern creierte pthread-Task bis zur Selbstterminierung oder bis zur Beendigung durch einen externen Befehl.
die dritte Methode muss dafür lauten
PID.stop();
nach externem Aufruf per
PID_A.stop();
soll dann der momentan laufende PID_A-Task mit seiner momentan vergebenen PID-ID gestoppt und dann gejoint werden (falls er tatsächlich läuft, ansonsten wirkungslos).
analog das ganze für jede andere erzeugte und dann bei Bedarf gestartete PID-Instanz
new PID PID_B;
new PID PID_C;
new PID PID_D;
new PID PID_E;
usw.
Wo der Thread gestartet wird macht keinen großen Unterschied. Bei std::thread würde sich am eigentlichen Code nichts ändern.
Beim Beispiel mit dem static member nur zwei Dinge:
class PidClass
{
private:
pthread_t t; // Ist jetzt in der Klasse, statt in main
und
pthread_create(&t, NULL, MakeTask, this); // Wenn in einem Member der Klasse verwendet
Oder wenn man in der reinen C Welt bleiben möchte - ist jetzt keine Kritik @Mxt - kann man's halt so machen:
#include <stdio.h>
#include <pthread.h>
struct pid {
pthread_t tid;
char* name;
float p;
float i;
float d;
};
struct pid PID_A = {0, "Task A", 0.015, 0.2, 0.7 };
struct pid PID_B = {0, "Task B", 0.3, 0.5, 0.80 };
/* thread main */
void *pid_task(void *arg) {
struct pid* pid = (struct pid*)arg;
int idx = 0;
for(idx = 0; idx < 8; idx++) {
pid->p += .03;
printf("[%d]: %s, p=%f, i=%f, d=%f\n", idx, pid->name, pid->p,
pid->i, pid->d);
}
return 0;
}
int pid_start(struct pid* pid, float p, float i, float d, int flag) {
pid->p = p;
pid->i = i;
pid->d = d;
if(flag){
; /* Whatever */
}
return pthread_create(&pid->tid, 0, pid_task, pid);
}
int pid_stop(struct pid* pid, int cancel) {
int rc = 0;
if(cancel)
pthread_cancel(pid->tid);
rc = pthread_join(pid->tid, 0);
pid->tid = 0;
return rc;
}
int main(int argc, char** argv) {
pid_start(&PID_A, .5, .3, .1, 0);
pid_start(&PID_B, .2, 1.2, .3, 1);
pid_stop(&PID_A, 0);
pid_stop(&PID_B, 0);
printf("Snd start:\n");
pid_start(&PID_B, .2, 1.2, .3, 1);
pid_start(&PID_A, .5, .3, .1, 0);
pid_stop(&PID_A, 1);
pid_stop(&PID_B, 0);
return 0;
}
und wie geht es, wenn die Klasse über
class PID;
PID.start()
selber ihren eigenen privaten pthread Task starten möchte?
(hat sich überschnitten, ich bezog mich auf mxt's code)
- - - Aktualisiert - - -
über botty's Variante muss ich nochmal etwas nachdenken...
- - - Aktualisiert - - -
@botty:
deinen Vorschlag davor
https://www.roboternetz.de/community/threads/69688-pthread-Task-als-Methode-einer-C-Klasse?p=631429&viewfull=1#post631429
hatte ich völlig überlesen - vielen Dank!
hat sich auch wohl mit einem anderen zeitlich überschnitten.
Ich blicke auch dabei noch nicht völlig durch, aber ich versuche gerade, mich da hineinzudenken... :)
So hier noch mal ein Beispiel, auf Raspian getestet. Die returns sind doch nötig.
#include <stdio.h>
#include <pthread.h>
class Pid
{
private:
pthread_t m_thread;
int m_a; // Nur Beispielparameter, hier kämen die richtigen Parameter des Reglers hin
int m_b;
static void* helper(void* instance)
{
return ((Pid*) instance)->run();
}
void* run()
{
printf("Die Parameter sind %d %d\n", m_a, m_b);
// Hier käme die Regelschleife rein
return 0;
}
public:
Pid(int a, int b) : m_a(a), m_b(b)
{}
void start()
{
pthread_create(&m_thread, NULL, &helper, this);
}
void stop()
{
pthread_join(m_thread, 0);
}
};
int main()
{
Pid test(10, 42);
test.start();
// ...
test.stop();
return 0;
}
Die eigentliche Regelung fände in der Funktion run statt. Man beachte, dass diese und die statische helper Funktion privat sind, also von außen nicht aufgerufen werden dürfen.
Die Parameter würden im Konstruktor gesetzt. Kann man aber auch über Parameter in start lösen.
Ich würde aber trotzdem empfehlen, entweder die Lösung von botty oder richtige C++ Threads zu nehmen.
Oder nochmal in C++11
#include <iostream>
#include <thread>
using namespace std;
typedef struct {
pthread_t tid;
char* name;
float p;
float i;
float d;
} PID_t;
class TestClass
{
public:
TestClass() {}
~TestClass() {}
void Start(PID_t *p)
{
t = new thread(&this->MyTask, p);
}
void Stop()
{
t->join();
}
private:
static void MyTask(PID_t *p)
{
cout << "PID: " << p->tid << endl;
cout << "Name: " << p->name << endl;
}
thread *t;
};
int main()
{
cout << "Start" << endl;
PID_t pp[3] = { {0, (char*)"MyName1", 5, 6, 7},
{1, (char*)"MyName2", 5, 6, 7},
{2, (char*)"MyName3", 5, 6, 7}};
TestClass tc1;
TestClass tc2;
TestClass tc3;
tc1.Start(&pp[0]);
tc2.Start(&pp[1]);
tc3.Start(&pp[2]);
//...
tc1.Stop();
tc2.Stop();
tc3.Stop();
return 0;
}
Danke für alle Tipps!
bin gerade ein paar Tage außer Haus, daher kann ich es z.Zt nicht testen.
Für MT kommen keine C++11 tasks in Frage, wie ich bereits schrieb, weil ich mich nicht damit auskenne - sondern nur pthread, das ich auch sonst verwende.
Bottys Version ganz ohne ++ hatte ich ja zunächst überlesen, das will ich mal als nächstes versuchen umzusetzen, wenn ich hier wieder einen Pi zum programmieren habe. Ich melde mich dann baldmöglichst!
hier erst einmal - nochmal - vielen Dank!
hallo,
2 Dinge sind mir mit der Struktur noch unklar:
warum muss am Anfang immer eine Null stehen
struct PID pids[] = {
{0, "Task A", 0.015, 0.2, 0.7 },
{0, "Task B", 0.3, 0.5, 0.80 }
};
und 2., ist es erlaubt, die Struktur statt mit einem String auch mit einem char (0, 1, 2, 3, ...) zu definieren und aufzurufen oder muss es unbedingt ein String sein?
struct PID pids[] = {
{0, 0, 0.015, 0.2, 0.7 },
{0, 1, 0.3, 0.5, 0.80 }
};
:?:
@botty:
je mehr ich drüber nachdenke, wird mir allerdings klar, dass ich gar nicht recht verstehe, wofür diese Struktur
https://www.roboternetz.de/community/threads/69688-pthread-Task-als-Methode-einer-C-Klasse?p=631429&viewfull=1#post631429
struct PID pids[] = {
{0, "Task A", 0.015, 0.2, 0.7 },
{0, "Task B", 0.3, 0.5, 0.80 }
};
überhaupt da ist:
Das "setten" der Kp, Ki, Kd (kurz: P, I, D) soll ja nicht im Aufruf der PID-Funktion stehen.
Die PID Funktion wird nur mit der Motornummer und dem RotationTarget aufgerufen plus einem pwm-Wert für die max. Rotationsgeschwindigkeit
void RotatePIDtoTarget (char port, long Target, float RotatSpeed); // approach absolute target once
void RotatePIDdegrees (char port, long Target, float RotatSpeed); // turn relative degrees
void RotatePIDcontinue (char port, long Target, float RotatSpeed); // approach target continuously
void StopPIDcontrol (char port); // stop PID
Das Setten der PID-Konstanten geschieht automatisch bei der Initialisiering und kann dann auf Wunsch auch optional durch extra Funktionen geschehen:
// first Initialization:
void PIDinit(); // P=0.4, I=0.4, D=10.0
//
// b) simple customized PID setting:
void SetPIDparam(char port, float P,float I,float D);
//
// c) extended customized parameter setting:
void SetPIDparamEx(char port, float P,float I,float D, float prec,int rtime, float damp);
wozu also deine
struct PID pids[]
:?:
Hallo HaWe,
sorry das ich mich jetzt erst melde aber bei dem schönen Wetter konnte mich am WE nichts vor den Bildschirm locken.
Zu Deinen Fragen:
Meine "struct pid" sollte nur ein Auszug aus Deiner "struct PIDstruct" sein. Worauf es mir ankommt ist, dass du diese um zwei Felder erweiterst:
einmal um ein Element pthread_t und
zweitens, um ein Element worüber du deine Motoren identifizieren kannst.
Dadurch kannst du deinen Code wesentlich verkürzen, schau mal deine
safecall void StopPIDcontrol (char port) {
if (port==OUT_A) {
if (PID_A.runstate!=0) { // stop PID task if already running
stop task_PID_A;
PlayTone(TONE_C7,100);
Off(OUT_A);
Wait(50);
PID_A.cont=false;
PID_A.runstate=0;
Wait(1);
return;
}
}
else
if (port==OUT_B) {
if (PID_B.runstate!=0) { // stop PID task if already running
stop task_PID_B;
PlayTone(TONE_C7,100);
Off(OUT_B);
Wait(50);
PID_B.cont=false;
PID_B.runstate=0;
Wait(1);
return;
}
}
else
if (port==OUT_C) {
if (PID_C.runstate!=0) { // stop PID task if already running
stop task_PID_C;
PlayTone(TONE_C7,100);
Off(OUT_C);
Wait(50);
PID_C.cont=false;
PID_C.runstate=0;
Wait(1);
return;
}
}
}
an.
Da steht dreimal quasi das Gleiche.
Wenn du jetzt deine Struktur erweiterst und statt dem Port einen Zeiger auf die Variable übergibst, läßt sich der Code um 2/3 kürzen.
struct PIDstruct {
/* Erweiterung */
pthread_t tid;
unsigned short motor_id;
// custom target values
long target; // set target
int tarpwm; // motor target speed
// custom regulation parameters
float P; // P: basic propotional to error
float I; // I: integral: avoid perish
float D; // D: derivative: avoid oscillating
float precis; // error precision to target
int regtime; // PID loop time
float damp; // damp the integral memory
char cont; // target: continue or hit once
// internal control variables
char runstate; // monitors runstate
int outp; // PID control output value
int maxout; // max output (max motor pwr)
long read; // current sensor reading
float err; // current error
float integr; // integral of errors
float speed; // current speed
} ;
void StopPIDcontrol (struct PIDstruct *pid) {
if (pid->tid) { // stop PID task if already running
pid_stop(pid, 1); // siehe botties zweites Beispiel
Off(pid->motor_id);
pid->cont=false;
pid->runstate=0;
return;
}
}
Wenn du jetzt eine Variable PID_A definierst, würde der Aufruf so aussehen:
struct PIDstruct PID_A;
void eine_stop_funktion(void) {
StopPIDcontrol( &PID_A );
}
Du wählst somit nicht mehr über eine port/switch Kombination deinen PID Controler aus, sondern beim Aufruf der Funktionen über den Zeiger auf die Variable, welche alle Daten für einen Controler enthält.
D.h. bei allen Funktionen ersetzt du "char port" durch "struct PIDstruct* pid" und dereferenzierst die Elemente der Struktur nicht mehr über den "." Operator sondern über den "->" Operator.
Auch bei der Erezugung der Threads können wir jetzt den Zeiger auf die Variable als Parameter in die Thread-Funktion hineingeben, denn die Signatur dieser Funktion lautet:
void *(*function)(void *arg);
Zwar ist arg vom Typ "void *" nur da wir immer nur Zeiger vom Typ "struct PIDcontrol *" hineingeben können wir am Anfang der Routine diesen Zeiger sicher auf den richtigen Typ casten:
void *pid_calc(void *arg) {
float aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
char regloop;
/* das ist ein ziemlich sicherer cast */
struct PIDcontrol *pid = (struct PIDcontrol *) arg;
pid->runstate = 0x10; // reg state: RAMPUP
pid->read = (MotorRotationCount(pid->motor_id)); // get current encoder reading
pid->err = pid->target - pid->read; // error to target
readstart = pid->read;
/* ... */
}
so brauchst du auch diese Routine nur einmal, denn bei der Thread Erzeugung übergibst du den Zeiger auf die Variable z.B.:
pthread_create(&PID_A.tid, 0, pid_calc, &PID_A);
ich hoffe das es jetzt klarer ist, warum ich Zeiger verwenden würde?
Noch ein Wort zu den Variablen für die einzelnen PID Controler, du definierst die so:
struct PIDcontrol PID_A;
struct PIDcontrol PID_B;
struct PIDcontrol PID_C;
bei sowas sehe ich sofort eine Aufzählung und wenn ich dann auch noch Operationen über diese machen muss, wie z.B. ein warten auf das Beenden eines Threads, dann ordne ich sowas in einem Array an:
enum motoren {
MOT_SCHULTER = 0,
MOT_ELLE,
MOT_HAND,
MOT_MAX
};
struct PIDcontrol pids [MOT_MAX];
/* Verwendung */
void ein_stop(void) {
StopPIDcontrol( &pids[MOT_SCHULTER );
}
void mehrere_stops(void) {
int idx;
for(idx = MOT_SCHULTER; idx < MOT_MAX; idx++)
StopPIDcontrol( &pids[idx] );
}
find ich bequemer, ist aber Geschmackssache.
Gruss
botty
vielen Dank, dass ich mehere Einzelfunktionen einsparen will ist kar, das war ja meine Absicht, dennoch sehe ich jetzt schon ein wenig klarer.
Dennoch ist das mit den doppelten Pointern un dem "casting" schon seeehr kompliziert...
Außerdem ist mir nicht klar, wie jetzt genau die Verknüpfung erfolgen muss von der aufrufenden simplen API Funktion bis hin zum Task:
der vereinfachende Wrapper
inline void RotatePIDtoTarget(char port, long Target, float RotatSpeed) {
RotatePID(port, Target, RotatSpeed, false);
Wait(1);
}
ruft auf die ein wenig komplexere Interface-Funktion inkl. dem Target-Haltemodus:
RotatePID(port, Target, RotatSpeed, false);
und hier wird nach Setzen der PID Laufzeit-Variablen der PID Task gestartet:
safecall void RotatePID(char port, long Target, float RotatSpeed, char cont) {
if (port==OUT_A) {
//...
//...
PID_A.runstate=1; // set runstate: PID active
// custom init PID_A
PID_A.target =Target; // assign target
PID_A.tarpwm =RotatSpeed; // assign rotation speed
PID_A.cont=cont; // cont vs. hit once
// Reset PID control defaults
PID_A.outp =0; // PID control output value
PID_A.maxout =100; // absolute max possible output (max pwr)
PID_A.read =0; // current reading
PID_A.err =0; // current error
PID_A.integr =0; // integral of errors
PID_A.speed =0; // current speed
start task_PID_A;
}
else
if (port==OUT_B) {
//...
statt der if-Schleifen für jeden Motor käme jetzt dein expliziter PID-Aufruf mit Motornummer als Parameter -
wie würde es aber denn heißen müssen, wenn ich den PID pthread task innerhalb deiner neuen Struktur starten will?
Na ja,
zuerst must du die PIDinit ergänzen:
void PIDinit(void) {
PID_A.tid = 0;
PID_A.motor_id = OUT_A;
// für alle Variablen und dann natürlich die anderen Elemente der Struktur.
}
Die Signaturen deiner High-Level Funktionen ändern sich nur im ersten Element, die anderen Parameter kannst du 1:1 übernehmen z.B.:
inline void RotatePIDtoTarget(struct PIDstruct *pid, long Target, float RotatSpeed) {
RotatePID(pid, Target, RotatSpeed, false);
}
wobei du festlegen musst ob evtl. Fehlercodes wieder hochgereicht werden? Hier spar ich mir das mal.
Deine zentrale RotatePID sähe dann so aus:
void RotatePID(struct PIDstruct *pid, long Target, float RotatSpeed, char cont) {
// Nullzeiger?
if( ! pid ) {
fprintf(stderr, "RotatePID Null Zeiger!\n");
return;
}
// Laeuft der PID in einem Thread bereits?
if( pid->tid != 0 ) {
fprintf(stderr, "RotatePID - PID schon gestartet!\n");
return;
}
// Koennte weg
PID_A.runstate=1; // set runstate: PID active
// custom init pid
pid->target =Target; // assign target
pid->tarpwm =RotatSpeed; // assign rotation speed
pid->cont=cont; // cont vs. hit once
// Reset PID control defaults
pid->outp =0; // PID control output value
pid->maxout =100; // absolute max possible output (max pwr)
pid->read =0; // current reading
pid->err =0; // current error
pid->integr =0; // integral of errors
pid->speed =0; // current speed
// Thread starten
if(pthread_create(&pid->tid, 0, pid_calc, pid)) {
perror("RotatePID");
pid->tid = 0;
return;
}
}
Stoppen ginge dann so
void StopPIDcontrol (struct PIDstruct *pid, int cancel) {
if (pid->tid) { // stop PID task if already running
Off(pid->motor_id);
if(cancel)
pthread_cancel(pid->tid);
pthread_join(pid->tid, 0);
pid->tid = 0; // wichtig!
pid->cont=false;
pid->runstate=0;
}
}
Die verwendung sähe dann etwa so aus:
struct PIDstruct PID_A;
int main(int argc, char** argv) {
PIDinit();
// Wartend
RotatePIDtoTarget(&PID_A, 25, 0.4); // Sinvolle Werte kennst du besser als ich ;)
StopPIDcontrol(&PID_A, 0);
// Abbrechend
RotatePIDtoTarget(&PID_A, 35, 0.3);
sleep(2);
StopPIDcontrol(&PID_A, 1);
return 0;
}
Ich habe diesen Code nicht durchkompiliert, hoffe aber das er trotzdem deine Fragen klärt.
Noch ein Wort zur pid_calc Funktion: In ihr solltest du alle paar Schritte, insbesondere in Schleifen, pthread_testcancel() aufrufen, damit ein cancel auch wirklich stattfinden kann.
Gruss
botty
hallo botty,
ganz herzlichen Dank für deine viele Mühe und deine ausführlichen Codebeispiele. Leider verstehe ich im Moment doch noch gar nichts über die ganzen neuen kryptischen Funktionen, Aufrufe und Konstruktionen, insb. noch nicht einmal, warum ich -> statt . verwenden muss.
Tatsächlich möchte ich allerdings im Aufruf auch nur eine Motornummer übergeben (0,1,2,3 oder per
#define OUT_A 0
#define OUT_B 1
etc.
und keine PID-Struktur.
Der Sinn ist, dass meine neue C Syntax an NXC-API Befehle angelehnt sein soll, denn das Ziel ist es auch, Lego Roboter für bisherige Lego NXT und NXC Nutzer mit einer Raspi API zu programmieren und zu betreiben, was bisher nicht möglich ist bei dem völlig fehlkonstruierten neuen Lego EV3-SoC: also quasi als Konkurrenz-Gegenentwurf ;) . Ich habe auch schon mit so einem Gegenprojekt angefangen, PID wäre der nächste Schritt hierfür: http://www.mindstormsforum.de/viewtopic.php?f=78&t=8851
NXC (bzw. Vorgänger NQC) ist den Lego Nutzern seit fast 20 Jahren bekannt, und hier werden ebenfalls Motor Steuerbefehle verwendet wie
motorcommand ( motorNr, rotationDegrees, pwm)
Ich bin für deine neuen Konstrukte aber einfach noch zusehr auf dem absoluten C Anfängerlevel, als dass ich das jetzt wirklich umsetzen könnte. Ich fürchte im Moment, dass meine 3 -10 Einzel Tasks wie bei NXC doch die einzige Möglichkeit sind, das ganze PID Projekt auf dem Raspi für mich zu verwirklichen.
Vlt könntest du mir aber doch quasi als minimal kleinst-möglichen Lernschritt erklären, warum ich hier oft -> statt . verwenden muss?
Bei Arduino und auch sonst nutzt man doch auch den . z.B.
Serial.print()
Serial.available()
Wire.begin()
Und Variablenwerte in Strukturen meine ich auch schon mit Punkt gesehen zu haben
mystruc.fvar=1.234;
Wenn du mit Zeigern arbeitest musst du dir im Klaren darüber sein, dass es immer zwei Speicherbereiche zu unterscheiden gibt. Der eine ist der Speicherbereich in dem die eigentlichen Daten stehen, die andere Speicherzelle ist der Zeiger, dessen Wert die Adresse des anderen enthält.
Wenn du jetzt eine Variable von einem einfachen Basistypen wie int, char oder double etc. hast und über den Zeiger den Wert ändern willst, muss der Compiler unterscheiden können ob du den Wert des Zeigers oder den Wert auf den den er zeigt ändern möchtest.
int aval = 5;
int main(void) {
int *ptr;
ptr = &aval;
printf("Der Wert an der Adresse %p ist %d\n", ptr, *ptr);
*ptr = 7;
printf("Der Wert an der Adresse %p ist %d\n", ptr, *ptr);
return 0;
}
Für einfache Typen ist das schlicht. "ptr = &aval" weisst dem Zeiger die Adresse von aval zu, während " *ptr = 7 " den Zeiger dereferenziert und in den Speicher dieser Adresse den Wert 7 hineinschreibt. Zwei gänzlich unterschiedliche Dinge.
Bei Strukturen kommt jetzt hinzu, dass der Compiler Offsets für die Elemente der Struktur berechnen muss, damit er den korrekten Speicherbereich liesst bzw. beschreibt.
Bei einer Variablen weiss der Compiler zum Übersetzungszeitpunkt wo der Start der Variablen ist und kann diese Offsets absolut oder relativ berechnen.
#include <stdio.h>
struct vec2d {
double x, y;
};
struct vec2d ein_vec;
int main(void) {
ein_vec.x = 3.0;
ein_vec.y = 7.0;
printf("Adressen von ein_vec=%p, ein_vec.x=%p, ein_vec.y%p\n", &ein_vec, &ein_vec.x, &ein_vec.y);
return 0;
}
Verwende ich einen Zeiger auf eine Variable einer Struktur, dann kennt der Compiler diesen Startpunkt nicht und ich brauche einen Mechanismus ihm diesen mitteilen zu können. Der "->" sagt also aus: in der Zeigervariable findest du die Startadresse, nimm die und berechne von da an. Während bei Variablen der Compiler die Verantwortung für den Start hat, habe ich als Programmierer die Verantwortung bei Zeigern den richtigen Startpunkt dem Compiler mitzuteilen.
#include <stdio.h>
struct vec2d {
double x, y;
};
struct vec2d ein_vec;
int main(void) {
struct vec2d *vptr;
vptr = &ein_vec;
vptr->x = 3.0;
vptr->y = 7.0;
printf("Adressen von vptr=%p, ein_vec=%p\n", vptr, &ein_vec);
/* Fehler gibt SEGFAULT */
vptr = 0;
vptr->x = 3.0;
vptr->y = 7.0;
return 0;
}
An die Adresse 0x00000000 darf man nichts schreiben.
Falls dir die Erklärungen nicht so ganz einleuchten, solltest du dann vielleicht doch nochmal in ein gutes C-Buch reinschauen und dort die Erklärungen über Zeiger durchgehen. Mir fehlt im Erklären die Übung. Nichts desto trotzt sind Zeiger ein wichtiges und mächtiges Werkzeug in der C-Programmierung.
Warum ich die überhaupt benutzen wollte hatte einen bestimmten Grund:
Bei Threads muss man immer aufpassen, dass man sich keine Wettbewerbsbedingungen einfängt, sprich ein Thread in Daten eines anderen hineinschreibt. Obwohl ich erstmal globale Variablen als Speicherort gewählt hatte, werden die einzelnen PIDs dadurch isoliert, dass in den Threads immer nur auf eine Variable über den zugehörigen Zeiger zugegriffen wird (das wäre selbst so, wenn ich alle in einem Feld anordnen würde.). Diese Zeiger liegen auf dem Thread lokalem Stack, sind nach Aussen nicht sichtbar.
Das war die Idee.
Gruss
botty
super, ganz tolle Erklärung, habe jetzt eine Menge Neues kapiert. Ganz herzlichen Dank ! 8)
@botty:
habe mir jetzt die letzten Tage deinen Vorschlag noch ein paar mal genauer angesehen und überdacht, etwas herumprobiert und die bereits in meiner "LegoPi" bestehende Raspi- Motorstuktur durch die PID-Parameter aufgebohrt:
//************************************************** ***********
// 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;
int16_t motorID;
// 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
int16_t regtime; // PID loop time
double damp; // damp the integral memory
int8_t cont; // target: continue or hit once
// PID 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
} tEncMotor;
tEncMotor motor[MAXMOTORS];
3 Fragen dazu:
1) da ich jetzt alles statisch in einem Array deklariert habe: käme man jetzt überwiegend ohne die Pointerpfeile -> aus ?
2) da jede einzelne Motorstruktur über den Platz im Array definiert ist (motor[0] für motorID=0) : braucht man dann noch die Strukturvariable motorID?
3) kann man eine zusätzliche Variable im pthread-Aufruf übergeben, indem man die Motornummer mit übergibt?
bisher läuft mein pthread Aufruf immer per
pthread_create(&threadID, NULL, threadname, NULL);
wofür man die 2 NULLs braucht, ist mir z.B. auch noch unklar, vlt kann man die ein oder andere verwenden - und würde das die Sache einfacher machen?
- - - Aktualisiert - - -
ps,
der API Aufruf
RotatePID(port, Target, RotatSpeed, false);
würde dann die folgende Funktion aufrufen, die wieder nach Setten der Variablen einen pthread task starten müsste..... (????) :
void RotatePID(char port, long Target, float RotatSpeed, char cont) {
motor[port].runstate=1; // set runstate: PID active
// custom init PID [port]
motor[port].target =Target; // assign target
motor[port].tarpwm =RotatSpeed; // assign rotation speed
motor[port].cont=cont; // cont vs. hit once
// Reset PID control defaults
motor[port].outp =0; // PID control output value
motor[port].maxout =100; // absolute max possible output (max pwr)
motor[port].read =0; // current reading
motor[port].err =0; // current error
motor[port].integr =0; // integral of errors
motor[port].speed =0; // current speed
// <<<<<<
// nur wie wäre das hier dann richtig zu implementieren ????
// threadname ist ja für jeden Motor immer ein anderer ????
// wäre hierfür eine tEncMotor Hilfsvariable char[12] tEncMotor.thrnamebuf hilfreich ????
// ("PIDthread00"... "PIDthread99", könnte man beim Initialisieren setten ????)
// aber schreiben will ich ja für alle nur 1x ein einziges PID-Funktionstask-Muster ????
// >>>>>>
pthread_create(& motor[port].tid, NULL??, motor[port].thrnamebuf, NULL??);
}
Hi,
die erste Frage hast du dir schon selbst beantwortet. Den Dereferenzierungsoperator "->" brauchst du nicht mehr, wenn du mit dem Array von Strukturen arbeitest. Du musst nur den Index für einen Motor als Parameter in die Thread-Main bekommen, indem deine PID-Kalkulationen sind.
Zu deiner zweiten Frage: das Element kannst du weglassen denn wir werden diese id als index übergeben.
Zu deiner letzten Frage:
schau mal die Signatur der pthread_create genau an (mit "$> man pthread_create" in 'nem Terminal unter Linux gibt's die Erklärungsseite zu sehen):
int pthread_create(pthread_t *thread,
const pthread_attr_t *attr,
void *(*start_routine) (void *),
void *arg);
Parameter eins ist ein Zeiger auf das pthread Handle. So wie in deinem RotatePID-Beispiel passt das schon mal.
Parameter zwei ist ein unveränderlicher Zeiger auf eine Attributstruktur. Es gibt paar, das wichtigste ist das deine Threads mit pthread_cancel sich unterbrechen liessen. Da das ein Default-Wert ist und die anderen Attribute erstmal nicht wichtig sind, kannst du hier eine 0 oder NULL reingeben, was heist, das die Defaults verwendet werden.
Parameter drei ist jetzt der Zeiger auf die Funktion, welche die Thread-Main sein soll. In deinem Fall gibst du da einfach den Namen der PID-Kalkulationsroutine an (die ist ja wohl für alle gleich?).
Parameter vier ist ein Zeiger ohne konkreten Datentypen, also void. Da Zeiger in C immer vorzeichenlose, integrale Datentypen sind können wir hier ausnahmsweise den Wert des Zeigers als unsigned int für den Index in das Array missbrauchen und so sicherstellen, das ein Thread immer nur auf ein bestimmtest Element des Arrays zurgreifen wird.
Der Erzeugungsaufruf in RotatePID sähe also so aus:
pthread_create( &motor[port].tid, // id speichern in dem dazugehörigen Array-Element
0, // Default Attribute (threads cancelbar)
pid_calc, // Name der PID-Kalkulationsroutinge
(void*)port); // der Index des Array-Elements für eine PID Struktur, die mit dem Motorindex gleich ist.
// Der cast ist notwendig, sonst meckert der Compiler.
in der "pid_calc" müssen wir dann den Wert des Zeigers einmal am Anfang in einen "unsigned int" casten und diese Variable verwendest du bei den Arrayzugriffen und in deinen Unterfunktionen als Wert für den port Parameter.
Die Signatur ist bindend!:
void *pid_calc(void *arg) {
// Was immer du für Variablen brauchst
// Zeigerwert in Arrayindex wandeln.
unsigned port = (unsigned)arg;
// Nur als sinnloses Bsp.
motor[port].P = 0.4;
// Was immer du rechnen magst.
pthread_exit(0);
}
Wenn du einen anderen Namen verwenden willst, musst du nur darauf achten das die Routine "void*" als Rückgabewert hat und es genau einen Parameter gibt mit "void*" als Typ.
void* welcher_name_auch_immer(void *arg) { ... }
Das war's.
Gruss
botty
P.S. leider sind die Formatierungen in den code-Tags verrutscht, hoffe es ist trotzdem lesbar.
ok, noch immer ein paar Verständnisprobleme:
zu pid_calc....:
bisher verwende ich ja diese Task-Funktion hier (in 3 Varianten, ab jetzt natürlich einheitlich 1x für alle):
(ich HASSE ES, dass hier nie Codetags im Antwort-Editor dabei sind!!! :( )
task task_PID_A() {
float aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
char regloop;
PID_A.runstate = 0x10; // reg state: RAMPUP
PID_A.read = (MotorRotationCount(OUT_A)); // get current encoder reading
PID_A.err = PID_A.target - PID_A.read; // error to target
readstart = PID_A.read;
regloop = 1;
#ifdef debug_PID_A
//.................................................. ..........................
// init variables for graph output
ClearScreen();
DisplayMask();
int timex, oldtx, oldy=15, pwm0=15; // values for graphic screen
float scrXratio, scrYratio;
scrXratio=abs(PID_A.err)/8;
if(PID_A.target<50) scrXratio=abs(PID_A.err)/4;
scrYratio=abs(PID_A.err)/40;
//.................................................. ..........................
#endif
damp=0; // damp the integral memory
starttime= CurrentTick();
// appoach target
_Astart:
PID_A.runstate = 0x10; // run state: RUNNING
do {
dtime = CurrentTick() - clock;
clock = CurrentTick();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((PID_A.err==errorold)&& (abs(PID_A.err)>PID_A.precis)) damp=1; // stalling
else
damp=PID_A.damp;
PID_A.integr = (damp * PID_A.integr) + PID_A.err;
if((PID_A.integr) > 3*PID_A.maxout) PID_A.integr = 3*PID_A.maxout; // cut away
else
if((PID_A.integr) <-3*PID_A.maxout) PID_A.integr =-3*PID_A.maxout;
PWMpwr= (PID_A.P*PID_A.err) + (PID_A.I*PID_A.integr)*tprop + (PID_A.D*(PID_A.err-errorold))/tprop;
if(PWMpwr > PID_A.maxout) PWMpwr= PID_A.maxout; // forward maxout
else
if(PWMpwr < -PID_A.maxout) PWMpwr= -PID_A.maxout; // reverse maxout
PID_A.speed= (PID_A.read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(PID_A.speed) ;
if (aspeed > PID_A.tarpwm) {
PWMpwr = sign(PWMpwr)*PID_A.tarpwm;
}
PID_A.outp = round(PWMpwr);
#ifdef debug_PID_A
//.................................................. ........................
// for graph output
timex= runtime/scrXratio;
PointOut(timex,(PID_A.read-readstart)/scrYratio);
LineOut(oldtx, oldy, timex, pwm0+PID_A.speed*0.3);
oldtx=timex; oldy=pwm0+PID_A.speed*0.3;
//.................................................. ........................
#endif
//************************************************** ************************
// PID regulation !
OnFwd(OUT_A, (PID_A.outp)); // action !
Wait(PID_A.regtime); // wait regulation time
//************************************************** ************************
readold = PID_A.read; // save old sensor
errorold = PID_A.err; // save old error
PID_A.read = (MotorRotationCount(OUT_A)); // get new encoder value
PID_A.err = PID_A.target-PID_A.read; // new error to target
if (PID_A.read>cmax) cmax=PID_A.read; // monitor overshooting
else
if (PID_A.read<cmin) cmin=PID_A.read; // monitor overshooting
if ((PID_A.cont)&& (abs(PID_A.err)<=PID_A.precis)) PID_A.runstate = 0x60;
else PID_A.runstate = 0x20;
#ifdef debug_PID_A
//.................................................. ........................
printf1(68, 48,"%-5d" , cmax);
printf1(68, 40,"%-5d" , cmin);
printf1(68, 32,"%-5d" , PID_A.read);
//.................................................. ........................
#endif
if (PID_A.cont) continue;
if (abs(PID_A.err)<=PID_A.precis) { regloop +=1 ; PID_A.runstate = 0x40; }
} while ((abs(PID_A.err)>=PID_A.precis) && (regloop<=5)); // target reached
Off(OUT_A); // finished - brake motor
PID_A.runstate = 0x40; // run state: RAMPDOWN
PID_A.outp=0;
Wait(50);
PID_A.read = MotorRotationCount(OUT_A);
regloop=1;
if (PID_A.read>cmax) cmax=PID_A.read; // detect overshooting
if (PID_A.read<cmin) cmin=PID_A.read; // detect overshooting
PID_A.err = PID_A.target-PID_A.read;
#ifdef debug_PID_A
//.................................................. ..........................
printf1(68, 56,"%-5d" , PID_A.target);
printf1(68, 48,"%-5d" , cmax);
printf1(68, 40,"%-5d" , cmin);
printf1(68, 32,"%-5d" , PID_A.read);
printf1(56, 24,"P %5.2f" , PID_A.P);
printf1(56, 16,"I %5.2f" , PID_A.I);
printf1(56, 8,"D %5.2f" , PID_A.D);
//.................................................. ..........................
#endif
if ((abs(PID_A.err)>PID_A.precis)) {goto _Astart;}
#ifdef debug_PID_A
//.................................................. ..........................
PointOut(timex,PID_A.read/scrYratio);
LineOut(oldtx, oldy, timex, pwm0);
LineOut(timex+2,PID_A.target/scrYratio, timex+10, PID_A.target/scrYratio);
LineOut(timex+2, pwm0, timex+10, pwm0);
//.................................................. ..........................
#endif
PID_A.runstate=0;
Wait(1); //runstate = IDLE
}
//================================================== ============================
a) statt
task task_PID_A()
stünde dann hier also
void * pid_calc( void *)
:?:
und der Rest bliebe gleich?
(die ..._A am Ende fallen ntl dann auch überall weg)
und b)
auch wenn ich jetzt kurz hinterenander mehrfach diesen Task für mehrere Motoren starte, wobei die Laufzeiten sich mehrfach überlappen
RotatePID(0, 3000, 15, false);
RotatePID(2, -4000, 90, false);
delay(1);
RotatePID(8, 10000, 50, true );
delay(10);
RotatePID(1, -5000, 60, false);
RotatePID(3, -3000, 20, true);
und also in diesem Falle 5 verschiedene Threads parallel asynchron laufen müssten, mit Starten eines neuen Tasks ohne auf die Beendigung des vorherigen warten zu müssen, die sich dann alle auch unabhängig zu verschiedenen Zeiten beenden oder (in 2 Fällen) "unendlich" weiter laufen -
das funktioniert dann :?:
zu a)
ja, task_PID_[ABC] wird durch die allgemeine form pid_calc ersetzt.
Sinngemäss wie du auf die Elemente des Arrays zugreifst sähe so aus (ohne das ich jetzt verstehe was dein Algo macht):
void *pid_calc(void *arg) {
float aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
char regloop;
// arg nach index casten
unsigned port = (unsigned)arg;
motor[port].runstate = 0x10; // reg state: RAMPUP
motor[port].read = (MotorRotationCount(port)); // get current encoder reading
motor[port].err = motor[port].target - motor[port].read; // error to target
readstart = motor[port].read;
regloop = 1;
damp=0; // damp the integral memory
starttime= CurrentTick();
// appoach target
_Astart:
motor[port].runstate = 0x10; // run state: RUNNING
do {
pthread_testcancel();
dtime = CurrentTick() - clock;
clock = CurrentTick();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((motor[port].err==errorold)&& (abs(motor[port].err)>motor[port].precis))
damp=1; // stalling
else
damp=motor[port].damp;
motor[port].integr = (damp * motor[port].integr) + motor[port].err;
if((motor[port].integr) > 3*motor[port].maxout)
motor[port].integr = 3*motor[port].maxout; // cut away
else if((motor[port].integr) <-3*motor[port].maxout)
motor[port].integr =-3*motor[port].maxout;
PWMpwr= (motor[port].P*motor[port].err) + (motor[port].I*motor[port].integr)*tprop + (motor[port].D*(motor[port].err-errorold))/tprop;
if(PWMpwr > motor[port].maxout)
PWMpwr= motor[port].maxout; // forward maxout
else if(PWMpwr < -motor[port].maxout)
PWMpwr= -motor[port].maxout; // reverse maxout
motor[port].speed= (motor[port].read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(motor[port].speed) ;
if (aspeed > motor[port].tarpwm) {
PWMpwr = sign(PWMpwr)*motor[port].tarpwm;
}
motor[port].outp = round(PWMpwr);
//************************************************** ************************
// PID regulation !
OnFwd(port, (motor[port].outp)); // action !
Wait(motor[port].regtime); // wait regulation time
//************************************************** ************************
readold = motor[port].read; // save old sensor
errorold = motor[port].err; // save old error
motor[port].read = (MotorRotationCount(port)); // get new encoder value
motor[port].err = motor[port].target-motor[port].read; // new error to target
if (motor[port].read>cmax)
cmax=motor[port].read; // monitor overshooting
else if (motor[port].read<cmin)
cmin=motor[port].read; // monitor overshooting
if ((motor[port].cont)&& (abs(motor[port].err)<=motor[port].precis))
motor[port].runstate = 0x60;
else
motor[port].runstate = 0x20;
if (motor[port].cont)
continue;
if (abs(motor[port].err)<=motor[port].precis) {
regloop +=1 ;
motor[port].runstate = 0x40;
}
} while ((abs(motor[port].err)>=motor[port].precis) && (regloop<=5)); // target reached
Off(port); // finished - brake motor
motor[port].runstate = 0x40; // run state: RAMPDOWN
motor[port].outp=0;
Wait(50);
motor[port].read = MotorRotationCount(port);
regloop=1;
if (motor[port].read>cmax) cmax=motor[port].read; // detect overshooting
if (motor[port].read<cmin) cmin=motor[port].read; // detect overshooting
motor[port].err = motor[port].target-motor[port].read;
if ((abs(motor[port].err)>motor[port].precis)) {
goto _Astart;
}
motor[port].runstate=0;
Wait(1); //runstate = IDLE
}
zu b)
klar geht das, in deinem Beispiel legst du einfach fünf Threads an, die vom OS ausgeführt werden.
Was du nicht machen darfst ist zweimal ein RotatePID mit der identischen Portnummer aufzurufen ohne vorher ein StopPIDcontrol aufzurufen. Würdest du das machen hättest du zwei Threads die dann doch wieder auf einem PID rumrechnen.
Ausserdem würde ich statt Nummern für die Ports eine enum erstellen, Namen sind einfacher zu lesen, als mit 0,4,7 einen Motor zu assoziieren.
toll, danke, auch für deine Mühe mit dem ganzen Code ;)
das war auch bisher der Punkt, den ich nicht verstanden hatte mit der Vielzahl der pthread-Tasks:
Ich dachte immer, um 2 (3,4,5) tasks unabhängig zu starten, müsse man ihnen außer eigene pthreadIDs auch 2 (3,4,5) verschiedene eindeutige unterscheidbare Namen geben
- dass es ausreicht, einen identischen Namen zu verwenden und zusätzlich zur eindeutigen pthreadID nur noch ein 4.Parameter damit sie verschiedene Variablen ansprechen, war mir nicht klar.
Es scheint also vor allem der eindeutige Handle (die pthread-ID im 1. Parameter) zu sein, die den task auf einer eigenen unabhängigen Timeslice starten lässt.
Perfekt.
Wenn ich das jetzt richtig verstanden habe, dann wird es funktionieren.
Tausend Dank, ich setz mich dran! 8)
ps,
das mit den Namen werde ich später auch so machen, wschl allerdings als #defines:
#define LEFTW 0
#define RIGHTW 1
#define TURNTABLE 2
#define SHOULDER 3
oder so ähnlich...
update:
hab meinen ursprünglichen NXC Code jetzt soweit fü gpp portiert und entlaust, dass er (bis auf ein paar Warnings) fehlerfrei compiliert.
Ich bn mir noch unklar darüber, wo das pthread_join hinmuss, wenn man es den Tasks überlässt, dass sie bis zur Selbstterminierung laufen, um hier keine Karteileichen im Scheduler zu hinterlassen.
Ist das richtig: direkt hinter dem Aufruf von pthread im RotatePID Wrapper?
void RotatePID(uint8_t port, long Target, float RotatSpeed, int8_t cont) {
motor[port].runstate=1; // set runstate: PID active
// custom init PID [port]
motor[port].target =Target; // assign target
motor[port].tarpwm =RotatSpeed; // assign rotation speed
motor[port].cont=cont; // cont vs. hit once
// Reset PID control defaults
motor[port].outp =0; // PID control output value
motor[port].maxout =100; // absolute max possible output (max pwr)
motor[port].read =0; // current reading
motor[port].err =0; // current error
motor[port].integr =0; // integral of errors
motor[port].speed =0; // current speed
pthread_create( & motor[port].tid, // id speichern in dem dazugehörigen Array-Element
0, // Default Attribute (threads cancelbar)
PID_calc, // Name der PID-Kalkulationsroutinge
(void*)port); // der Index des Array-Elements für eine PID Struktur,
// die mit dem Motorindex gleich ist.
pthread_join(motor[port].tid, NULL); // <<<<<<<<<<<<<<<<<< ????
}
oder muss hier evtl auch noch eine zusätzliche while Abfrage dazwischen, denn der Task soll ja nicht unmittelbar nachdem er gestartet wurde schon wieder beendt/gejoint werden?
z.B.:
while(motor[port].tid);
:?:
Nein, das join kommt woanders hin.
Aber erstmal musst du zwei Typen von Threads unterscheiden:
1) Ein Thread auf den du nicht warten willst und der einfach durchlaufen soll ist ein "Detached" Thread. Ist dieser durch seine Routine gelaufen, dann räumt das OS von alleine auf. Damit ein Thread "Detached" markiert ist, rufst du nach dem pthread_create mit der ThreadId pthread_detach auf:
#include <stdbool.h>
void RotatePID(uint8_t port, long Target, float RotatSpeed, int8_t cont, bool detach) {
motor[port].runstate=1; // set runstate: PID active
// custom init PID [port]
motor[port].target =Target; // assign target
motor[port].tarpwm =RotatSpeed; // assign rotation speed
motor[port].cont=cont; // cont vs. hit once
// Reset PID control defaults
motor[port].outp =0; // PID control output value
motor[port].maxout =100; // absolute max possible output (max pwr)
motor[port].read =0; // current reading
motor[port].err =0; // current error
motor[port].integr =0; // integral of errors
motor[port].speed =0; // current speed
pthread_create( & motor[port].tid, // id speichern in dem dazugehörigen Array-Element
0, // Default Attribute (threads cancelbar)
PID_calc, // Name der PID-Kalkulationsroutinge
(void*)port); // der Index des Array-Elements für eine PID Struktur,
// die mit dem Motorindex gleich ist.
// Zur Unterscheidung Parameterliste von RotatePID und High-Level-Funktionen erweitern.
if(detach)
pthread_detach(& motor[port].tid);
}
Zu beachten gilt hier noch, dass du so einen Thread nach dem detach nie wieder joinen kannst. Sinvollerweise markierst du das Ende des Threads in der Routine zum Schluss im runstate und setzt die tid wieder auf 0.
2) Bei den anderen Threads die du joinen möchtest mußt du unterscheiden ob du auf das natürliche Ende warten willst oder ob du sie abbrechen (pthread_cancel) möchtest.
In deinem API hattest du eine StopPIDcontroler Funktion, in diese würde ich das optionale cancel und das join implementieren. In etwa so:
#include <stdbool.h>
void StopPIDcontrol(char port, bool cancel) {
if(cancel)
pthread_cancel(motor[port].tid)
pthread_join(motor[port].tid, 0);
motor[port].tid = 0;
motor[port].runstate = 0;
// und andere Variablen nach dem Ende des Threads setzen
}
Dann hast du eine klare Aufteilung: StopPIDcontrol hält joinable Threads (gewaltsam) an und alle anderen High-Level-Routinen starten einen Thread.
Gruss
botty
P.S. Sinnvollerweise guckst du dir in den man Pages die Rückgabewerte der pthread_ Funktionen an und behandelst noch die möglichen Fehler, das habe ich hier weg gelassen.
meine PID threads müssen sowohl durchlaufen können (sich selbst überlassen) als auch von außen beendet werden können, falls nötig.
Wie das gehen soll, ist im Prinzip auch "Verhandlungssache" (gewaltsam oder über einen Semaphor, s.u.).
Wer dann hinterher aufräumt, ist mir wurscht, am praktischsten ntl, wenn es automatisch geht ;)
A-Bär: wenn er einmal beendet ist, muss er (bzw. sein Nachfolger mit derselben port-Nr) ntl jederzeit wieder neu gestartet werden können, für die nächste PID-Annäherung. Ob er dann vom OS eine neue pthread-ID zugewiesen bekommt, ist aber ntl auch wieder egal.
Was das Joinen bewirkt, ist mir nicht ganz klar, ich hatte es so verstanden, dass ihre Timeslice wieder mit der von main() vereinigt wird.
Alle PID-Tasks sollen so gleichartig behandelt werden, während meine Tasks, die von main() am Anfang gestartet werden, so bleiben können wie bisher:
hier muss ich nichts unbedingt gewaltsam beenden, und wenn, dann funktioniert das dort über eine globale Variable (Semaphor) _TASKS_ACTIVE_ :
sobald die iwo auf false gesetzt wird, beenden sich alle Haupt-Threads dann selbsttätig und werden dann in main() gejoint:
// ***SNIP***
void* thread3Go (void* ) { // highest priority: 100µs encoder timer
while(_TASKS_ACTIVE_) {
updateEncoders();
usleep(100);
}
return NULL;
}
void* thread4Go (void* ) { // medium priority: UART
while(_TASKS_ACTIVE_) {
UARTcomm();
}
return NULL;
}
// ***SNIP***
int main() {
char sbuf[128];
int err;
// wiringPi
err = wiringPiSetup();
if( err == -1 ) return 1;
// UART Serial com port
fserialRemote = serialOpen (usb0, UARTclock); // UART baud rate
initarrays();
setupDpins();
setupI2C();
pthread_t thread0, thread1, thread2, thread3, thread4, thread5;
struct sched_param param;
pthread_create(&thread0, NULL, thread0Go, NULL); // lowest priority task: screen output
param.sched_priority = 10;
pthread_setschedparam(thread0, SCHED_RR, ¶m);
pthread_create(&thread1, NULL, thread1Go, NULL); // low priority: keyboard monitoring (stop program)
param.sched_priority = 20;
pthread_setschedparam(thread1, SCHED_RR, ¶m);
pthread_create(&thread2, NULL, thread2Go, NULL); // medium priority: motor control
param.sched_priority = 40;
pthread_setschedparam(thread2, SCHED_RR, ¶m);
pthread_create(&thread3, NULL, thread3Go, NULL); // highest priority: encoder reading
param.sched_priority = 80;
pthread_setschedparam(thread3, SCHED_FIFO, ¶m);
pthread_create(&thread4, NULL, thread4Go, NULL); // medium priority: UART comm <<< test !!
param.sched_priority = 40;
pthread_setschedparam(thread4, SCHED_FIFO, ¶m);
pthread_create(&thread5, NULL, thread5Go, NULL); // medium priority: navigation
param.sched_priority = 40;
pthread_setschedparam(thread5, SCHED_FIFO, ¶m);
while(_TASKS_ACTIVE_) { delay(1); }
// wait for threads to join before exiting
pthread_join(thread0, NULL);
pthread_join(thread1, NULL);
pthread_join(thread2, NULL);
pthread_join(thread3, NULL);
pthread_join(thread4, NULL);
pthread_join(thread5, NULL);
serialClose( fserialRemote);
exit(0);
}
falls es das irgendwie einfacher macht, könnte man auch die PID-threads auf diese Weise mit einer zusätzlichen Abbruchbedingung stoppen!
Der Standardfall wäre sicher, dass der PID Task gestartet wird und man überlässt ihn sich selber.
trotzdem kann es sein
(z.B.: bei ursprünglich geplante "Annäherung um 9000 Grad vorwärts drehen"
oder bei "Ziel annähern und kontinuierlich, dauerhaft gegen (passive) Verstellung halten")
, dass er vorzeitig gestoppt werden muss , wenn es die Situation erfordert,
und dann nach Abbruch eventuell sofort eine neue Annäherung erfolgen muss
(auf z.B. -1000 Grad per rückwärts drehen oder was auch immer) .
Wie wäre dann der einfachste Weg, das Stoppen zu bewerkstelligen?
Idee zum vorzeitigen Abbruch der PID-Regulation-Loop:
evtl das Stoppen über Implementierung einer weiteren PID-Strukturvariable
char StopPIDcontrol
die standardmässig anfangs auf Null gesetzt wird,
und wenn sie extern zum Beenden auf 1 gesetzt wird, wird nach interner Prüfung in der pthread PID loop der pthread beendet:
if(motor[port].StopPIDcontrol) {
motorCoast(port);
motor[port].runstate=0;
motor[port].speed =0;
motor[port].outp =0;
motor[port].cont =0;
return NULL;
}
wie und wo kämen dann weitere Aufräumarbeiten hin, und wie oder wo muss dann etwas gejoint werden oder nicht?
Nein eine weitere Variable brauchst du nicht.
Mit der Funktion pthread_cancel wird der Thread am nächsten cancel point unterbrochen. Ein cancel point ist entweder eine Systemfunktion oder du kannst explizit an Stellen in deiner PID Routine ein cancel point mit pthread_testcancel einbauen. Vorzugsweise vor einer Zeitintensiven Aktion. Ich hatte diese Funktion gestern Abend schon an den Anfang in der do{}while()-Schleife gepackt (Wie sieht der Code der aktuellen Version deiner PID Kalkulation aus?).
Zum join: diese Funktion dient der synchronisation zwischen aufrufenden Thread und dem Thread dessen id du beim Aufruf angibst. Sprich der Aufrufende wartet solange bis der andere zu Ende ist, diese Funktion blockiert also ggf. In deinem letzten Beispiel eine Seite vorher kannst du dir
while(_TASKS_ACTIVE_) { delay(1); }
sparen. Der main Thread blockiert am ersten join bis sich die Threads beenden.
Was für mich jetzt zum Verständnis hilfreich wäre: Wo entscheidest du was gemacht werden soll? Und in welchem Thread läuft das dann ab? Wie sieht grob dieser Teil des Programms aus?
(zur späteren Gesamt-Programmstruktur komme ich später, wenn du einverstanden bist - im Moment sind , genau wie die schon vorhandenen simplen Befehle
motorOn()
motorBrake()
motorCoast()
die jetzt neuen PID-Funktionen
RotatePIDtoTarget ()
RotatePIDdegrees ()
RotatePIDcontinue ()
StopPIDcontrol ();
PIDinit();
SetPIDparam();
SetPIDparamEx();
nur allgemeine "bequeme" Motor-API Funktionen, die universell eingesetzt werden können/sollen zur exakten Steuerung der Encodermotoren;
hinzu kommen sollen später auch noch weitere Funktionen wie rampup/down und motorSync(port1, port2, syncratio, pwm )
der aktuelle Code ist dieser hier - es gibt zu PID_calc noch eine Variante 2 wo die #ifdef debugs noch drin sind:
(das mit dem Stoppen ist noch nicht geändert bzw. berichtigt)
//************************************************** ***********
// PID control
//************************************************** ***********
void RotatePIDtoTarget (uint8_t port, long Target, double RotatSpeed); // approach absolute target once
void RotatePIDdegrees (uint8_t port, long Target, double RotatSpeed); // turn relative degrees
void RotatePIDcontinue (uint8_t port, long Target, double RotatSpeed); // approach target continuously
void StopPIDcontrol (uint8_t port);
//************************************************** ***********
void PIDinit(); // P=0.4, I=0.4, D=10.0
// simple customized PID setting:
void SetPIDparam(uint8_t port, double P,double I,double D);
// extended customized parameter setting:
void SetPIDparamEx(uint8_t port, double P, double I, double D, double prec, int16_t regtime, double damp); // stop PID
//************************************************** ***********
void * PID_calc(void *arg) {
double aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
int regloop;
// arg nach index casten
unsigned port = (unsigned)arg;
motor[port].runstate = 0x10; // reg state: RAMPUP
motor[port].read = motor[port].motenc; // get current encoder reading
motor[port].err = motor[port].target - motor[port].read; // error to target
readstart = motor[port].read;
regloop = 1;
damp=0; // damp the integral memory
starttime= millis();
// appoach target
_Astart:
motor[port].runstate = 0x10; // run state: RUNNING
do {
pthread_testcancel();
dtime = millis() - clock;
clock = millis();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((motor[port].err==errorold)&& (abs(motor[port].err)>motor[port].precis))
damp=1; // stalling
else
damp=motor[port].damp;
motor[port].integr = (damp * motor[port].integr) + motor[port].err;
if((motor[port].integr) > 3*motor[port].maxout)
motor[port].integr = 3*motor[port].maxout; // cut away
else if((motor[port].integr) <-3*motor[port].maxout)
motor[port].integr =-3*motor[port].maxout;
PWMpwr= (motor[port].P*motor[port].err) + (motor[port].I*motor[port].integr)*tprop + (motor[port].D*(motor[port].err-errorold))/tprop;
if(PWMpwr > motor[port].maxout)
PWMpwr= motor[port].maxout; // forward maxout
else if(PWMpwr < -motor[port].maxout)
PWMpwr= -motor[port].maxout; // reverse maxout
motor[port].speed= (motor[port].read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(motor[port].speed) ;
if (aspeed > motor[port].tarpwm) {
PWMpwr = sign(PWMpwr)*motor[port].tarpwm;
}
motor[port].outp = round(PWMpwr);
//************************************************** **********************
// PID regulation !
motorOn(port, (motor[port].outp)); // action !
delay(motor[port].regtime); // wait regulation time
//************************************************** ************************
readold = motor[port].read; // save old sensor
errorold = motor[port].err; // save old error
motor[port].read = motor[port].motenc; // get new encoder value
motor[port].err = motor[port].target-motor[port].read; // new error to target
if (motor[port].read>cmax)
cmax=motor[port].read; // monitor overshooting
else if (motor[port].read<cmin)
cmin=motor[port].read; // monitor overshooting
if ((motor[port].cont)&& (abs(motor[port].err)<=motor[port].precis))
motor[port].runstate = 0x60;
else
motor[port].runstate = 0x20;
if (motor[port].cont)
continue;
if (abs(motor[port].err)<=motor[port].precis) {
regloop +=1 ;
motor[port].runstate = 0x40;
}
} while ((abs(motor[port].err)>=motor[port].precis) && (regloop<=5)); // target reached
motorCoast(port); // finished - brake motor
motor[port].runstate = 0x40; // run state: RAMPDOWN
motor[port].outp=0;
delay(50);
motor[port].read = motor[port].motenc;
regloop=1;
if (motor[port].read>cmax) cmax=motor[port].read; // detect overshooting
if (motor[port].read<cmin) cmin=motor[port].read; // detect overshooting
motor[port].err = motor[port].target-motor[port].read;
if ((abs(motor[port].err)>motor[port].precis)) {
goto _Astart;
}
motor[port].runstate=0;
delay(1); //runstate = IDLE
return NULL;
}
//************************************************** **********************
void * PID_calc2(void *arg) {
double aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
int regloop;
// arg nach index casten
unsigned port = (unsigned)arg;
motor[port].runstate = 0x10; // reg state: RAMPUP
motor[port].read = motor[port].motenc; // get current encoder reading
motor[port].err = motor[port].target - motor[port].read; // error to target
readstart = motor[port].read;
regloop = 1;
#ifdef debug_motor0
//.................................................. ..........................
// init variables for graph output
ClearScreen();
DisplayMask();
int timex, oldtx, oldy=15, pwm0=15; // values for graphic screen
float scrXratio, scrYratio;
scrXratio=abs(motor[port].err)/8;
if(motor[port].target<50) scrXratio=abs(motor[port].err)/4;
scrYratio=abs(motor[port].err)/40;
//.................................................. ..........................
#endif
damp=0; // damp the integral memory
starttime= millis();
// appoach target
_Astart:
motor[port].runstate = 0x10; // run state: RUNNING
do {
dtime = millis() - clock;
clock = millis();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((motor[port].err==errorold)&& (abs(motor[port].err)>motor[port].precis)) damp=1; // stalling
else
damp=motor[port].damp;
motor[port].integr = (damp * motor[port].integr) + motor[port].err;
if((motor[port].integr) > 3*motor[port].maxout) motor[port].integr = 3*motor[port].maxout; // cut away
else
if((motor[port].integr) <-3*motor[port].maxout) motor[port].integr =-3*motor[port].maxout;
PWMpwr= (motor[port].P*motor[port].err) + (motor[port].I*motor[port].integr)*tprop + (motor[port].D*(motor[port].err-errorold))/tprop;
if(PWMpwr > motor[port].maxout) PWMpwr= motor[port].maxout; // forward maxout
else
if(PWMpwr < -motor[port].maxout) PWMpwr= -motor[port].maxout; // reverse maxout
motor[port].speed= (motor[port].read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(motor[port].speed) ;
if (aspeed > motor[port].tarpwm) {
PWMpwr = sign(PWMpwr)*motor[port].tarpwm;
}
motor[port].outp = round(PWMpwr);
#ifdef debug_motor0
//.................................................. ........................
// for graph output
timex= runtime/scrXratio;
PointOut(timex,(motor[port].read-readstart)/scrYratio);
LineOut(oldtx, oldy, timex, pwm0+motor[port].speed*0.3);
oldtx=timex; oldy=pwm0+motor[port].speed*0.3;
//.................................................. ........................
#endif
//************************************************** ************************
// PID regulation !
motorOn(port, motor[port].outp); // action !
delay(motor[port].regtime); // wait regulation time
//************************************************** *************************
readold = motor[port].read; // save old sensor
errorold = motor[port].err; // save old error
motor[port].read = motor[port].motenc; // get new encoder value
motor[port].err = motor[port].target-motor[port].read; // new error to target
if (motor[port].read>cmax) cmax=motor[port].read; // monitor overshooting
else
if (motor[port].read<cmin) cmin=motor[port].read; // monitor overshooting
if ((motor[port].cont)&& (abs(motor[port].err)<=motor[port].precis)) motor[port].runstate = 0x60;
else motor[port].runstate = 0x20;
#ifdef debug_motor0
//.................................................. ........................
printf1(68, 48,"%-5d" , cmax);
printf1(68, 40,"%-5d" , cmin);
printf1(68, 32,"%-5d" , motor[port].read);
//.................................................. ........................
#endif
if (motor[port].cont) continue;
if (abs(motor[port].err)<=motor[port].precis) { regloop +=1 ; motor[port].runstate = 0x40; }
} while ((abs(motor[port].err)>=motor[port].precis) && (regloop<=5)); // target reached
motorCoast(port); // finished - brake motor
motor[port].runstate = 0x40; // run state: RAMPDOWN
motor[port].outp=0;
delay(50);
motor[port].read = motor[port].motenc;
regloop=1;
if (motor[port].read>cmax) cmax=motor[port].read; // detect overshooting
if (motor[port].read<cmin) cmin=motor[port].read; // detect overshooting
motor[port].err = motor[port].target-motor[port].read;
#ifdef debug_motor0
//.................................................. ..........................
printf1(68, 56,"%-5d" , motor[port].target);
printf1(68, 48,"%-5d" , cmax);
printf1(68, 40,"%-5d" , cmin);
printf1(68, 32,"%-5d" , motor[port].read);
printf1(56, 24,"P %5.2f" , motor[port].P);
printf1(56, 16,"I %5.2f" , motor[port].I);
printf1(56, 8,"D %5.2f" , motor[port].D);
//.................................................. ..........................
#endif
if ((abs(motor[port].err)>motor[port].precis)) {goto _Astart;}
#ifdef debug_motor0
//.................................................. ..........................
PointOut(timex,motor[port].read/scrYratio);
LineOut(oldtx, oldy, timex, pwm0);
LineOut(timex+2,motor[port].target/scrYratio, timex+10, motor[port].target/scrYratio);
LineOut(timex+2, pwm0, timex+10, pwm0);
//.................................................. ..........................
#endif
motor[port].runstate=0;
delay(1); //runstate = IDLE
return (NULL);
}
//************************************************** ***********
void RotatePID(uint8_t port, long Target, float RotatSpeed, int8_t cont) {
motor[port].runstate=1; // set runstate: PID active
// custom init PID [port]
motor[port].target =Target; // assign target
motor[port].tarpwm =RotatSpeed; // assign rotation speed
motor[port].cont=cont; // cont vs. hit once
// Reset PID control defaults
motor[port].outp =0; // PID control output value
motor[port].maxout =100; // absolute max possible output (max pwr)
motor[port].read =0; // current reading
motor[port].err =0; // current error
motor[port].integr =0; // integral of errors
motor[port].speed =0; // current speed
pthread_create( & motor[port].tid, // id speichern in dem dazugehörigen Array-Element
0, // Default Attribute (threads cancelbar)
PID_calc, // Name der PID-Kalkulationsroutinge
(void*)port); // der Index des Array-Elements für eine PID Struktur,
// die mit dem Motorindex gleich ist.
pthread_join(motor[port].tid, NULL); // <<<<<<<<<<<<<<<<<< ????
}
void StopPIDcontrol (uint8_t port, int cancel) {
if (motor[port].tid) { // stop PID task if already running
motorCoast(port);
if(cancel)
pthread_cancel(motor[port].tid);
pthread_join(motor[port].tid, 0);
motor[port].tid = 0; // wichtig!
motor[port].cont=false;
motor[port].runstate=0;
}
}
//************************************************** ***********
// ^^^^^^^^^^^^^^ PID end ^^^^^^^^^^^^^
//************************************************** ***********
zu deinen Anmerkungen / den offenen Fragen:
ohne das
while(_TASKS_ACTIVE_) { delay(1); }
läuft das Programm mit den Tasks nicht korrekt, es ist einfach "durchgerauscht", ich habe es daher nachträglich noch hinzufügen müssen.
Das
pthread_cancel
wollte ich mit der neuen zusätzlichen Struktur-Variablen einsparen, denn das canceln schien mir vlt etwas zu rabiat.
Nur so ein Gefühl.
Es erschien mir nachträglich, alternativ, auch einfacher, fürs Stoppen nur eine Variable zu setzen, damit sich dann der Task selber beendet, als dies von außen über eine recht komplizierte Zusatzfunktion zu machen; stattdessen also nur
stopPID(char port) {
motor[port].StopPIDcontrol=1;
}
( anders war es bei NXC, da hat die VM alles mit start und stop task für mich erledigt (ähnlich wie bei Java). )
Wenn ich also die Variablen-/Semaphor-Methode auch für die PID-Tasks nutzen wollte: wo im Code wäre dann was aufzuräumen oder zu joinen?
ps,
da ich ja jetzt alle PID Threads einfach "durchlaufen lasse", sind sie ja i.P. alle "detached" -
also käme dann doch einfach hinter alle pthread_create(& motor[port].tid,...)
einfach ein
pthread_detach(motor[port].tid);
ohne weiteres join(..) ?
- denn nun beenden sich alle immer selber (auch die, die den externen Anschubser zum Beenden gekriegt haben)
- und können sich daher auch immer automatisch selber aufräumen - oder....?
Ja, ich denke das geht so mit dem detach.
Du musst nur darauf achten das du vor dem start eines neuen Threads dein runstate Element testest. Erst wenn das 0 ist kannst du einen neuen Thread auf diese PID Struktur los lassen, sonst gibt's Quddel-Muddel.
Gruss
botty
jap, mach ich!
void RotatePID(uint8_t port, long Target, float RotatSpeed, int8_t cont) {
if( !motor[port].tid && motor[port].runstate) motor[port].runstate=0; // repair
if( motor[port].runstate ) return;
if( motor[port].tid ) return;
motor[port].runstate=1; // set runstate: PID active
// custom init PID [port]
motor[port].target =Target; // assign target
motor[port].tarpwm =RotatSpeed; // assign rotation speed
motor[port].cont=cont; // cont vs. hit once
// Reset PID control defaults
motor[port].outp =0; // PID control output value
motor[port].maxout =100; // absolute max possible output (max pwr)
motor[port].read =0; // current reading
motor[port].err =0; // current error
motor[port].integr =0; // integral of errors
motor[port].speed =0; // current speed
motor[port].stopPIDcontrol =0; // flag for external termination
pthread_create( & motor[port].tid, // id speichern in dem dazugehörigen Array-Element
0, // Default Attribute (threads cancelbar)
PID_calc, // Name der PID-Kalkulationsroutinge
(void*)port); // der Index des Array-Elements für eine PID Struktur,
// die mit dem Motorindex gleich ist.
pthread_detach(motor[port].tid);
}
Powered by vBulletin® Version 4.2.5 Copyright ©2024 Adduco Digital e.K. und vBulletin Solutions, Inc. Alle Rechte vorbehalten.