Mindstormsforum Dateiupload | Links | Lexikon | NXT Shop | RobotC.de | Wettbewerbe |    Seitenreport - Die SEO und Website Analyse

bild
bild
Unbeantwortete Themen | Aktive Themen Aktuelle Zeit: 21. Okt 2014 04:42


Auf das Thema antworten  [ 13 Beiträge ] 
NXC: motor PID controller: move to encoder/sensor target 
Autor Nachricht
Administrator
Administrator
Benutzeravatar

Registriert: 11. Jan 2006 21:01
Beiträge: 4327
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze
Beitrag NXC: motor PID controller: move to encoder/sensor target
hey,
nach einigem Herumprobieren anlässlich des anderen PID-threads habe ich hier einen PID-Controller programmiert, der einen Motor-Counter-Zielwert (encoder target) möglichst exakt erreichen soll, nicht überschießt oder wackelt gegen Ende und auch unter starker Last oder Lastwechsel nicht vorzeitig verreckt.
Hier ist mein Versuch - bitte mal testen, wer mag, und n. M. verbessern!

Ausgabe von Zielwert, Endwert, min- und max Wert während der Laufzeit.
(ab version 4.2: Bug mit negativen Zielwerten beseitigt.)

Code:
// PID controller: exactly approaching an encoder target by a motor

Zitat:
edit:
neueste Version mit Graphic display output siehe unten!

_________________
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


5. Mai 2013 22:23
Profil
Schreibt super viel
Schreibt super viel

Registriert: 17. Dez 2011 11:39
Beiträge: 490
Wohnort: Österreich
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
Stell mal das Ziel auf 90° und lass dir die Kurve anzeigen.
Da sieht man dann besser wie der Regler reagiert.

Also einmal Motorpower-Graph und einmal Encoder Graph.

_________________
Projekte und Informationen auf meiner Website: http://roboticsaumair.jimdo.com/


7. Mai 2013 08:28
Profil
Administrator
Administrator
Benutzeravatar

Registriert: 11. Jan 2006 21:01
Beiträge: 4327
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
bei mittlerer Belastung lässt sich visuell z.B. gar kein Pendeln feststellen.

hab aber keine Möglichkeit, sowas in einer Excel-Tabelle zu speichern, weil ich immer nur komplett ohne jede Verbindung zum PC arbeite.

Hast du ein PC-comm-Programm?
(Die Frage ist dann auch, ob die Datenübermittlung in real-time erfolgt oder ggf durch die Datenübermittlung (NXT-CPU-Belastung) evtl der Regler gestört wird)

_________________
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


7. Mai 2013 08:48
Profil
Schreibt super viel
Schreibt super viel

Registriert: 17. Dez 2011 11:39
Beiträge: 490
Wohnort: Österreich
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
Mit Labview lässt sich sowas schnell machen. Über USB gibt es normal keine Probleme.

Ich weis nicht ob du noch mein Labview Programm hast was was ich hier mal hochgeladen habe.
Dort könnte man auch einfach immer Werte an die Konsole schicken und nachher mit copy/past in Excel reinkopieren.


Ps: Wenn du noch warten kannst, ich arbeite gerade an einer schönen Umsetzung mit C++.
Das Tool wird dann jedenfalls mehr Ansprechender, da es den Windows-GUI Style besitzt und mehr handlicher ist.
(Es hat den BCC Style ;) )

_________________
Projekte und Informationen auf meiner Website: http://roboticsaumair.jimdo.com/


Zuletzt geändert von Martin0x5E am 7. Mai 2013 09:03, insgesamt 2-mal geändert.



7. Mai 2013 08:57
Profil
Schreibt super viel
Schreibt super viel

Registriert: 17. Dez 2011 11:39
Beiträge: 490
Wohnort: Österreich
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
Die Zeit wie lang der Motor zum Ziel braucht ist auch noch interessant.

Wird der pid Regler nur eingeschaltet wenn die Funktion aufgerufen wird, oder versucht der Regler den Motor auch auf dem Zielpunkt zu halten?

_________________
Projekte und Informationen auf meiner Website: http://roboticsaumair.jimdo.com/


9. Mai 2013 15:30
Profil
Administrator
Administrator
Benutzeravatar

Registriert: 11. Jan 2006 21:01
Beiträge: 4327
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
Zitat:
edit:
- Der Code kann inzw. beides: sowohl nur 1x approximieren, dann automatisch beenden - oder - approximieren und dann dauerhaft nachfolgen und nachregulieren.
- bug mit negativen Zielwerten gekillt - außer die Graphen, die funktionieren nur mit positiven.


Testaufbau: an alle 3 Ports je einen Motor.
Der folgende Code enthält einen debug-Mode, um die Motorsteuerung als Graph auf dem Display anzeigen zu lassen (zum Tuning und Fein-Tuning).


wen es interessiert: Bitte testen und berichten!

Zitat:
Edit: neueste Version weiter unten im Thread!

_________________
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


2. Jun 2013 12:28
Profil
Schreibt super viel
Schreibt super viel

Registriert: 17. Dez 2011 11:39
Beiträge: 490
Wohnort: Österreich
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
Wow, da hast schon einiges an Arbeit geleistet!

Wird der Regler nur zum Erreichen einer bestimmten Position verwendet oder auch zum halten er aktuellen Position?

Wie müsste man den Code wohl eventuell modifizieren wenn man die Motoren aktiv Bremsen möchte (zb Drehrichtung invertieren) ?

_________________
Projekte und Informationen auf meiner Website: http://roboticsaumair.jimdo.com/


2. Jun 2013 18:48
Profil
Administrator
Administrator
Benutzeravatar

Registriert: 11. Jan 2006 21:01
Beiträge: 4327
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
Zitat:
edit: - Der Code kann inzw. beides: sowohl nur 1x approximieren, dann automatisch beenden - oder - approximieren und dann dauerhaft nachfolgen und nachregulieren.


Abschließend wird aktiv gebremst, um das Ziel zu fixieren.
Danach kann man im Code aber weitermachen und z.B. auf Coast umschalten u/o einen neuen Zielwert anvisieren.

Und: hast du es mal ausprobiert?

_________________
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


2. Jun 2013 19:03
Profil
Administrator
Administrator
Benutzeravatar

Registriert: 11. Jan 2006 21:01
Beiträge: 4327
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
so,
jetzt ist auch der "continuous-mode" = kontinuierliche Ziel-Verfolgung enthalten:
(englischsprachige Erläuterungen siehe http://sourceforge.net/apps/phpbb/mindboards/viewtopic.php?f=3&t=1850#p16362 - Achtung - US-Forum down ! )

Code:
// PID controller: exactly approaching sensor targets by motor control
// ver 1.05

// (c) HaWe 2013
// protected under the friendly Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http://creativecommons.org/licenses/by-nc-sa/3.0/

///////////////////////////////////////////////////////////////////////////////////////
//        basic PID rotate and stop functions:
//
// 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
//
///////////////////////////////////////////////////////////////////////////////////////
//
//        how to set PID custom parameters
//
//        a) initialize by default settings
// 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);
//
///////////////////////////////////////////////////////////////////////////////////////

// NXC API: runstate constants
/*
#define    OUT_RUNSTATE_IDLE     0x00
#define    OUT_RUNSTATE_RAMPUP   0x10
#define    OUT_RUNSTATE_RUNNING  0x20
#define    OUT_RUNSTATE_RAMPDOWN 0x40
#define    OUT_RUNSTATE_HOLD     0x80
*/

#define   OUT_RUNSTATE_ACTIVE   0x01

#define debug_PID_A       // debug mode featuring graphic output
//#define debug_PID_B       // debug mode featuring graphic output
//#define debug_PID_C       // debug mode featuring graphic output


//forward:
void DisplayMask();


#define printf1( _x, _y, _format1, _value1) {  \
  string sval1 = FormatVal(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}


inline long round(float f) { return (f>0?(f+0.5):(f-0.5)); }

//==============================================================================

struct PIDstruct {
                 // 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


} ;


PIDstruct PID_A, PID_B, PID_C;

//==============================================================================


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

}

//==============================================================================


task task_PID_B() {
  float aspeed, damp, PWMpwr, readold, errorold, tprop;
  long  readstart, cmax, cmin;                         // for monitoring
  long  starttime, runtime, clock, dtime;              // timer
  char  regloop;

  PID_B.runstate = 0x10;                               // reg state: RAMPUP
  PID_B.read     = (MotorRotationCount(OUT_B));        // get current encoder reading
  PID_B.err      = PID_B.target - PID_B.read;          // error to target

  readstart      = PID_B.read;
  regloop        = 1;



#ifdef debug_PID_B
  //............................................................................
  // init variables for graph output
  ClearScreen();
  DisplayMask();


  int timex, oldtx, oldy=15, pwm0=15;           // values for graphic screen

  float scrXratio, scrYratio;

  scrXratio=abs(PID_B.err)/8;
  if(PID_B.target<50) scrXratio=abs(PID_B.err)/4;

  scrYratio=abs(PID_B.err)/40;
  //............................................................................
#endif


  damp=0;                                 // damp the integral memory

  starttime= CurrentTick();


  // appoach target
  _Bstart:
  PID_B.runstate = 0x10;                  // run state: RUNNING

  do {

    dtime    = CurrentTick() - clock;
    clock    = CurrentTick();
    runtime  = clock - starttime;
    tprop    = dtime/20.0;

    if ((PID_B.err==errorold)&& (abs(PID_B.err)>PID_B.precis)) damp=1;    // stalling
    else
    damp=PID_B.damp;

    PID_B.integr = (damp * PID_B.integr) + PID_B.err;

    if((PID_B.integr) > 3*PID_B.maxout) PID_B.integr = 3*PID_B.maxout; // cut away
    else
    if((PID_B.integr) <-3*PID_B.maxout) PID_B.integr =-3*PID_B.maxout;

    PWMpwr= (PID_B.P*PID_B.err) + (PID_B.I*PID_B.integr)*tprop + (PID_B.D*(PID_B.err-errorold))/tprop;


    if(PWMpwr >  PID_B.maxout) PWMpwr=  PID_B.maxout;   // forward maxout
    else
    if(PWMpwr < -PID_B.maxout) PWMpwr= -PID_B.maxout;   // reverse maxout


    PID_B.speed= (PID_B.read-readold)*100/dtime;  // rotat speed [degrees/100ms]

    aspeed = abs(PID_B.speed) ;

    if (aspeed > PID_B.tarpwm)  {
        PWMpwr = sign(PWMpwr)*PID_B.tarpwm;
    }

    PID_B.outp = round(PWMpwr);

#ifdef debug_PID_B
  //............................................................................
    //  for graph output
    timex= runtime/scrXratio;
    PointOut(timex,(PID_B.read-readstart)/scrYratio);
    LineOut(oldtx, oldy, timex, pwm0+PID_B.speed*0.3);

    oldtx=timex; oldy=pwm0+PID_B.speed*0.3;
  //............................................................................
#endif

    //**************************************************************************
                                                   // PID regulation !
    OnFwd(OUT_B, (PID_B.outp));                         // action !
    Wait(PID_B.regtime);                                // wait regulation time

    //**************************************************************************

    readold     = PID_B.read;                           // save old sensor
    errorold    = PID_B.err;                            // save old error

    PID_B.read  = (MotorRotationCount(OUT_B));          // get new encoder value
    PID_B.err   = PID_B.target-PID_B.read;              // new error to target

    if (PID_B.read>cmax) cmax=PID_B.read;               // monitor overshooting
    else
    if (PID_B.read<cmin) cmin=PID_B.read;               // monitor overshooting

    if ((PID_B.cont)&& (abs(PID_B.err)<=PID_B.precis)) PID_B.runstate = 0x60;
    else PID_B.runstate = 0x20;


#ifdef debug_PID_B
    //..........................................................................
    printf1(68, 48,"%-5d"   , cmax);
    printf1(68, 40,"%-5d"   , cmin);
    printf1(68, 32,"%-5d"   , PID_B.read);
    //..........................................................................
#endif

    if (PID_B.cont) continue;
    if (abs(PID_B.err)<=PID_B.precis) { regloop +=1 ; PID_B.runstate = 0x40; }

  } while ((abs(PID_B.err)>=PID_B.precis) && (regloop<=5));  // target reached

  Off(OUT_B);                                      // finished - brake motor
  PID_B.runstate = 0x40;                           // run state: RAMPDOWN
  PID_B.outp=0;

  Wait(50);
  PID_B.read = MotorRotationCount(OUT_B);
  regloop=1;

  if (PID_B.read>cmax) cmax=PID_B.read;            // detect overshooting
  if (PID_B.read<cmin) cmin=PID_B.read;            // detect overshooting
  PID_B.err = PID_B.target-PID_B.read;


#ifdef debug_PID_B
  //............................................................................
  printf1(68, 56,"%-5d"    , PID_B.target);
  printf1(68, 48,"%-5d"    , cmax);
  printf1(68, 40,"%-5d"    , cmin);
  printf1(68, 32,"%-5d"    , PID_B.read);
  printf1(56, 24,"P %5.2f" , PID_B.P);
  printf1(56, 16,"I %5.2f" , PID_B.I);
  printf1(56,  8,"D %5.2f" , PID_B.D);
  //............................................................................
#endif


  if ((abs(PID_B.err)>PID_B.precis))  {goto _Bstart;}



#ifdef debug_PID_B
  //............................................................................
  PointOut(timex,PID_B.read/scrYratio);
  LineOut(oldtx, oldy, timex, pwm0);
  LineOut(timex+2,PID_B.target/scrYratio, timex+10, PID_B.target/scrYratio);
  LineOut(timex+2, pwm0, timex+10, pwm0);
  //............................................................................
#endif

  PID_B.runstate=0;
  Wait(1);                                //runstate = IDLE

}

//==============================================================================

task task_PID_C() {
  float aspeed, damp, PWMpwr, readold, errorold, tprop;
  long  readstart, cmax, cmin;                         // for monitoring
  long  starttime, runtime, clock, dtime;              // timer
  char  regloop;

  PID_C.runstate = 0x10;                               // reg state: RAMPUP
  PID_C.read     = (MotorRotationCount(OUT_C));        // get current encoder reading
  PID_C.err      = PID_C.target - PID_C.read;          // error to target

  readstart      = PID_C.read;
  regloop        = 1;


#ifdef debug_PID_C
  //............................................................................
  // init variables for graph output
    ClearScreen();
    DisplayMask();
  int timex, oldtx, oldy=15, pwm0=15;           // values for graphic screen

  float scrXratio, scrYratio;

  scrXratio=abs(PID_C.err)/8;
  if(PID_C.target<50) scrXratio=abs(PID_C.err)/4;

  scrYratio=abs(PID_C.err)/40;
  //............................................................................
#endif


  damp=0;                                 // damp the integral memory

  starttime= CurrentTick();


  // appoach target
  _Cstart:
  PID_C.runstate = 0x10;                  // run state: RUNNING

  do {

    dtime    = CurrentTick() - clock;
    clock    = CurrentTick();
    runtime  = clock - starttime;
    tprop    = dtime/20.0;

    if ((PID_C.err==errorold)&& (abs(PID_C.err)>PID_C.precis)) damp=1;    // stalling
    else
    damp=PID_C.damp;

    PID_C.integr = (damp * PID_C.integr) + PID_C.err;

    if((PID_C.integr) > 3*PID_C.maxout) PID_C.integr = 3*PID_C.maxout; // cut away
    else
    if((PID_C.integr) <-3*PID_C.maxout) PID_C.integr =-3*PID_C.maxout;

    PWMpwr= (PID_C.P*PID_C.err) + (PID_C.I*PID_C.integr)*tprop + (PID_C.D*(PID_C.err-errorold))/tprop;


    if(PWMpwr >  PID_C.maxout) PWMpwr=  PID_C.maxout;   // forward maxout
    else
    if(PWMpwr < -PID_C.maxout) PWMpwr= -PID_C.maxout;   // reverse maxout


    PID_C.speed= (PID_C.read-readold)*100/dtime;  // rotat speed [degrees/100ms]

    aspeed = abs(PID_C.speed) ;

    if (aspeed > PID_C.tarpwm)  {
        PWMpwr = sign(PWMpwr)*PID_C.tarpwm;
    }

    PID_C.outp = round(PWMpwr);

#ifdef debug_PID_C
    //..........................................................................
    //  for graph output
    timex= runtime/scrXratio;
    PointOut(timex,(PID_C.read-readstart)/scrYratio);
    LineOut(oldtx, oldy, timex, pwm0+PID_C.speed*0.3);

    oldtx=timex; oldy=pwm0+PID_C.speed*0.3;
    //..........................................................................
#endif

    //**************************************************************************
                                                   // PID regulation !
    OnFwd(OUT_C, (PID_C.outp));                         // action !
    Wait(PID_C.regtime);                                // wait regulation time

    //**************************************************************************

    readold     = PID_C.read;                           // save old sensor
    errorold    = PID_C.err;                            // save old error

    PID_C.read  = (MotorRotationCount(OUT_C));          // get new encoder value
    PID_C.err   = PID_C.target-PID_C.read;              // new error to target

    if (PID_C.read>cmax) cmax=PID_C.read;               // monitor overshooting
    else
    if (PID_C.read<cmin) cmin=PID_C.read;               // monitor overshooting

    if ((PID_C.cont)&& (abs(PID_C.err)<=PID_C.precis)) PID_C.runstate = 0x60;
    else PID_C.runstate = 0x20;

#ifdef debug_PID_C
    //..........................................................................
    printf1(68, 48,"%-5d"   , cmax);
    printf1(68, 40,"%-5d"   , cmin);
    printf1(68, 32,"%-5d"   , PID_C.read);
    //..........................................................................
#endif

    if (PID_C.cont) continue;
    if (abs(PID_C.err)<=PID_C.precis) { regloop +=1 ; PID_C.runstate = 0x40; }

  } while ((abs(PID_C.err)>=PID_C.precis) && (regloop<=5));  // target reached

  Off(OUT_C);                                      // finished - brake motor
  PID_C.runstate = 0x40;                           // run state: RAMPDOWN
  PID_C.outp=0;

  Wait(50);
  PID_C.read = MotorRotationCount(OUT_C);
  regloop=1;

  if (PID_C.read>cmax) cmax=PID_C.read;            // detect overshooting
  if (PID_C.read<cmin) cmin=PID_C.read;            // detect overshooting
  PID_C.err = PID_C.target-PID_C.read;


#ifdef debug_PID_C
  //............................................................................
  printf1(68, 56,"%-5d"    , PID_C.target);
  printf1(68, 48,"%-5d"    , cmax);
  printf1(68, 40,"%-5d"    , cmin);
  printf1(68, 32,"%-5d"    , PID_C.read);
  printf1(56, 24,"P %5.2f" , PID_C.P);
  printf1(56, 16,"I %5.2f" , PID_C.I);
  printf1(56,  8,"D %5.2f" , PID_C.D);
  //............................................................................
#endif


  if ((abs(PID_C.err)>PID_C.precis))  {goto _Cstart;}



#ifdef debug_PID_C
  //............................................................................
  PointOut(timex,PID_C.read/scrYratio);
  LineOut(oldtx, oldy, timex, pwm0);
  LineOut(timex+2,PID_C.target/scrYratio, timex+10, PID_C.target/scrYratio);
  LineOut(timex+2, pwm0, timex+10, pwm0);
  //............................................................................
#endif

  PID_C.runstate=0;
  Wait(1);                                //runstate = IDLE

}

//==============================================================================


safecall void RotatePID(char port, long Target, float RotatSpeed, char cont) {


  if (port==OUT_A) {

    if ((PID_A.runstate!=0)|| (PID_A.cont)) { // stop PID task if already running
      stop task_PID_A;
      PlayTone(TONE_C7,100);
      Off(OUT_A);
      Wait(50);
    }

    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) {

    if ((PID_B.runstate!=0)|| (PID_B.cont)) { // stop PID task if already running
      stop task_PID_B;
      PlayTone(TONE_C7,100);
      Off(OUT_B);
      Wait(50);
    }

    PID_B.runstate=1;                // set runstate: PID active

    // custom init PID_B
    PID_B.target =Target;                   // assign target
    PID_B.tarpwm =RotatSpeed;               // assign rotation speed
    PID_B.cont=cont;                        // cont vs. hit once

    // Reset PID control defaults
    PID_B.outp    =0;                // PID control output value
    PID_B.maxout  =100;              // absolute max possible output (max pwr)
    PID_B.read    =0;                // current reading
    PID_B.err     =0;                // current error
    PID_B.integr  =0;                // integral of errors
    PID_B.speed   =0;                // current speed

    Wait(1);
    start task_PID_B;
  }

  else
  if (port==OUT_C) {

    if ((PID_C.runstate!=0)|| (PID_C.cont)) { // stop PID task if already running
      stop task_PID_C;
      PlayTone(TONE_C7,100);
      Off(OUT_C);
      Wait(50);
    }

    PID_C.runstate=1;                // set runstate: PID active

    // custom init PID_C
    PID_C.target =Target;                   // assign target
    PID_C.tarpwm =RotatSpeed;               // assign rotation speed
    PID_A.cont=cont;                        // cont vs. hit once

    // Reset PID control defaults
    PID_C.outp    =0;                // PID control output value
    PID_C.maxout  =100;              // absolute max possible output (max pwr)
    PID_C.read    =0;                // current reading
    PID_C.err     =0;                // current error
    PID_C.integr  =0;                // integral of errors
    PID_C.speed   =0;                 // current speed

    Wait(1);
    start task_PID_C;
  }

}

//==============================================================================



inline void RotatePIDtoTarget(char port, long Target, float RotatSpeed) {
   RotatePID(port, Target, RotatSpeed, false);
   Wait(1);
}

inline void RotatePIDcont(char port, long Target, float RotatSpeed) {
   RotatePID(port, Target, RotatSpeed, true);
   Wait(1);
}

inline void RotatePIDdegrees(char port, long Target, float RotatSpeed)  {
   RotatePID(port, Target+MotorRotationCount(port), RotatSpeed, false);
   Wait(1);
}


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;
    }

  }


}


//==============================================================================


void SetPIDparamEx(char port,float P,float I,float D,float prec,int rtime, float damp) {  // custom PID parameters

  if (port==OUT_A) {
    PID_A.P       =P;             // P: propotional to error
    PID_A.I       =I;             // I: avoid perish
    PID_A.D       =D;             // D: derivative: avoid oscillating
    PID_A.precis  =prec;          // error precision to target
    PID_A.regtime =rtime;         // PID regulation time
    PID_A.damp    =damp;          // damp error integral

  } else

  if (port==OUT_B) {
    PID_B.P       =P;             // P: propotional to error
    PID_B.I       =I;             // I: avoid perish
    PID_B.D       =D;             // D: derivative: avoid oscillating
    PID_B.precis  =prec;          // error precision to target
    PID_B.regtime=rtime;          // PID regulation time
    PID_B.damp    =damp;          // damp error integral

  } else

  if (port==OUT_C) {
    PID_C.P       =P;             // P: propotional to error
    PID_C.I       =I;             // I: integral: avoid perish
    PID_C.D       =D;             // D: derivative: avoid oscillating
    PID_C.precis  =prec;          // error precision to target
    PID_C.regtime=rtime;          // PID regulation time
    PID_C.damp    =damp;          // damp error integral

  }

}

//==============================================================================

void SetPIDparam(char port,float P,float I,float D) {  // custom PID parameters

  if (port==OUT_A) {
    PID_A.P       =P;             // P: propotional to error
    PID_A.I       =I;             // I: integral: avoid perish
    PID_A.D       =D;             // D: derivative: avoid oscillating

  } else

  if (port==OUT_B) {
    PID_B.P       =P;             // P: propotional to error
    PID_B.I       =I;             // I: integral: avoid perish
    PID_B.D       =D;             // D: derivative: avoid oscillating

  } else

  if (port==OUT_C) {
    PID_C.P       =P;             // P: propotional to error
    PID_C.I       =I;             // I: integral: avoid perish
    PID_C.D       =D;             // D: derivative: avoid oscillating

  }

}
//==============================================================================


void PIDinit() {

  for (char port=0; port<3; ++port) {
    SetPIDparamEx(port,0.40, 0.40, 10, 1, 5, 0.75); // p,i,d, precis, reg_time, damp
    Wait(1);
  }
}

//==============================================================================
// display, monitor, and debug

void DisplayMask() {
  printf1(44, 56,"tgt","" );     // target value
  printf1(44, 48,"max","" );     // max value
  printf1(44, 40,"min","" );     // min value
  printf1(44, 32,"now","" );     // current value
}

//==============================================================================

task main() {

  long Target;                     // custom values
  int  RotatSpeed;

  PIDinit();



  while(true) {

    Target     = 200;                // set new RELATIVE motor rotation target
    RotatSpeed =  80;                // set max rotation speed


    SetPIDparam(OUT_A, 1.4, 0.8,  12 );                    // p,i,d
    RotatePIDdegrees(OUT_A, Target, RotatSpeed);

    SetPIDparam(OUT_B, 0.6, 0.5, 10);                     // p,i,d
    //RotatePIDdegrees(OUT_B, Target, RotatSpeed);

    SetPIDparam(OUT_C, 1.0, 2.0, 30);                     // p,i,d
    //RotatePIDdegrees(OUT_C, Target, RotatSpeed);

    while ((PID_A.runstate!=0)||(PID_B.runstate!=0)||(PID_C.runstate!=0));

    PlayTone(TONE_C4,100); Wait(100);
    printf1(40, 0,"%s",">press btn");

    while (BTNCENTER!= getchar());      // wait for BtnCtr to continue

  }

}


schlechte p,i,d :
Dateianhang:
PID_bad.png
PID_bad.png [ 21.88 KiB | 4620-mal betrachtet ]


gute p,i,d :
Dateianhang:
PID_good.png
PID_good.png [ 1.2 KiB | 4620-mal betrachtet ]



bitte testen!

8-)

_________________
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


2. Jun 2013 21:47
Profil
Schreibt super viel
Schreibt super viel

Registriert: 17. Dez 2011 11:39
Beiträge: 490
Wohnort: Österreich
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
Hab gerade mal etwas herumprobiert.
Ich bewundere dich, wenn du bei den ganzen Daten auf dem kleinen LCD noch was erkennen kannst :D
Diese Woche bin ich ziemlich im Stress. Sobald ich besser Zeit habe werde ich mal ein paar Graphen mit Labview aufnehmen.

2 Dinge:
Nachdem der PID Regler die Position erreicht hat würde ich den Regler zum halten der Position eingeschaltet lassen.
Der Break-Mode von der Lego-Fw ist ja nicht wirklich streng wenns ums halten der aktuellen Position geht.
Man könnte ja den Regler im "Halte-Modus" etwas mehr P und I Anteil geben damit der Motor aggressiv die Position hält.

Eigentlich könnte man sich ja eine Menge Zeit sparen, wenn man den Motor ordentlich bremst vorm Ziel anstatt ihn allmählich langsamer werden zu lassen.
Nur wie würde der Regler darauf reagieren?
Mit aktiv Bremsen mein ich das zb die aktuelle Drehrichtung invertiert wird...

_________________
Projekte und Informationen auf meiner Website: http://roboticsaumair.jimdo.com/


3. Jun 2013 19:29
Profil
Administrator
Administrator
Benutzeravatar

Registriert: 11. Jan 2006 21:01
Beiträge: 4327
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze
Beitrag Re: NXC: motor PID controller: move to encoder/sensor target
Haltemodus ist jetzt auch implementiert!
wer will, kann also jetzt auch den Haltemodus benutzen. Er funktioniert völlig automatisch und genau so, wie du es dir gedacht hast.

Aktiv bremsen kann er z.B. jetzt auch bereits völlig automatisch (über das i und das d-Glied), das siehst du an den Grafen, wenn du sie erst einmal auf dem kleinen LCD erkannt hast. ;)
Andererseits: Künstliche unabhängige "Invers-Brems-Interventionen" (wie du sie vorgeschlagen hast) sind dann aber auch kein PID-Controller mehr (sondern irgendetwas anderes) - unabhängig davon, dass sie nicht in allen denkbaren Lebenslagen zuverlässig und kalkulierbar funktionieren.


Zitat:
edit: Vorsicht mit dem BCC Tool "NXTScreen/ScreenCapture", das stört u.U. bereits im USB-Modus die Ausführungsgeschwindigkeit!!
evtl. können auch andere Verbindungen vom NXT zum PC (wschl dann auch BT!) für online data logging ähnliche Störungen verursachen!
vgl. http://www.mindstormsforum.de/viewtopic.php?f=25&t=7569


Wenn du anfängst mit dem Tuning und Fine-tuning des Reglers für deine eigene, ganz spezielle Mechanik, wirst du schnell merken, wie dir die Graphen helfen, um Überschießen, Oszillieren und Zappeln oder Zittern zu vermeiden - oder volle Kraft auch gegen wechselnde, starke Widerstände in unmittelbarer Zielnähe zu generieren. Du hast pro Port 6 Parameter zur Verfügung, um alles nach deinen eigenen Bedürfnissen individuell zu justieren:
void SetPIDparamEx(char port, float P, float I, float D, float prec, int rtime, float damp)

Vergleiche dann die Ergebnisse mit den NXC-API-Funktionen (RotateMotorEx und RotateMotorPID).

Versuche vor allem, selber herauszukriegen, wie die pid-Parameter und -Glieder sich zwischendurch und in Ziel-Nähe verhalten, bremse dazu einen Motor mit der Hand und beobachte den Bildschirm, du wirst erstaunt sein, was da alles passiert, vor allem, wenn du die pid-Werte auch mal grob verstellst oder völlig wirr mit der Hand gegen die Motor-Drehung dagegendrehst.
Dann lass den motor mit 20%, 40%, 60%, 80%, 100% pwm power laufen und vergleiche das Ergebnis mit RotateMotorPID.
Probiere ab 2000° abwärts mal alle möglichen Winkel aus und beobachte die Regelung über längere Strecken, auch bei zwischendurch auftretendem "stalling" (z.B.: 2000°, 1000°, 360°, 180°, 90°, 40°, 20°, 10°). Dann wird dir sicher einiges klar werden.

Aber ums ausgiebige selber-Herumprobieren und -Herumspielen mit den pids wirst du nicht herumkommen - diese Arbeit musst du dir schon selber machen.
Theoretisieren bringt da nichts. 8-)


3. Jun 2013 20:51
Profil
Schreibt super viel
Schreibt super viel

Registriert: 17. Dez 2011 11:39
Beiträge: 490
Wohnort: Österreich
Beitrag Re: NXC: motor PID controller: move to encoder/sensor targe
So hab jetzt ziemlich viele Versuche gemacht.
Es ist wirklich extrem aufwendig den Regler für jeden verschiedenen Weg (Target) den Regler aufs perfekte einzustellen.
Hier zum Beispiel die perfekten Werte für 90° Drehung bei Leerlauf und Speed 100:
Dateianhang:
capture1.jpg
capture1.jpg [ 83.36 KiB | 5989-mal betrachtet ]


Wenn man Werte haben will die für verschiedene Drehungen passen sollen, muss man eben Kompromisse eingehen (Überschießen, langsam..)
Ein Autotuning-Algorithmus wäre super. Bei Labview gibts sowas auch: http://zone.ni.com/reference/en-XX/help/370401J-01/lvpid/pid_with_autotuning/
Vielleicht könnte man irgendwie diese Library von Arduino in NXC schreiben: http://brettbeauregard.com/blog/2012/01/arduino-pid-autotune-library/

_________________
Projekte und Informationen auf meiner Website: http://roboticsaumair.jimdo.com/


10. Jul 2013 12:54
Profil
Administrator
Administrator
Benutzeravatar

Registriert: 11. Jan 2006 21:01
Beiträge: 4327
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze
Beitrag Re: NXC: motor PID controller: move to encoder/sensor targe
Danke für die schöne Labview-Grafik! http://mindstormsforum.de/uploads/index.php?downloade_datei=8fea01112a71c6b9f67874fe7aed22be
auch Autotuning wird aber nichts anderes tun als manuelles Tuning, mit +/- demselben Ergebnis: für wenige Fälle sehr gut passend, aber für die übrigen Fälle eben nicht.
Wenn man kein Overshooting zulasssen will, kann ein pid Regler immer nur bestimmte Bereiche abdecken, sowohl was die Laufzeiten, die Winkelbereiche, die Belastungsbereiche und auch was die Geschwindigkeitsbereiche betrifft, da p,i und d während der Regulation Konstanten sind - es sei denn, man ändert sie dynamisch während des laufenden Programms, was im obigen Code ebenfalls möglich ist.

Beispiel: wichtig z.B. für Plotter ist, dass diese exakte Zielwerte brauchen, eine hohe Bewegungsdynamik besitzen müssen, aber absolut ohne Overshooting, sie haben aber dafür auch keine wechselnden Belastungsmomente, und man kann daher in der Nähe des Ziels mit relativ schwacher Leistung arbeiten: P=mittel, I=niedrig, D=mittel. Bei zunehmender konstanter Last müssen dann I und D gleichsinnig erhöht werden (z.B. Fräse).

Oder folgender Fall: Overshooting wäre durchaus zulässig, weil nur die Endposition wichtig ist, dafür muss man auch mit extremen unvorhersehbaren Belastungsschwankungen rechnen (in alle Richtungen, gegen und in Drehrichtung), und die Bewegung darf z.B. auch niemals unter extremer Belastung zwischendrin verrecken, selbst bei langsamen Geschwindigkeiten, und erst Recht auch nicht in Zielnähe, wie es P-Regler gerne tun.

Beispiel: wie z.B. bei Montage-Roboterarmen oder auch bei Off-Road-Fahrzeugantrieben, die extreme wechselnde Lasten exakt bewegen müssen oder passive Kräfte (Gefälle) gegenregulieren müssen; diese brauchen ganz andere p,i,d-Werte, nämlich für hohe Leistung auch bei sehr geringen Geschwindigkeiten auch noch im ganz nahen Zielbereich: P niedrig, I=hoch, D=sehr hoch. Durch den I Parameter wird dadurch bei zunehmenden Stalling die Motorkraft maximal "reaktiv" erhöht, D glättet das Schwingungsverhalten.

In diesem Fall wird auch Autotuning immer nur für eng definierte Belastungen die Werte liefern, bei breiten und unvorhersehbaren Belastungsbereichen also auch u.U. absolut falsche.

Overshooting (was für deine Anwendung besonders vermieden werden muss, wie ich weiß) ist also an sich nichts Negatives, und auch geringe Leistungsreserven im Zielbereich sind nicht negativ, wenn man sie nicht braucht - negativ ist es nur, wenn der pid-Regler im Einzelfall nicht darauf justiert werden kann, wie z.B. der pid-Regler der Lego-Firmware. Ich behaupte aber, mit meinem pid-Regler hat man diese sämtlichen Möglichkeiten.

_________________
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


10. Jul 2013 14:36
Profil
Beiträge der letzten Zeit anzeigen:  Sortiere nach  
Auf das Thema antworten   [ 13 Beiträge ] 

Wer ist online?

Mitglieder in diesem Forum: 0 Mitglieder und 15 Gäste


Du darfst keine neuen Themen in diesem Forum erstellen.
Du darfst keine Antworten zu Themen in diesem Forum erstellen.
Du darfst deine Beiträge in diesem Forum nicht ändern.
Du darfst deine Beiträge in diesem Forum nicht löschen.
Du darfst keine Dateianhänge in diesem Forum erstellen.

Suche nach:
Gehe zu:  
Impressum Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.
Deutsche Übersetzung durch phpBB.de