Na ja,
zuerst must du die PIDinit ergänzen:
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.
}
Die Signaturen deiner High-Level Funktionen ändern sich nur im ersten Element, die anderen Parameter kannst du 1:1 übernehmen z.B.:
Code:
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:
Code:
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
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;
}
}
Die verwendung sähe dann etwa so aus:
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;
}
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
Lesezeichen