Na ja,
zuerst must du die PIDinit ergänzen:
Die Signaturen deiner High-Level Funktionen ändern sich nur im ersten Element, die anderen Parameter kannst du 1:1 übernehmen z.B.:Code: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. }
wobei du festlegen musst ob evtl. Fehlercodes wieder hochgereicht werden? Hier spar ich mir das mal.Code:inline void RotatePIDtoTarget(struct PIDstruct *pid, long Target, float RotatSpeed) { RotatePID(pid, Target, RotatSpeed, false); }
Deine zentrale RotatePID sähe dann so aus:
Stoppen ginge dann soCode: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; } }
Die verwendung sähe dann etwa so aus:Code: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; } }
Ich habe diesen Code nicht durchkompiliert, hoffe aber das er trotzdem deine Fragen klärt.Code: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; }
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







Zitieren

Lesezeichen