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:
aufgerufen werden soll das Objekt bzw. seine Instanz mit seinem Task von so einer FunktionCode:// "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 }
(port == "Motornummer" oder auch A, B, C, D, E,.... ):
Code: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):
- - - Aktualisiert - - -Code: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 } ;
edit:
ich muss auch sagen, die Erklärungen in http://stackoverflow.com/questions/1...n-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.
Zitieren
Lesezeichen