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:
Code:
// "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,.... ):
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):
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
} ;
- - - Aktualisiert - - -
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.
Lesezeichen